MobBob:самостоятельный робот Arduino, управляемый смартфоном Android

Необходимые инструменты и машины

	Паяльник (общий)
	3D-принтер (общий)

Приложения и онлайн-сервисы

IDE Arduino

Об этом проекте

Я сделал этого робота по инструкции на сайте. Отличная идея для симбиоза между Arduino и Android.

MobBob - это робот, управляемый смартфоном. Используя возможности вашего смартфона, MobBob становится ходячим говорящим роботом с распознаванием голоса и компьютерным зрением.

Детали, напечатанные на 3D-принтере:https://www.thingiverse.com/thing:715688

Файл Android.apk:https://apkpure.com/mobbob/com.cevinius.MobBob

В коде вы захотите:

- Обновите контактные переменные, чтобы они соответствовали вашей сборке

- Настройте сервоцентр, минимальные и максимальные значения

- Установите "FRONT_JOINT_HIPS" на 1 или -1 в зависимости от того, как установлены ваши бедренные сервоприводы. Я устанавливаю его с сервоакселем в передней части MobBob. Для этой конфигурации установите это значение на 1.

Код

• код

код Arduino

 / * * ===============================================================* Программа управления MobBob - программная последовательная версия Bluetooth * Кевин Чан (он же Cevinius) * =============================================================* * Эта программа позволяет управлять MobBob с помощью последовательных команд. В этой версии кода команды * принимаются через программный последовательный порт, а контакты определены в #define вверху. * Это означает, что вы можете использовать любую совместимую с Arduino плату и подключить карту Bluetooth к контактам, установленным для * программного последовательного порта. (В отличие от другой версии, разработанной для платы Bluno от DFRobot.) * * Эта программа длинная и содержит 2 основных компонента - программу плавной сервоанимации и программу синтаксического анализатора последовательных * команд. * * Система анимации * ================* Программа анимации предназначена для плавной анимации массивов серво ключевых кадров. Код пытается * сделать все возможное, чтобы его было легко использовать. * * Система анимации ставит в очередь только 1 команду. т.е. одна команда может быть запущена * и одна команда может быть поставлена ​​в очередь. Если вы отправите больше команд, они перезапишут команду в очереди. * * Система анимации по умолчанию ожидает завершения текущей анимации перед запуском следующей. Это * означает, что если данные анимации заканчиваются тем, что робот находится в его базовой позе, все будет плавно соединяться. Чтобы * поддержать это, система анимации также имеет функцию, при которой анимация может иметь «завершающую последовательность» *, чтобы вернуть робота в базовую позу. Эта функция используется для анимации ходьбы вперед / назад. * Эти анимации имеют финальную последовательность, которая возвращает робота в базовую позу. * * По окончании воспроизведения анимации система анимации выведет строку ответа на последовательный порт. * Это позволяет вызывающим абонентам узнать, когда анимация, которую они запросили, закончила воспроизведение. Это полезно * для пользователей, чтобы упорядочить анимацию - дождаться завершения одной перед запуском другой. * * В коде анимации есть много переменных, позволяющих изменять настройки. Например. Частота обновления, контакты Arduino и т. Д. * * Формат массива данных анимации также разработан таким образом, чтобы его можно было легко редактировать вручную. * * Командный синтаксический анализатор * ==============* Эта система анализирует команды, полученные через последовательный порт, и обрабатывает их. Команды включают в себя команду для прямой * установки положений сервоприводов, а также команды для запуска заранее определенных анимаций и прогулок. * * Таким образом, пользователи, которые не хотят беспокоиться о деталях ходьбы, могут просто использовать заранее определенные прогулки / анимации. * И пользователи, которым нужен полный контроль над сервоприводами (для создания новой анимации на лету), тоже могут это сделать. * * Как упоминалось выше, эти команды можно использовать в интерактивном режиме из последовательного монитора Arduino. Их также можно * отправить с помощью Bluetooth LE (если используется Bluno). Приложение для телефона отправит команды через Bluetooth LE на * Bluno. * * Общие команды:* ----------------- * Готов / ОК Проверка: * Проверка состояния. Ответ возвращается немедленно, чтобы проверить, работает ли контроллер. * * Set Servo: * time - время перехода к заданным углам, 0 будет немедленно переходить к углам * leftHip - микросекунды от центра. -ve бедро внутрь, + ve бедро наружу * leftFoot - микросекунды от плоскости. -ve - стопа вниз, + ve - ступня вверх * rightHip - микросекунды от центра. -ve бедро внутрь, + ve бедро наружу * rightFoot - микросекунды от плоскости. -ve - стопа вниз, + ve - стопа вверх * Эта команда используется для получения полного контроля над сервоприводами. Вы можете перемещать робота из его * текущей позы в заданную в течение указанного времени. * * Стоп / Сброс: * Останавливает робота после текущей анимации. Может использоваться для остановки анимации, установленной в цикл * на неопределенный срок. Это также можно использовать для установки робота в его базовую позу (стоя прямо) * * Немедленная остановка: * Немедленно останавливает робота, не дожидаясь завершения текущей анимации. Это * прерывает текущую анимацию робота. Потенциально робот может находиться в середине анимации * и в нестабильной позе, поэтому будьте осторожны при использовании этого. * * Стандартные команды ходьбы:* ----------------------- * Вперед:, -1 означает непрерывно, 0 или нет параметров. так же, как 1 раз. * Назад:, -1 означает непрерывно, 0 или отсутствие параметров равно 1 разу. * Поверните налево:, -1 означает непрерывно, 0 или отсутствие параметров совпадает с 1 разом. * Поверните направо:, -1 означает непрерывно, 0 или отсутствие параметров совпадает с 1 разом. * * Команды забавной анимации:* ----------------------- * Встряхнуть головой:, -1 означает непрерывно, 0 или без параметров то же самое, что и 1 раз. * * Bounce:, -1 означает непрерывный, 0 или no param совпадает с 1 разом. * * Колебание:, -1 означает непрерывный, 0 или отсутствие параметра совпадает с 1 разом. * Колебание влево:, -1 означает непрерывно, 0 или отсутствие параметров совпадает с 1 разом. * Колебание вправо:, -1 означает непрерывно, 0 или отсутствие параметров равно 1 разу. * * Tap Feet:, -1 означает непрерывно, 0 или отсутствие параметра совпадает с 1 разом. * Нажмите на левую ногу:, -1 означает непрерывно, 0 или отсутствие параметров совпадает с 1 разом. * Нажмите правой ногой:, -1 означает непрерывно, 0 или отсутствие параметров равно 1 разу. * * Встряхните ноги:, -1 означает непрерывно, 0 или no param совпадает с 1 разом. * Встряхните левую ногу:, -1 означает непрерывно, 0 или отсутствие параметров равно 1 разу. * Встряхните правую ногу:, -1 означает непрерывно, 0 или отсутствие параметров равно 1 разу. * * Кроме того, код отправит строку ответа обратно через Serial, когда * выполнение команд завершится. * * Если команда завершилась нормально, строка ответа - это код команды без * параметров. Например. Когда он закончит движение вперед, он отправит ответ «». * * Если команда была прервана с помощью , текущая анимация могла быть остановлена ​​на полпути. * В этом случае робот может быть в странной позе на полпути, а finishAnims, возможно, не был * воспроизведен. Чтобы пользователь знал, что это произошло, в строке ответа будет указан параметр * -1. Например, если прогулка была остановлена ​​на полпути с помощью , строка ответа будет * , чтобы указать, что прогулка остановлена, но была остановлена ​​на полпути. * (Примечание:если вы используете  для остановки, это будет ждать завершения текущего цикла анимации * перед остановкой. Таким образом, в этом случае анимация не будет остановлена ​​на полпути.) * * Поскольку ответы отправляются после После завершения анимации отправитель команды может * искать строки ответа, чтобы определить, когда робот готов к новой команде. * Например, Если вы используете команду , строка ответа не будет отправлена, пока не будут выполнены все 3 шага * (и конец анимации). Таким образом, отправитель команды может дождаться строки ответа *, прежде чем сказать роботу, что нужно сделать следующее. * / #include  #include  // -------------------------------- -------------------------------------------------- // Скорость последовательной связи - Установите это для вашей последовательной (bluetooth) карты .//------------------------------- -------------------------------------------------- - // Скорость последовательной связи с платой bluetooth .// Некоторые платы по умолчанию 9600. У меня есть плата по умолчанию 115200. # define SERIAL_SPEED 115200 // Настроить программный последовательный порт на этих выводах. Const int rxPin =11; // вывод, используемый для приема данных int txPin =12; // вывод, используемый для отправки данных SoftwareSerial softwareSerial (rxPin, txPin); // ---------------------------------- ------------------------------------------------ // Настройка контактов Arduino - установите их для вашего конкретного робота. --------------------------------------------- const int SERVO_LEFT_HIP =5; const int SERVO_LEFT_FOOT =2; const int SERVO_RIGHT_HIP =3; const int SERVO_RIGHT_FOOT =4; // Я хочу, чтобы этот код можно было использовать на всех двуногих с 4 сервоприводами! (Как Боб, MobBob) // Я заметил, что некоторые сборки устанавливают сервоприводы бедра, // обращенные другим способом, чем я использовал MobBob, поэтому этот параметр позволяет вам настроить код // для любого стиля сборки. // 1 для стиля MobBob. бедра, обращенные вперед (суставы вперед) // -1 для бобов, обращенные назад к бедрам (суставы вперед) #define FRONT_JOINT_HIPS 1 // ------------------- -------------------------------------------------- ------------- // Макс. / Мин. / Константы сервопривода - Установите их для вашего конкретного робота. //------------------ -------------------------------------------------- -------------- const int LEFT_HIP_CENTRE =1580; const int LEFT_HIP_MIN =LEFT_HIP_CENTRE - 500; const int LEFT_HIP_MAX =LEFT_HIP_CENTRE + 500; const int LEFT_FOOT_CENTRE =1410; const int LEFT_FOOT_CENTRE_ 1410; const int LEFT_FOOT_MIN_MIN =LEFT_FOOT_MIN - const int LEFT_FOOT_MAX =LEFT_FOOT_CENTRE + 500; const int RIGHT_HIP_CENTRE =1500; const int RIGHT_HIP_MIN =RIGHT_HIP_CENTRE - 500; const int RIGHT_HIP_MAX =RIGHT_HIP_CENTRE + 500; const int RIGHT_FOOT_CENTRE + 500; const int RIGHT_FOOT_CENTRE 465; const int RIGHT_FOOT_MIN =RIGHT_FOOT_CENTRE - 500; const int RIGHT_FOOT_MAX =RIGHT_FOOT_CENTRE + 500; // ------------------------------ ------------------------------------------------ // Вспомогательные функции, которые помогают вычислять совместные значения более удобным для пользователя способом. // Здесь можно настроить знаки, если сервоприводы настроены другим способом. // Обновление здесь означает, что данные анимации не нужно изменять, если // сервоприводы настраиваются по-другому. // (Например Исходные сервоприводы для бедер Боба расположены наоборот. // ------------------------------------------------ ------------------------------ int LeftHipCentre () {return LEFT_HIP_CENTRE; } int LeftHipIn (целое число миллисекунд) {return LEFT_HIP_CENTRE + (FRONT_JOINT_HIPS * миллисекунды); } int LeftHipOut (целое число миллисекунд) {return LEFT_HIP_CENTRE - (FRONT_JOINT_HIPS * миллисекунды); } int RightHipCentre () {return RIGHT_HIP_CENTRE; } int RightHipIn (целое число миллисекунд) {return RIGHT_HIP_CENTRE - (FRONT_JOINT_HIPS * миллисекунды); } int RightHipOut (целое число миллисекунд) {return RIGHT_HIP_CENTRE + (FRONT_JOINT_HIPS * миллисекунды); } int LeftFootFlat () {return LEFT_FOOT_CENTRE; } int LeftFootUp (int миллисекунды) {return LEFT_FOOT_CENTRE - миллисекунды; } int LeftFootDown (целое число миллисекунд) {return LEFT_FOOT_CENTRE + миллисекунды; } int RightFootFlat () {return RIGHT_FOOT_CENTRE; } int RightFootUp (целое число миллисекунд) {return RIGHT_FOOT_CENTRE + миллисекунды; } int RightFootDown (int миллисекунды) {return RIGHT_FOOT_CENTRE - миллисекунды; } // ----------------------------------------------- ----------------------------------- // Данные анимации по ключевым кадрам для стандартной ходьбы и других анимаций серво // // Формат:{, ​​, , , } // миллисекунды - время перехода к положениям этого ключевого кадра. Например. 500 означает, что потребуется 500 мс, чтобы перейти от // позиции робота в начале этого кадра к положению, указанному в этом кадре // leftHipMicros - положение левого бедра в микросекундах сервопривода. // leftFootMicros - положение левого бедра в сервоприводе. микросекунды .// rightHipMicros - положение левого бедра в микросекундах сервопривода. // rightFootMicros - положение левого бедра в микросекундах сервопривода. // // Микро значения сервопривода поддерживают специальное значение -1. Если это значение дано, оно // сообщает коду анимации игнорировать сервопривод в этом ключевом кадре. т.е. сервопривод будет // оставаться в том положении, в котором он был в начале этого ключевого кадра. //// Кроме того, первый элемент в данных анимации является особенным. Это элемент метаданных .// Первый элемент - {, 0, 0, 0, 0}, который сообщает нам количество кадров // в анимации. Итак, первый фактический ключевой кадр находится в animData [1], а последний ключевой кадр // находится в animData []. (Где  - значение в animData [0] [0].) // ----------------------------- -------------------------------------------------- --- // Константы, чтобы сделать доступ к массивам ключевых кадров более удобочитаемым. Const int TWEEN_TIME_VALUE =0; const int LEFT_HIP_VALUE =1; const int LEFT_FOOT_VALUE =2; const int RIGHT_HIP_VALUE =3; const int RIGHT_FOOT_VALUE Используемые константы =4; // в данных анимации ходьбы при ходьбе. const int FOOT_DELTA =150; const int HIP_DELTA =FRONT_JOINT_HIPS * 120; // Переход в исходное прямое положение стоя. Используется stopAnim (). Int standStraightAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {1, 0, 0, 0, 0}, // Ноги ровные, Ноги ровные {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Перед этим заставим робота Ноги ровные, стопы ровные (т.е. standStraightAnim) .int walkForwardAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {8, 0, 0, 0, 0}, // наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // наклон влево, правая ступня вперед {300, LeftHipIn (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Ноги на плоской подошве, Правая нога вперед {300, LeftHipIn (HIP_DELTA), LeftFoot_DELTA (), RightFoot_DELTA (), RightFoot_DELTA) RightFootFlat ()}, // Наклон вправо, правая нога вперед {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootUp (FOOT_DELTA)}, // Наклон вправо, Feet даже {300, LeftHipentre) (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, левая нога вперед {300, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn_p), RightHipIn_p) , // Ноги на плоской подошве, Левая нога вперед {300, LeftHipOut (HIP_DELTA), LeftFootFlat (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Наклон влево, Левая ступня вперед {300, LeftHipOut (HIP_DEL TA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}}; // Перед этим переведите робота в положение «Feet Flat, Feet Even» (т.е. standStraightAnim) .int walkBackwardAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {8, 0, 0, 0, 0}, // наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // наклон влево, левая стопа вперед {300, LeftHipOut (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Ноги на плоской подошве, левая нога вперед {300, LeftHipOut (HIPTA_DELTA), LeftFootFlat (), RightFootFlat (), RightFootFlat (), RightFootFlat () RightFootFlat ()}, // Наклон вправо, левая нога вперед {300, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootUp (FOOT_DELTA)}, // Наклон вправо, Feet даже {300, LeftHipOut (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, правая нога вперед {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipOut_d (HIP_DELTA), RightHipOut_d (HIP_DELTA) , // Ноги на плоской подошве, Правая ступня вперед {300, LeftHipIn (HIP_DELTA), LeftFootFlat (), RightHipOut (HIP_DELTA), RightFootFlat ()}, // Наклон влево, Правая ступня вперед {300, LeftHipIn (HIP_DELTA ), LeftFootUp (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootDown (FOOT_DELTA)}}; // Анимация завершения прогулки переводит робота из конца walkForwardAnim / walkBackwardAnim обратно в standStraightAnim.int walkEndata] =[] [] . Первый элемент - это количество кадров. {2, 0, 0, 0, 0}, // Наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // стопы ровные, стопы ровные { 300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Перед этим переведите робота в режим Feet Flat, Feet Even (т.е. standStraightAnim) .int turnLeftAnim [] [5] ={/ / Метаданные. Первый элемент - это количество кадров. {6, 0, 0, 0, 0}, // наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // наклон влево, поворот налево бедро, повернуть правое бедро {300, LeftHipIn (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Ноги ровно, повернуть левое бедро, повернуть правое бедро {300, LeftHipIn), Left_DootUpIn (HIP_FELTA) (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Наклон вправо, Повернуть бедро влево, Повернуть бедро вправо {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootUp (FOOT)} // Наклон вправо, ровные ноги {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // стопы ровные, ровные ноги {300, LeftHipCentre (), LeftFootFlatent (), RightHipCentre (), LeftFootFlatent (), RightHipCentre () ), RightFootFlat ()}}; // Перед этим переведите робота в режим Feet Flat, Feet Even (т.е. standStraightAnim) .int turnRightAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {6, 0, 0, 0, 0}, // наклон вправо, ровные ноги {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // наклон вправо, поворот налево бедро, повернуть правое бедро {300, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootUp (FOOT_DELTA)}, // Ноги ровно, повернуть левое бедро, повернуть правое бедро {300, LeftHipIn (HIP_FELTA), Left_DootIn (HIP_FELTA) (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Наклон влево, поворот бедра влево, поворот бедра вправо {300, LeftHipIn (HIP_DELTA), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFoot_Down (FOOT)} // Наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // стопы ровные, ровные ноги {300, LeftHipCentre (), LeftFootFlatent (), RightHipCentre (), LeftFootFlatent (), RightHipCentre () ), RightFootFlat ()}}; // Качать головой аним. Быстро влево-вправо для имитации тряски головы. Int shakeHeadAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {4, 0, 0, 0, 0}, // Ноги ровные, Поворот влево {150, LeftHipOut (HIP_DELTA), LeftFootFlat (), RightHipIn (HIP_DELTA), RightFootFlat ()}, // Ноги ровные, Ноги ровные {150 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}, // Ноги ровно, поворот вправо {150, LeftHipIn (HIP_DELTA), LeftFootFlat (), RightHipOut (HIP_DELTA), RightFootFlat ()}, // FeetFlat ()}, // flat, Feet even {150, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Колебание анимации. Наклоняйте влево и вправо, чтобы весело провести время. Wobble.int wobbleAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {4, 0, 0, 0, 0}, // Наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // стопы ровные, даже стопы {300 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}, // наклон вправо, ровные ноги {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT), // Feet flat, Feet even {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Колебание влево аним. Наклоните влево и назад. Int wobbleLeftAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {2, 0, 0, 0, 0}, // Наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // стопы ровные, ровные {300 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Колебание анимации вправо. Наклонить вправо и назад.int wobbleRightAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {2, 0, 0, 0, 0}, // Наклон вправо, ровные ноги {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // стопы ровные, ровные {300 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Коснитесь анимации ног. Нажмите обе ноги. Int tapFeetAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {2, 0, 0, 0, 0}, // Поднять обе ступни, ровные ступни {500, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Ноги ровные, ступни ровные { 500, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Коснитесь левой ноги anim.int tapLeftFootAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {2, 0, 0, 0, 0}, // Поднять левую ногу, Ноги ровные {500, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootFlat ()}, // Ноги ровные, Ноги ровные {500 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Нажмите правой ногой anim.int tapRightFootAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {2, 0, 0, 0, 0}, // Поднять правую ногу, Ноги ровные {500, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Ноги ровные, Ноги ровные {500 , LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Отскок вверх и вниз anim.int bounceAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {2, 0, 0, 0, 0}, // Поднять обе ступни, ровные ступни {500, LeftHipCentre (), LeftFootDown (300), RightHipCentre (), RightFootDown (300)}, // Ноги ровные, ступни ровные { 500, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // Встряхните ноги Animation.int shakeLegsAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {14, 0, 0, 0, 0}, // наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // наклон влево, правое бедро внутрь { 100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Наклон влево, ровные ноги {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), Right) , // Наклон влево, правое бедро наружу {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Наклон влево, даже ноги {100, LeftHipCentre (), LeftFoot_DELTA , RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Наклон влево, правое бедро в {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Наклон влево {FOOT_DELTA)}, // даже наклон 100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Ноги ровные, Ноги ровные {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFoot}, //Flat Наклонная установка ht, Feet even {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, левое бедро в {100, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), LeftFootDown (FOOT_DELTA), LeftFootDown (FOOT_DELTA) , RightFootUp (FOOT_DELTA)}, // Наклон вправо, даже ноги {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, левое бедро наружу {100, LeftHipOot ), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, ровные ноги {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipTACentre (), RightFoot_DUp (), RightFoot_DUp (), RightFoot_DUp () , Feet even {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Встряхнуть Left Leg Animation.int shakeLeftLegAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {12, 0, 0, 0, 0}, // наклон вправо, ровные ноги {300, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // наклон вправо, левое бедро внутрь { 100, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, ровные ступни {100, LeftHipCentre (), LeftFootDTA (FOOTU_DELTA), Right) , // наклон вправо, левое бедро наружу {100, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // наклон вправо, даже ступни {100, LeftHipCentre (), LeftFoot_Down (), LeftFoot_Down (), LeftFoot_Down , RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, левое бедро в {100, LeftHipIn (HIP_DELTA), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA right, Feet even {FOOT_DELTA right, Feet even) 100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // наклон вправо, левое бедро наружу {100, LeftHipOut (HIP_DELTA), LeftFootDown (FOOT_DELCentre), RightFootDown (FOOT_DELTA), otUp (FOOT_DELTA)}, // наклон вправо, ровные ноги {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // наклон вправо, левое бедро в {100, Left_DELTA) (HIP) , LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFootUp (FOOT_DELTA)}, // Наклон вправо, ровные ступни {100, LeftHipCentre (), LeftFootDown (FOOT_DELTA), RightHipCentre (), RightFoot_DEL), FOOT_DELTA, // (FOOT_DELTA), RightFoot_Up (FOOT_DELTA), Feet even {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()}}; // Встряхните Right Leg Animation.int shakeRightLegAnim [] [5] ={// Метаданные. Первый элемент - это количество кадров. {12, 0, 0, 0, 0}, // наклон влево, ровные ноги {300, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // наклон влево, правое бедро внутрь { 100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Наклон влево, ровные ноги {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), Right) , // Наклон влево, правое бедро наружу {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipOut (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Наклон влево, даже ноги {100, LeftHipCentre (), LeftFoot_DELTA , RightHipCentre (), RightFootDown (FOOT_DELTA)}, // Наклон влево, правое бедро в {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Наклон влево {FOOT_DELTA)}, // даже наклон 100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // наклон влево, правое бедро наружу {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), Right_FootUp (FOOT_DELTA), RightHipOut () (FOOT_DELTA)}, // наклон влево, ровные ноги {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA)}, // наклон влево, правое бедро в {100, LeftHipCentre (), LeftFootUp (), LeftFootUp (), Left (FOOT_DELTA), RightHipIn (HIP_DELTA), RightFootDown (FOOT_DELTA)}, // Наклон влево, даже ноги {100, LeftHipCentre (), LeftFootUp (FOOT_DELTA), RightHipCentre (), RightFootDown (FOOT_DELTA), // FeetDown (FOOT_DELTA) // даже {300, LeftHipCentre (), LeftFootFlat (), RightHipCentre (), RightFootFlat ()},}; // ------------------------- -------------------------------------------------- ------- // Специальные данные динамической анимации для настройки / анимации положения сервопривода .//---------------------------- -------------------------------------------------- ---- // Это 2 специальных данных анимации, которые мы используем для функции SetServos (). У них // единственный кадр. Они изменят данные в этих анимированных данных и воспроизведут их, чтобы // переместить servos.int setServosAnim1 [] [5] ={// Метаданные. Первый элемент - это количество кадров. { 1, 0, 0, 0, 0 }, // Tilt left, Feet even { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};int setServosAnim2[][5] ={ // Metadata. First element is number of frames. { 1, 0, 0, 0, 0 }, // Tilt left, Feet even { 0, LeftHipCentre(), LeftFootFlat(), RightHipCentre(), RightFootFlat() }};//----------------------------------------------------------------------------------// Servo Variables//----------------------------------------------------------------------------------Servo servoLeftHip;Servo servoLeftFoot;Servo servoRightHip;Servo servoRightFoot;//----------------------------------------------------------------------------------// State variables for playing animations.//----------------------------------------------------------------------------------// Milliseconds between animation updates.const int millisBetweenAnimUpdate =20;// Time when we did the last animation update.long timeAtLastAnimUpdate;// Related to currently playing anim.int (*currAnim)[5]; // Current animation we're playing.int (*finishAnim)[5]; // Animation to play when the currAnim finishes or is stopped.long timeAtStartOfFrame; // millis() at last keyframe - frame we're lerping fromint targetFrame; // Frame we are lerping toint animNumLoops; // Number of times to play the animation. -1 means loop forever.char animCompleteStr[3] ="--"; // This is a 2 character string. When the anim is complete, // we print out the status as "<" + animComplereStr + ">".// Related to anim queue. Т.е. Next anim to play.bool animInProgress; // Whether an animation is playingint (*nextAnim)[5]; // This is the next animation to play once the current one is done. // i.e. It's like a queue of size 1! // If curr is non-looping, we play this at the end of the current anim. // If curr is looping, this starts at the end of the current loop, // replacing curr anim. // If nothing is playing, this starts right away. int (*nextFinishAnim)[5]; // This is the finish animation for the queued animation.int nextAnimNumLoops; // Number of times to play the animation. -1 means loop forever.char nextAnimCompleteStr[3] ="--"; // This is a 2 character string. When the anim is complete, // we print out the status as "<" + animComplereStr + ">".bool interruptInProgressAnim; // Whether to change anim immediately, interrupting the current one.// Curr servo positionsint currLeftHip;int currLeftFoot;int currRightHip;int currRightFoot;// Servo positions at start of current keyframeint startLeftHip;int startLeftFoot;int startRightHip;int startRightFoot;//-------------------------------------------------------------------------------// Parser Variables//-------------------------------------------------------------------------------// Constant delimiter tag charsconst char START_CHAR ='<';const char END_CHAR ='>';const char SEP_CHAR =',';// Constants and a variable for the parser state.const int PARSER_WAITING =0; // Waiting for '<' to start parsing.const int PARSER_COMMAND =1; // Reading the command string.const int PARSER_PARAM1 =2; // Reading param 1.const int PARSER_PARAM2 =3; // Reading param 2.const int PARSER_PARAM3 =4; // Reading param 3.const int PARSER_PARAM4 =5; // Reading param 3.const int PARSER_PARAM5 =6; // Reading param 3.const int PARSER_EXECUTE =7; // Finished parsing a command, so execute it.// Current parser state.int currParserState =PARSER_WAITING; // String for storing the command. 2 chars for the command and 1 char for '\0'.// We store the command here as we're parsing.char currCmd[3] ="--";// For tracking which letter we are in the command.int currCmdIndex;// Max command length.const int CMD_LENGTH =2;// Current param values. Store them here after we parse them.int currParam1Val;int currParam2Val;int currParam3Val;int currParam4Val;int currParam5Val;// Variable for tracking which digit we're parsing in a param.// We use this to convert the single digits back into a decimal value.int currParamIndex;// Whether the current param is negative.boolean currParamNegative;// Max parameter length. Stop parsing if it exceeds this.const int MAX_PARAM_LENGTH =6;//===============================================================================// Arduino setup() and loop().//===============================================================================void setup() { // Setup the main serial port softwareSerial.begin(SERIAL_SPEED); // Setup the Servos servoLeftHip.attach( SERVO_LEFT_HIP, LEFT_HIP_MIN, LEFT_HIP_MAX); servoLeftFoot.attach( SERVO_LEFT_FOOT, LEFT_FOOT_MIN, LEFT_FOOT_MAX); servoRightHip.attach( SERVO_RIGHT_HIP, RIGHT_HIP_MIN, RIGHT_HIP_MAX); servoRightFoot.attach(SERVO_RIGHT_FOOT, RIGHT_FOOT_MIN, RIGHT_FOOT_MAX); // Set things up for the parser. setup_Parser(); // Set things up for the animation code. setup_Animation();}void loop() { // Update the parser. loop_Parser(); // Update the animation. loop_Animation();}//===============================================================================// Related to the parser//===============================================================================// Sets up the parser stuff. Called in setup(). Should not be called elsewhere.void setup_Parser(){ // Wait for first command. currParserState =PARSER_WAITING; // Print this response to say we've booted and are ready. softwareSerial.println("");}// Loop() for the parser stuff. Called in loop(). Should not be called elsewhere.void loop_Parser(){ //--------------------------------------------------------- // PARSER // // If there is data, parse it and process it. //--------------------------------------------------------- // Read from pin serial port and write it out on USB port. if (softwareSerial.available()> 0) { char c =softwareSerial.read(); // If we're in WAITING state, look for the START_CHAR. if (currParserState ==PARSER_WAITING) { // If it's the START_CHAR, move out of this state... if (c ==START_CHAR) { // Start parsing the command. currParserState =PARSER_COMMAND; // Reset thing ready for parsing currCmdIndex =0; currCmd[0] ='-'; currCmd[1] ='-'; currParam1Val =0; currParam2Val =0; currParam3Val =0; currParam4Val =0; currParam5Val =0; } // Otherwise, stay in this state. } // In the state to look for the command. else if (currParserState ==PARSER_COMMAND) { // Else if it's a separator, parse parameter 1. But make sure it's not // empty, or else it's a parse error. if (c ==SEP_CHAR) { if (currCmdIndex ==CMD_LENGTH) { currParserState =PARSER_PARAM1; currParamIndex =0; currParamNegative =false; } else { currParserState =PARSER_WAITING; } } // Else if it's the end char, there are no parameters, so we're ready to // process. But make sure it's not empty. Otherwise, it's a parse error. else if (c ==END_CHAR) { if (currCmdIndex ==CMD_LENGTH) { currParserState =PARSER_EXECUTE; } else { currParserState =PARSER_WAITING; } } // If we've got too many letters here, we have a parse error, // so abandon and go back to PARSER_WAITING else if ( (currCmdIndex>=CMD_LENGTH) || (c <'A') || (c> 'Z') ) { currParserState =PARSER_WAITING; } // Store the current character. else { currCmd[currCmdIndex] =c; currCmdIndex++; } } // In the state to parse param 1. else if (currParserState ==PARSER_PARAM1) { // Else if it's a separator, parse parameter 1. if (c ==SEP_CHAR) { if (currParamNegative) { currParam1Val =-1 * currParam1Val; } currParserState =PARSER_PARAM2; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam1Val =-1 * currParam1Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam1Val =(currParam1Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 2. else if (currParserState ==PARSER_PARAM2) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam2Val =-1 * currParam2Val; } currParserState =PARSER_PARAM3; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam2Val =-1 * currParam2Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam2Val =(currParam2Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 3. else if (currParserState ==PARSER_PARAM3) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_PARAM4; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam3Val =-1 * currParam3Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam3Val =(currParam3Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 4. else if (currParserState ==PARSER_PARAM4) { // Else if it's a separator, parse parameter 2. if (c ==SEP_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_PARAM5; currParamIndex =0; currParamNegative =false; } // Else if it's the end char, there are no parameters, so we're ready to // process. else if (c ==END_CHAR) { if (currParamNegative) { currParam4Val =-1 * currParam4Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam4Val =(currParam4Val * 10) + currDigitVal; currParamIndex++; } } // In the state to parse param 5. else if (currParserState ==PARSER_PARAM5) { // If it's the end char, there are no parameters, so we're ready to // process. if (c ==END_CHAR) { if (currParamNegative) { currParam5Val =-1 * currParam5Val; } currParserState =PARSER_EXECUTE; } // Check for negative at the start. else if ( (currParamIndex ==0) &&(c =='-') ) { currParamNegative =true; currParamIndex++; } // If it's too long, or the character is not a digit, then it's // a parse error, so abandon and go back to PARSER_WAITING. else if ( (currParamIndex>=MAX_PARAM_LENGTH) || (c <'0') || (c> '9') ) { currParserState =PARSER_WAITING; } // It's a valid character, so process it. else { // Shift existing value across and add new digit at the bottom. int currDigitVal =c - '0'; currParam5Val =(currParam5Val * 10) + currDigitVal; currParamIndex++; } } //--------------------------------------------------------- // PARSER CODE HANDLER (Still part of Parser, but section that // processes completed commands) // // If the most recently read char completes a command, // then process the command, and clear the state to // go back to looking for a new command. // // The parsed items are stored in:// currCmd, currParam1Val, currParam2Val, currParam3Val, // currParam4Val, currParam5Val //--------------------------------------------------------- if (currParserState ==PARSER_EXECUTE) { // Ready/OK Check: if ((currCmd[0] =='O') &&(currCmd[1] =='K')) { softwareSerial.println(""); } // Set Servo: // time - time to tween to specified angles // leftHip - microsecs from centre. -ve is hip in, +ve is hip out // leftFoot - microsecs from flat. -ve is foot down, +ve is foot up // rightHip - microsecs from centre. -ve is hip in, +ve is hip out // rightFoot - microsecs from flat. -ve is foot down, +ve is foot up else if ((currCmd[0] =='S') &&(currCmd[1] =='V')) { int tweenTime =currParam1Val; if (currParam1Val <0) { tweenTime =0; } SetServos(tweenTime, currParam2Val, currParam3Val, currParam4Val, currParam5Val, "SV"); } // Stop/Reset:, Stops current anim. Also can be used to put robot into reset position. else if ((currCmd[0] =='S') &&(currCmd[1] =='T')) { StopAnim("ST"); } // Stop Immediate: else if ((currCmd[0] =='S') &&(currCmd[1] =='I')) { StopAnimImmediate("SI"); } // Forward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='F') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkForwardAnim, walkEndAnim, numTimes, "FW"); } // Backward:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='W')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(walkBackwardAnim, walkEndAnim, numTimes, "BW"); } // Turn Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnLeftAnim, NULL, numTimes, "LT"); } // Turn Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='R') &&(currCmd[1] =='T')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(turnRightAnim, NULL, numTimes, "RT"); } // Shake Head:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='S') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeHeadAnim, NULL, numTimes, "SX"); } // Bounce:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='B') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(bounceAnim, NULL, numTimes, "BX"); } // Wobble:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleAnim, NULL, numTimes, "WX"); } // Wobble Left:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleLeftAnim, NULL, numTimes, "WY"); } // Wobble Right:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='W') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(wobbleRightAnim, NULL, numTimes, "WZ"); } // Tap Feet:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapFeetAnim, NULL, numTimes, "TX"); } // Tap Left Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapLeftFootAnim, NULL, numTimes, "TY"); } // Tap Right Foot:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='T') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(tapRightFootAnim, NULL, numTimes, "TZ"); } // Shake Legs:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='X')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLegsAnim, NULL, numTimes, "LX"); } // Shake Left Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Y')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeLeftLegAnim, NULL, numTimes, "LY"); } // Shake Right Leg:, -1 means continuous, 0 or no param is the same as 1 time. else if ((currCmd[0] =='L') &&(currCmd[1] =='Z')) { int numTimes =currParam1Val; if (currParam1Val <0) { numTimes =-1; } PlayAnimNumTimes(shakeRightLegAnim, NULL, numTimes, "LZ"); } //-------------------------------------------------- // Clear the state and wait for the next command! // This must be done! //-------------------------------------------------- currParserState =PARSER_WAITING; } }}//===============================================================================// Related to playing servo animations.//===============================================================================// Call this to play the given animation once. Pass in NULL if there is no finishAnim.void PlayAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, 1, completeStr);}// Call this to loop the given animation. Pass in NULL if there is no finishAnim.void LoopAnim(int animToPlay[][5], int finishAnim[][5], const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(animToPlay, finishAnim, -1, completeStr);}// Call this to play the given animation the specified number of times. // -1 number of times will make it loop forever.// Pass in NULL if there is no finishAnim.void PlayAnimNumTimes(int animToPlay[][5], int finishAnim[][5], int numTimes, const char *completeStr){ // Put this in the queue. nextAnim =animToPlay; nextFinishAnim =finishAnim; nextAnimNumLoops =numTimes; // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; }}// Stop after the current animation.void StopAnim(const char *completeStr){ // Put this in the queue. PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Stop immediately and lerp robot to zero position, interrupting // any animation that is in progress.void StopAnimImmediate(const char *completeStr){ // Put this in the queue. interruptInProgressAnim =true; PlayAnimNumTimes(standStraightAnim, NULL, 1, completeStr);}// Moves servos to the specified positions. Time 0 will make it immediate. Otherwise,// it'll tween it over a specified time.// For positions, 0 means centered.// For hips, -ve is hip left, +ve is hip right// For feet, -ve is foot down, +ve is foot upvoid SetServos(int tweenTime, int leftHip, int leftFoot, int rightHip, int rightFoot, const char* completeStr){ // Save the completeStr if (completeStr ==NULL) { nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; } else { nextAnimCompleteStr[0] =completeStr[0]; nextAnimCompleteStr[1] =completeStr[1]; } // Decide which tween data we use. We don't want to over-write the one that is // in progress. We have and reuse these to keep memory allocation fixed. int (*tweenServoData)[5]; if (currAnim !=setServosAnim1) { tweenServoData =setServosAnim1; } else { tweenServoData =setServosAnim2; } // Set the tween information into the animation data. tweenServoData[1][TWEEN_TIME_VALUE] =tweenTime; tweenServoData[1][LEFT_HIP_VALUE] =LeftHipIn(leftHip); tweenServoData[1][LEFT_FOOT_VALUE] =LeftFootUp(leftFoot); tweenServoData[1][RIGHT_HIP_VALUE] =RightHipIn(rightHip); tweenServoData[1][RIGHT_FOOT_VALUE] =RightFootUp(rightFoot); // Queue this tween to be played next. PlayAnim(tweenServoData, NULL, completeStr);}// Set up variables for animation. This is called in setup(). Should be not called by anywhere else.void setup_Animation(){ // Set the servos to the feet flat, feet even position. currLeftHip =LEFT_HIP_CENTRE; currLeftFoot =LEFT_FOOT_CENTRE; currRightHip =RIGHT_HIP_CENTRE; currRightFoot =RIGHT_FOOT_CENTRE; UpdateServos(); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // No animation is playing yet, and nothing in the queue yet. timeAtLastAnimUpdate =millis(); animInProgress =false; interruptInProgressAnim =false; currAnim =NULL; finishAnim =NULL; nextAnim =NULL; nextFinishAnim =NULL;}// Loop function for processing animation. This is called in every loop(). Should be be called by anywhere else.//// NOTE:The way looping animations work is that they basically add themselves back to the queue// when a cycle is done, and if there's nothing already queued up! This way, looping animations// work in a similar way to single-play animations, and fits into the queueing system.void loop_Animation(){ // Get the time at the start of this frame. long currTime =millis(); //-------------------------------------------------------------------------------------- // Decide if we want to perform the animation update. We don't execute this every frame. //-------------------------------------------------------------------------------------- if (timeAtLastAnimUpdate + millisBetweenAnimUpdate> currTime) { // Not yet time to do an anim update, so jump out. возвращение; } else { // We reset the timer, and then proceed below to handle the current anim update. timeAtLastAnimUpdate =currTime; } //-------------------------------------------------------------------------------------- // Decide if we need to setup and start a new animation. We do if there's no anim // playing or we've been asked to interrupt the anim. //-------------------------------------------------------------------------------------- if ( (nextAnim !=NULL) &&(!animInProgress || interruptInProgressAnim) ) { // If this was an interrupt, we also set the "start" servo positions // to the current ones. This way, the animation system will tween from the // current positions. if (interruptInProgressAnim) { // This is the place to notify someone of an animation finishing after getting interrupted // Print the command string we just finished. -1 parameter indicates it was interrupted. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(",-1>"); // Set the "start" positions to the current ones. So, when // we pay the next anim, we will tween from the current positions. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // We've handled any interrupt request, so clear the flag. interruptInProgressAnim =false; } // Store the animation we are now playing. currAnim =nextAnim; finishAnim =nextFinishAnim; animCompleteStr[0] =nextAnimCompleteStr[0]; animCompleteStr[1] =nextAnimCompleteStr[1]; nextAnim =NULL; // Queue is cleared. nextFinishAnim =NULL; nextAnimCompleteStr[0] ='-'; nextAnimCompleteStr[1] ='-'; // Record the number of times to play the animation. animNumLoops =nextAnimNumLoops; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } //-------------------------------------------------------------------------------------- // If we are currently playing an animation, then update the animation state and the // servo positions. //-------------------------------------------------------------------------------------- if (animInProgress) { // Determine if we need to switch to the next frame. int timeInCurrFrame =currTime - timeAtStartOfFrame; if (timeInCurrFrame> currAnim[targetFrame][TWEEN_TIME_VALUE]) { // Set the servo positions to the targetFrame's values. // We only set this if the value is> 0. -ve values means that // the current target keyframe did not alter that servos position. if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =currAnim[targetFrame][LEFT_HIP_VALUE]; } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =currAnim[targetFrame][LEFT_FOOT_VALUE]; } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =currAnim[targetFrame][RIGHT_HIP_VALUE]; } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =currAnim[targetFrame][RIGHT_FOOT_VALUE]; } UpdateServos(); // These current values are now the start of frame values. startLeftHip =currLeftHip; startLeftFoot =currLeftFoot; startRightHip =currRightHip; startRightFoot =currRightFoot; // Now, we try to move to the next frame. // - If there is a next frame, set that as the new target, and proceed. // - If there's no next frame, but it's looping, we re-add this animation // to the queue. // - If there's no next frame, and this is not looping, we stop animating. // (Remember that targetFrame is 1-based since the first element of the animation // data array is metadata) // Increment targetFrame, and reset time in the current frame. targetFrame++; timeAtStartOfFrame =currTime; // If there is no next frame, we stop this current animation. // If it is looping, then we re-queue the current animation if the queue is empty. if (targetFrame> NumOfFrames(currAnim)) { // Stop the current animation. animInProgress =false; // If we're looping forever, and there's no next anim, re-queue the // animation if the queue is empty. if ((animNumLoops <0) &&(nextAnim ==NULL)) { LoopAnim(currAnim, finishAnim, animCompleteStr); } // If we're looping forever, and there is something in the queue, then // finish the animation and proceed. else if ((animNumLoops <0) &&(nextAnim !=NULL)) { if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } else { // We've stopped, so can notify if needed. // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } // If we're looping a limited number of times, and there's no next anim, // re-queue the animation if the queue is empty. else if ((animNumLoops> 1) &&(nextAnim ==NULL)) { PlayAnimNumTimes(currAnim, finishAnim, animNumLoops-1, animCompleteStr); } // In this case, numAnimLoops is 1, this is the last loop through, so // we're done. We play the finishAnim first if needed. else { // If there is a finish animation, switch to that animation. if (finishAnim !=NULL) { // Switch to the finish anim. currAnim =finishAnim; finishAnim =NULL; // Record the number of times to play the animation. animNumLoops =1; // Treat current time as start of frame for the initial lerp to the first frame. timeAtStartOfFrame =currTime; // Set the frame counters. targetFrame =1; // First frame we are lerping to. Index 0 is metadata, so skip. // An animation is now in progress animInProgress =true; } // Otherwise, we're done! We've played the finishAnim if there was one. else { // Print the command string we just finished. softwareSerial.print("<"); softwareSerial.print(animCompleteStr); softwareSerial.println(">"); } } } } // If we're still animating (i.e. the previous check didn't find that // we've finished the current animation), then proceed. if (animInProgress) { // Set the servos per data in the current frame. We only update the servos that have target // microsecond values> 0. This is to support the feature where we leave a servo at its // existing position if an animation data item is -1. float frameTimeFraction =(currTime - timeAtStartOfFrame) / ((float) currAnim[targetFrame][TWEEN_TIME_VALUE]); if (currAnim[targetFrame][LEFT_HIP_VALUE]>=0) { currLeftHip =startLeftHip + ((currAnim[targetFrame][LEFT_HIP_VALUE] - startLeftHip) * frameTimeFraction); } if (currAnim[targetFrame][LEFT_FOOT_VALUE]>=0) { currLeftFoot =startLeftFoot + ((currAnim[targetFrame][LEFT_FOOT_VALUE] - startLeftFoot) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_HIP_VALUE]>=0) { currRightHip =startRightHip + ((currAnim[targetFrame][RIGHT_HIP_VALUE] - startRightHip) * frameTimeFraction); } if (currAnim[targetFrame][RIGHT_FOOT_VALUE]>=0) { currRightFoot =startRightFoot + ((currAnim[targetFrame][RIGHT_FOOT_VALUE] - startRightFoot) * frameTimeFraction); } UpdateServos(); } }}// Move all the servo to the positions set in the curr... variables.// In the code, we update those variables and then call this to set the servos.void UpdateServos(){ servoLeftHip.writeMicroseconds(currLeftHip); servoLeftFoot.writeMicroseconds(currLeftFoot); servoRightHip.writeMicroseconds(currRightHip); servoRightFoot.writeMicroseconds(currRightFoot);}// Return the number of frames in the given animation data.// Have this helper function to avoid the "magic number" reference of animData[0][0].int NumOfFrames(int animData[][5]){ return animData[0][0];}

Схема