Arduino.ru

Сразу возникает вопрос: а зачем нужно было считать углы?

И почему именно углы?

Робот ведь у Вас на сервах? А у сервы единственная функция - поворачиваться на заданный угол.

Вопрос на засыпку: и что же надо делать с рассчитанными углами?

засовывать их в сервы

Это понятно, но их же надо подавать в определенной последовательности. И двигаться все ноги должны одновременно. В файле с формулами есть Gait table, вот с ней и не могу разобраться. Что это и как её использовать.

Когда углы известны я делал примерно так:

for (uint8_t i = 0; i < speedValue; i++) {
    LF_coxa.write(map(i, 0, speedValue, LF_COXA_MID_ANG, LF_COXA_HALF_RIGHT_ANG));
    LF_femur.write(map(i, 0, speedValue, LF_FEMUR_DOWN_ANG, LF_FEMUR_UP_ANG));

    RM_coxa.write(map(i, 0, speedValue, RM_COXA_MID_ANG, RM_COXA_HALF_RIGHT_ANG));
    RM_femur.write(map(i, 0, speedValue, RM_FEMUR_DOWN_ANG, RM_FEMUR_UP_ANG));

    LB_coxa.write(map(i, 0, speedValue, LB_COXA_MID_ANG, LB_COXA_HALF_RIGHT_ANG));
    LB_femur.write(map(i, 0, speedValue, LB_FEMUR_DOWN_ANG, LB_FEMUR_UP_ANG));

    RF_coxa.write(map(i, 0, speedValue, RF_COXA_MID_ANG, RF_COXA_HALF_LEFT_ANG));
    LM_coxa.write(map(i, 0, speedValue, LM_COXA_MID_ANG, LM_COXA_HALF_LEFT_ANG));
    RB_coxa.write(map(i, 0, speedValue, RB_COXA_MID_ANG, RB_COXA_HALF_LEFT_ANG));

    delay(stepDelayValue);
  }

  delay(cycleDelayValue);

а, нахрена это?   delay(stepDelayValue);

есть у тебя угол, так понимаю, есть у тебя скорость движения сервы speedValue , если я правильно понимаю - воткнул угол и скорость во все сервы и жди, когда оно на ноги встанет

Если тупо подать значения сразу на ВСЕ сервы то никакого движения вперед не будет. Для этого я так понял и сделана в статье таблица Gait table, а у меня используется куча разных конструкций for-map.

А delay(stepDelayValue); это небольшая (5-20 мс) временная задержка, тоже для регулировки скорости движения.

nanotroll, Вы пишете, что у Вас уже есть скетч, двигающий робота. Насколько я понял, в 3-м посте фрагмент именно из него.

Калапуция Вам уже намекнул, скажу еще я открытым текстом:

Начните с того, что перепишите то, что у Вас есть, без использования delay(). Принцип изложен здесь: https://www.arduino.cc/en/Tutorial/BlinkWithoutDelay Пока Вы с этим хорошенько не разберетесь, делать что-либо другое бессмысленно.

Пока переписал только функции "встать" и "сесть" для одного сервопривода, используя millis() . Если возможно, посмотрите пожалуйста код, вроде едет вверх и вниз, но может быть все реализуется проще, чем я понял.

#include <Servo.h>

Servo RF_femur;

const int INTERVAL = 20;
const uint8_t IN_PROGRESS = 0;
const uint8_t COMPLETED = 1;

uint8_t taskState;

uint8_t RF_FEMUR_DOWN = 85;
uint8_t RF_FEMUR_UP = 45;

uint8_t RF_femurPin = 13;

unsigned long previousMillis = 0;

void moveServo(Servo servo, uint8_t curPos, uint8_t newPos) {
  if (curPos < newPos) {
    servo.write(curPos + 1);
  }
  if (curPos > newPos) {
    servo.write(curPos - 1);
  }
  if (curPos == newPos) {
    taskState = COMPLETED;
  }
}

void attachServos() {
  RF_femur.attach(RF_femurPin);
}

void initServos() {
  RF_femur.write(RF_FEMUR_UP);
}

void wake_up() {
  uint8_t curPos = 0;
  unsigned long currentMillis = millis();
  
  curPos = RF_femur.read();
  
  if (currentMillis - previousMillis >= INTERVAL) {
    previousMillis = currentMillis;
    moveServo(RF_femur, curPos, RF_FEMUR_DOWN);
  }
}

void sleep() {
  uint8_t curPos = 0;
  unsigned long currentMillis = millis();

  curPos = RF_femur.read();
  
  if (currentMillis - previousMillis >= INTERVAL) {
    previousMillis = currentMillis;
    moveServo(RF_femur, curPos, RF_FEMUR_UP);
  }
}

void getCommand() {
  if (Serial.available() > 0) {
    uint8_t inputVal = Serial.read();

    if (inputVal == '0') {
      taskState = IN_PROGRESS;
      while(taskState != COMPLETED) {
        sleep();
      }
    }

    if (inputVal == '1') {
      taskState = IN_PROGRESS;
      while(taskState != COMPLETED) {
        wake_up();
      }
    }
    
  }
}

void setup() {
  Serial.begin(9600);
  attachServos();
  initServos();
}

void loop() {
  getCommand(); 
}

Реализовано неверно.

У Вас из loop управление передается getCommand, а там при наличии команд зависает пока taskState != COMPLETED. Так Вы опять будете двигать сервы по очереди вместо того, чтобы двигать их одновременно. Нельзя при управлении движения использовать цикл while. На каждом проходе loop нужно проанализировать ситуацию, при необходимости осуществить однократное воздействие (возможно, по одному на каждую серву) и тут же вернуть управление. Т.е. управлять нужно всеми серваит одновременно, а как сделали Вы - получается только по очереди.

И двигать сервы шажками по единице - плохая практика. Фактически Вы только существенно снижаете скорость перемещения серв (до менее радиана в секунду), не имея никакого положительного эффекта. Пусть сервы двигаются с максимально допустимой для них скоростью, если это возможно. А если нет - нужно обеспечить синхронное перемещение серв, скорее всего, их придется перемещать с разными шагами, а не с постоянным шагом равным 1.

Добавил переменные 

const uint8_t TASK_COMPLETED = 0;
const uint8_t TASK_WAKE_UP = 1;
const uint8_t TASK_SLEEP = 2;

Переделал функцию получения команды и добавил выполнение задачи отдельно, теперь вроде по одному шагу добавляет за заход, циклов нету.

void getCommand() {
  if (Serial.available() > 0) {
    uint8_t inputVal = Serial.read();

    if (inputVal == '0') {
      taskState = TASK_SLEEP;
      sleep();
    }

    if (inputVal == '1') {
      taskState = TASK_WAKE_UP;
      wake_up();
    }
  }
}

void doTask() {
  switch(taskState) {
    case(0):
      break;
    case(1): 
      wake_up();
      break;
    case(2):
      sleep();
      break;
  }
}

void loop() {
  getCommand();
  doTask();
}

Ну, раз добавили констаны, сразу бы и внесли их в doTask().

А серв - лучше бы сразу завести массив, хотя бы из двух элементов, чтобы уже на этом этапе планировать, как они должны будут управляться.

Спасибо Вам за ответы, еще хотелось бы уточнить несколько моментов. 

У меня есть в программе много констант, которые содержат номера пинов, к которым подключены сервы, временные интервалы, разные позиции для каждой сервы. Как их правильно описать? Через const или через define? Я бы хотел их вообще вынести в отдельный файл. 

Функция "шаг вперед" состоит из 4 кусков, вроде шагает, но кажется как будто 1 и 2 куски выполняются медленнее, чем 2 и 3. 

void trotGaitStepForward() {
  unsigned long currentMillis = millis();
  moveState = MOVE_START;
  
  if (currentMillis - previousMillis >= MOVE_INTERVAL) {
    previousMillis = currentMillis;

    if (trotGaitStepForwardState == PHASE_1) {
      moveServo(RF_femur, getCurPos(RF_femur), RF_FEMUR_UP);
      moveServo(RF_coxa, getCurPos(RF_coxa), RF_COXA_HALF_CW);

      moveServo(LM_femur, getCurPos(LM_femur), LM_FEMUR_UP);
      moveServo(LM_coxa, getCurPos(LM_coxa), LM_COXA_HALF_CW);

      moveServo(RB_femur, getCurPos(RB_femur), RB_FEMUR_UP);
      moveServo(RB_coxa, getCurPos(RB_coxa), RB_COXA_HALF_CW);

      moveServo(LF_coxa, getCurPos(LF_coxa), LF_COXA_HALF_ACW);
      moveServo(RM_coxa, getCurPos(RM_coxa), RM_COXA_HALF_ACW);
      moveServo(LB_coxa, getCurPos(LB_coxa), LB_COXA_HALF_ACW);
      
      if (moveState == MOVE_COMPLETED) {
        trotGaitStepForwardState = PHASE_2;
        moveState = MOVE_START;
      }
    }
    
    if (trotGaitStepForwardState == PHASE_2) {
      moveServo(RF_femur, getCurPos(RF_femur), RF_FEMUR_DOWN);
      moveServo(RF_coxa, getCurPos(RF_coxa), RF_COXA_FULL_CW);

      moveServo(LM_femur, getCurPos(LM_femur), LM_FEMUR_DOWN);
      moveServo(LM_coxa, getCurPos(LM_coxa), LM_COXA_FULL_CW);

      moveServo(RB_femur, getCurPos(RB_femur), RB_FEMUR_DOWN);
      moveServo(RB_coxa, getCurPos(RB_coxa), RB_COXA_FULL_CW);

      moveServo(LF_coxa, getCurPos(LF_coxa), LF_COXA_FULL_ACW);
      moveServo(RM_coxa, getCurPos(RM_coxa), RM_COXA_FULL_ACW);
      moveServo(LB_coxa, getCurPos(LB_coxa), LB_COXA_FULL_ACW);

      if (moveState == MOVE_COMPLETED) {
        trotGaitStepForwardState = PHASE_3;
        moveState = MOVE_START;
      }
    }

    if (trotGaitStepForwardState == PHASE_3) {
      moveServo(LF_femur, getCurPos(LF_femur), LF_FEMUR_UP);
      moveServo(LF_coxa, getCurPos(LF_coxa), LF_COXA_HALF_ACW);

      moveServo(RM_femur, getCurPos(RM_femur), RM_FEMUR_UP);
      moveServo(RM_coxa, getCurPos(RM_coxa), RM_COXA_HALF_ACW);

      moveServo(LB_femur, getCurPos(LB_femur), LB_FEMUR_UP);
      moveServo(LB_coxa, getCurPos(LB_coxa), LB_COXA_HALF_ACW);

      moveServo(RF_coxa, getCurPos(RF_coxa), RF_COXA_HALF_CW);
      moveServo(LM_coxa, getCurPos(LM_coxa), LM_COXA_HALF_CW);
      moveServo(RB_coxa, getCurPos(RB_coxa), RB_COXA_HALF_CW);

      if (moveState == MOVE_COMPLETED) {
        trotGaitStepForwardState = PHASE_4;
        moveState = MOVE_START;
      }
    }

    if (trotGaitStepForwardState == PHASE_4) {
      moveServo(LF_femur, getCurPos(LF_femur), LF_FEMUR_DOWN);
      moveServo(LF_coxa, getCurPos(LF_coxa), LF_COXA_MID);

      moveServo(RM_femur, getCurPos(RM_femur), RM_FEMUR_DOWN);
      moveServo(RM_coxa, getCurPos(RM_coxa), RM_COXA_MID);

      moveServo(LB_femur, getCurPos(LB_femur), LB_FEMUR_DOWN);
      moveServo(LB_coxa, getCurPos(LB_coxa), LB_COXA_MID);

      moveServo(RF_coxa, getCurPos(RF_coxa), RF_COXA_MID);
      moveServo(LM_coxa, getCurPos(LM_coxa), LM_COXA_MID);
      moveServo(RB_coxa, getCurPos(RB_coxa), RB_COXA_MID);

      if (moveState == MOVE_COMPLETED) {
        taskState = TASK_COMPLETED;
        moveState = MOVE_START;
        trotGaitStepForwardState = PHASE_1;
      }
    }
    
    tmpPos += 1;
  }
}

moveServo() отдельно:

void moveServo(Servo servo, uint8_t curPos, uint8_t newPos) {
  if (curPos < newPos) {
    servo.write(map(tmpPos, 0, SPEED_VALUE, curPos + 1, newPos));
  }
  if (curPos > newPos) {
    servo.write(map(tmpPos, 0, SPEED_VALUE, curPos - 1, newPos));
  }
  if (curPos == newPos) {
    moveState = MOVE_COMPLETED;
  }
}

Получается в итоге мне нужно будет углы, которые определены константами заменить на посчитанные из координат?