Четвероногий Arduino

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

Паяльник (универсальный)

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

IDE Arduino

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

Четвероногий на базе Ардуино! Четвероногий обозначает четвероногого бота, который в основном выглядит как четвероногий паук, поэтому давайте узнаем, как ходит паук, и попробуем воспроизвести его с помощью Arduino.

Расходные материалы:

Шаг 1. Необходимые компоненты

• 1 х Arduino Mega или Arduino Uno
• 1 просверленная печатная плата
• 12 серводвигателей (9 г)
• 1 ультразвуковой датчик HC-SR04
• 4 светодиода RGB
• Картон

Шаг 2. Поддержание компьютерной графики

центр тяжести (ЦТ) является основным фактором при ходьбе. Центр тяжести остается в центре тела, чтобы удерживать равновесие, если ЦТ смещается из центра в определенных пределах, это влияет на равновесие и приводит к падению

Итак, давайте посмотрим, как поддерживать компьютерную графику во время ходьбы.

Если каждая нога находится под углом 45 градусов, тогда ЦТ будет идеально главной в центре. Но если мы переместим любую ногу, ЦТ сместится в эту сторону, что приведет к падению на эту сторону.

Таким образом, чтобы избежать этого, обе конечные ножки поддерживаются под углом более 45 градусов в зависимости от размера бота, поэтому три ножки будут образовывать треугольник, если ЦТ будет внутри него, а четвертая ножка будет свободно двигаться, а ЦТ останется внутри треугольника.

Шаг 3. Процедура прогулки

• Это исходное положение:две ноги (C, D) вытянуты с одной стороны, а две другие ноги (A, B) вытянуты внутрь.
• Правая верхняя нога (B) поднимается и вытягивается далеко впереди робота.
• Все ноги смещаются назад, а корпус движется вперед.
• Левая задняя нога (D) поднимается и шагает вперед вместе с телом. Эта позиция является зеркальным отображением исходной позиции.
• Левая верхняя нога (B) поднимается и тянется вперед далеко впереди робота.
• Опять же, все ноги смещаются назад, а тело перемещается вперед.
• Правая задняя нога поднимается (B) и возвращается в тело, возвращая нас в исходное положение.

Шаг 4. Планы для четвероногих

Шаг 5:Построение тела

Постройте тело согласно PDF.

Шаг 6:Подключение схемы

Сделайте свой собственный экран в соответствии с вашими требованиями. Arduino Mega имеет вывод 15 pwm, используйте 12 из них для сервоприводов, 3 для светодиода RBG и любые два контакта для ультразвукового датчика

Шаг 7:инициализация сервопривода

• Загрузите программу на arduino mega и приступайте к сборке ножки согласно рисунку.

  #include  Servo servo [4] [3]; // определение портов сервоприводовconst int servo_pin [4] [3] ={{10,11,2}, {3,4 , 5}, {6,7,8}, {9, 12, 13}}; void setup () {// инициализируем все сервоприводы для (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .attach (servo_pin [i] [j]); задержка (20); }}} цикл void (void) {for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .write (90); задержка (20); }}}  

Шаг 8:Заключительный шаг

  / * Включает ----------------------------------------- ------------------------- * / # include  // для определения и управления сервоприводами # include  // чтобы установить таймер для управления всеми сервоприводами # определить ledred 46 # определить ledblue 44 # определить ledgreen 45 / * сервоприводы --------------------------- ----------------------------------------- * /// определяем 12 сервоприводов для 4 legServo servo [4] [3]; // определяем порты сервоприводовconst int servo_pin [4] [3] ={{2, 3, 4}, {20, 6, 7}, {8, 9, 17}, { 16, 12, 13}}; / * Размер робота ------------------------------------ --------------------- * / const float length_a =55; const float length_b =77,5; const float length_c =27,5; const float length_side =71; const float z_absolute =-28; / * Константы движения ----------------------------------------- ----------- * / const float z_default =-50, z_up =-30, z_boot =z_absolute; const float x_default =62, x_offset =0; const float y_start =0, y_step =40; const float y_default =x_default; / * переменные для движения ---------------- ------------------------------------ * / volatile float site_now [4] [3]; // текущие координаты конца каждого legvolatile float site_expect [4] [3]; // ожидаемые координаты конца каждого плавучего хода temp_speed [4] [3]; // скорость каждой оси необходимо пересчитывать перед каждым движением. float move_speed; // скорость движения на плаву speed_multiple =1; // скорость движения multiconst float spot_turn_speed =4; const float leg_move_speed =8; const float body_move_speed =3; const float stand_seat_speed =1; volatile int rest_counter; //+1/0.02с, для автоматического // функции остановки параметрconst float KEEP =255; // определяем PI для вычисленияconst float pi =3.1415926; / * Константы для поворота ------------- ------------------------------------------- * /// temp lengthconst float temp_a =sqrt (pow (2 * x_default + length_side, 2) + pow (y_step, 2)); const float temp_b =2 * (y_start + y_step) + length_side; const float temp_c =sqrt (pow (2 * x_default + length_side , 2) + pow (2 * y_start + y_step + length_side, 2)); const float temp_alpha =acos ((pow (temp_a, 2) + pow (temp_b, 2) - pow (temp_c, 2)) / 2 / temp_a / temp_b); // сайт для Turnconst float turn_x1 =(temp_a - length_side) / 2; const float turn_y1 =y_start + y_step / 2; const float turn_x0 =turn_x1 - temp_b * cos (temp_alpha); const float turn_y0 =temp_b * sin (temp_alpha) - turn_y1 - length_side; / * ---------------------------------------- ----------------------------------- * // * - функция настройки -------- -------------------------------------------------- ----------------- * / void setup () {pi nMode (ledred, ВЫХОД); pinMode (ledblue, ВЫХОД); pinMode (ledgreen, ВЫХОД); // запускаем серийник для отладки Serial.begin (115200); Serial.println («Робот начинает инициализацию»); // инициализировать параметр по умолчанию set_site (0, x_default - x_offset, y_start + y_step, z_boot); set_site (1, x_default - x_offset, y_start + y_step, z_boot); set_site (2, x_default + x_offset, y_start, z_boot); set_site (3, x_default + x_offset, y_start, z_boot); для (int я =0; я <4; я ++) {для (int j =0; j <3; j ++) {site_now [я] [j] =site_expect [я] [j]; }} // запускаем сервосервис FlexiTimer2 ::set (20, servo_service); FlexiTimer2 ::start (); Serial.println («Сервосервис запущен»); // инициализируем сервоприводы servo_attach (); Serial.println («Сервоприводы инициализированы»); Serial.println ("Инициализация робота завершена");} void servo_attach (void) {for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .attach (servo_pin [i] [j]); задержка (100); }}} void servo_detach (void) {для (int i =0; i <4; i ++) {для (int j =0; j <3; j ++) {servo [i] [j] .detach (); задержка (100); }}} / * - функция цикла ------------------------------------------ --------------------------------- * / void loop () {analogWrite (ledred, 255); Serial.println («Стенд»); стоять(); задержка (2000); analogWrite (ledred, 0); analogWrite (ledblue, 255); Serial.println («Шаг вперед»); step_forward (5); задержка (2000); analogWrite (ledblue, 0); analogWrite (ledgreen, 255); Serial.println («Шаг назад»); step_back (5); задержка (2000); analogWrite (ledgreen, 0); analogWrite (ledred, 255); analogWrite (ledblue, 255); Serial.println («Поверните налево»); Turn_left (5); задержка (2000); analogWrite (ledgreen, 255); analogWrite (ledred, 0); analogWrite (ledblue, 255); Serial.println («Поверните направо»); Turn_right (5); задержка (2000); analogWrite (ledgreen, 255); analogWrite (ledred, 255); analogWrite (ledblue, 0); Serial.println («Ручной взмах»); hand_wave (3); задержка (2000); Serial.println («Ручной взмах»); hand_shake (3); задержка (2000); int x =100; для (int я =0; я <5; я ++) {analogWrite (ledgreen, 255); analogWrite (ledred, 255); // белый analogWrite (ledblue, 255); задержка (х); analogWrite (ledgreen, 255); // желтый analogWrite (ledred, 255); analogWrite (ledblue, 0); задержка (х); analogWrite (ledgreen, 255); // голубой analogWrite (ledred, 0); analogWrite (ledblue, 255); задержка (х); analogWrite (ledgreen, 0); analogWrite (ledred, 255); // фиолетовый analogWrite (ledblue, 255); задержка (х); analogWrite (ledgreen, 0); analogWrite (ledred, 255); // красный analogWrite (ledblue, 0); задержка (х); analogWrite (ledgreen, 0); // синий analogWrite (ledred, 0); analogWrite (ledblue, 255); задержка (х); analogWrite (ledgreen, 255); analogWrite (ledred, 0); analogWrite (ledblue, 0); // зеленая задержка (x); } analogWrite (ledgreen, 0); analogWrite (ledred, 0); analogWrite (ledblue, 0); //Serial.println("Body dance "); // body_dance (10); // задержка (2000); //Serial.println("Sit");// sit (); delay (1000);} / * - sit - функция блокировки ------------------------------------- -------------------------------------- * / void sit (void) {move_speed =stand_seat_speed; для (int leg =0; leg <4; leg ++) {set_site (leg, KEEP, KEEP, z_boot); } wait_all_reach ();} / * - стенд - функция блокировки ------------------------------------- -------------------------------------- * / void stand (void) {move_speed =stand_seat_speed; для (int leg =0; leg <4; leg ++) {set_site (leg, KEEP, KEEP, z_default); } wait_all_reach ();} / * - поворот точки налево - функция блокировки - шаги шага параметра хотели повернуть --------------------------- ------------------------------------------------ * / void turn_left (unsigned int step) {move_speed =spot_turn_speed; while (step--> 0) {if (site_now [3] [1] ==y_start) {// перемещение 3 и 1 отрезка set_site (3, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (0, turn_x1 - x_offset, turn_y1, z_default); set_site (1, turn_x0 - x_offset, turn_y0, z_default); set_site (2, turn_x1 + x_offset, turn_y1, z_default); set_site (3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach (); set_site (3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach (); set_site (0, turn_x1 + x_offset, turn_y1, z_default); set_site (1, turn_x0 + x_offset, turn_y0, z_default); set_site (2, turn_x1 - x_offset, turn_y1, z_default); set_site (3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach (); set_site (1, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach (); set_site (0, x_default + x_offset, y_start, z_default); set_site (1, x_default + x_offset, y_start, z_up); set_site (2, x_default - x_offset, y_start + y_step, z_default); set_site (3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach (); set_site (1, x_default + x_offset, y_start, z_default); wait_all_reach (); } else {// ноги 0 и 2 перемещаем set_site (0, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (0, turn_x0 + x_offset, turn_y0, z_up); set_site (1, turn_x1 + x_offset, turn_y1, z_default); set_site (2, turn_x0 - x_offset, turn_y0, z_default); set_site (3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach (); set_site (0, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach (); set_site (0, turn_x0 - x_offset, turn_y0, z_default); set_site (1, turn_x1 - x_offset, turn_y1, z_default); set_site (2, turn_x0 + x_offset, turn_y0, z_default); set_site (3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach (); set_site (2, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach (); set_site (0, x_default - x_offset, y_start + y_step, z_default); set_site (1, x_default - x_offset, y_start + y_step, z_default); set_site (2, x_default + x_offset, y_start, z_up); set_site (3, x_default + x_offset, y_start, z_default); wait_all_reach (); set_site (2, x_default + x_offset, y_start, z_default); wait_all_reach (); }}} / * - точечный поворот направо - функция блокировки - параметр шага шагов хотел повернуть ------------------------------ --------------------------------------------- * / void turn_right ( unsigned int step) {move_speed =spot_turn_speed; while (step-> 0) {if (site_now [2] [1] ==y_start) {// нога 2 &0 перемещаем set_site (2, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (0, turn_x0 - x_offset, turn_y0, z_default); set_site (1, turn_x1 - x_offset, turn_y1, z_default); set_site (2, turn_x0 + x_offset, turn_y0, z_up); set_site (3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach (); set_site (2, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach (); set_site (0, turn_x0 + x_offset, turn_y0, z_default); set_site (1, turn_x1 + x_offset, turn_y1, z_default); set_site (2, turn_x0 - x_offset, turn_y0, z_default); set_site (3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach (); set_site (0, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach (); set_site (0, x_default + x_offset, y_start, z_up); set_site (1, x_default + x_offset, y_start, z_default); set_site (2, x_default - x_offset, y_start + y_step, z_default); set_site (3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach (); set_site (0, x_default + x_offset, y_start, z_default); wait_all_reach (); } else {// ноги 1 и 3 перемещаем set_site (1, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (0, turn_x1 + x_offset, turn_y1, z_default); set_site (1, turn_x0 + x_offset, turn_y0, z_up); set_site (2, turn_x1 - x_offset, turn_y1, z_default); set_site (3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach (); set_site (1, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach (); set_site (0, turn_x1 - x_offset, turn_y1, z_default); set_site (1, turn_x0 - x_offset, turn_y0, z_default); set_site (2, turn_x1 + x_offset, turn_y1, z_default); set_site (3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach (); set_site (3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach (); set_site (0, x_default - x_offset, y_start + y_step, z_default); set_site (1, x_default - x_offset, y_start + y_step, z_default); set_site (2, x_default + x_offset, y_start, z_default); set_site (3, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (3, x_default + x_offset, y_start, z_default); wait_all_reach (); }}} / * - вперед - функция блокировки - требуется выполнить шаги шага параметра -------------------------------- ------------------------------------------- * / void step_forward (unsigned int шаг) {move_speed =leg_move_speed; while (step--> 0) {if (site_now [2] [1] ==y_start) {// перемещение 2 и 1 отрезка set_site (2, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (2, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach (); move_speed =body_move_speed; set_site (0, x_default + x_offset, y_start, z_default); set_site (1, x_default + x_offset, y_start + 2 * y_step, z_default); set_site (2, x_default - x_offset, y_start + y_step, z_default); set_site (3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach (); move_speed =leg_move_speed; set_site (1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (1, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (1, x_default + x_offset, y_start, z_default); wait_all_reach (); } else {// ноги 0 и 3 перемещаем set_site (0, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (0, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach (); move_speed =body_move_speed; set_site (0, x_default - x_offset, y_start + y_step, z_default); set_site (1, x_default - x_offset, y_start + y_step, z_default); set_site (2, x_default + x_offset, y_start, z_default); set_site (3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach (); move_speed =leg_move_speed; set_site (3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (3, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (3, x_default + x_offset, y_start, z_default); wait_all_reach (); }}} / * - вернуться - функция блокировки - требуется выполнить шаги шага параметра -------------------------------- ------------------------------------------- * / void step_back (unsigned int шаг) {move_speed =leg_move_speed; while (step-> 0) {if (site_now [3] [1] ==y_start) {// перемещение 3 и 0 отрезка set_site (3, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach (); move_speed =body_move_speed; set_site (0, x_default + x_offset, y_start + 2 * y_step, z_default); set_site (1, x_default + x_offset, y_start, z_default); set_site (2, x_default - x_offset, y_start + y_step, z_default); set_site (3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach (); move_speed =leg_move_speed; set_site (0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (0, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (0, x_default + x_offset, y_start, z_default); wait_all_reach (); } else {// ноги 1 и 2 перемещаем set_site (1, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (1, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach (); move_speed =body_move_speed; set_site (0, x_default - x_offset, y_start + y_step, z_default); set_site (1, x_default - x_offset, y_start + y_step, z_default); set_site (2, x_default + x_offset, y_start + 2 * y_step, z_default); set_site (3, x_default + x_offset, y_start, z_default); wait_all_reach (); move_speed =leg_move_speed; set_site (2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach (); set_site (2, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (2, x_default + x_offset, y_start, z_default); wait_all_reach (); }}} // добавляем RegisHsuvoid body_left (int i) {set_site (0, site_now [0] [0] + i, KEEP, KEEP); set_site (1, site_now [1] [0] + я, СОХРАНИТЬ, СОХРАНИТЬ); set_site (2, site_now [2] [0] - я, СОХРАНИТЬ, СОХРАНИТЬ); set_site (3, site_now [3] [0] - я, СОХРАНИТЬ, СОХРАНИТЬ); wait_all_reach ();} void body_right (int i) {set_site (0, site_now [0] [0] - я, KEEP, KEEP); set_site (1, site_now [1] [0] - я, СОХРАНИТЬ, СОХРАНИТЬ); set_site (2, site_now [2] [0] + я, СОХРАНИТЬ, СОХРАНИТЬ); set_site (3, site_now [3] [0] + я, СОХРАНИТЬ, СОХРАНИТЬ); wait_all_reach ();} void hand_wave (int i) {float x_tmp; float y_tmp; float z_tmp; move_speed =1; если (site_now [3] [1] ==y_start) {body_right (15); x_tmp =site_now [2] [0]; y_tmp =site_now [2] [1]; z_tmp =site_now [2] [2]; move_speed =body_move_speed; для (int j =0; j  i / 4) move_speed =body_dance_speed * 2; если (j> i / 2) move_speed =body_dance_speed * 3; set_site (0, СОХРАНИТЬ, y_default - 20, СОХРАНИТЬ); set_site (1, СОХРАНИТЬ, y_default + 20, СОХРАНИТЬ); set_site (2, СОХРАНИТЬ, y_default - 20, СОХРАНИТЬ); set_site (3, СОХРАНИТЬ, y_default + 20, СОХРАНИТЬ); wait_all_reach (); set_site (0, СОХРАНИТЬ, y_default + 20, СОХРАНИТЬ); set_site (1, СОХРАНИТЬ, y_default - 20, СОХРАНИТЬ); set_site (2, СОХРАНИТЬ, y_default + 20, СОХРАНИТЬ); set_site (3, СОХРАНИТЬ, y_default - 20, СОХРАНИТЬ); wait_all_reach (); } move_speed =body_dance_speed; head_down (30);} / * - служба микросервисов / функция прерывания таймера / 50 Гц - при установке ожидаемого сайта эта функция перемещает конечную точку к нему по прямой линии - temp_speed [4] [3] должен быть установлен перед установкой ожидаемого сайта , убедитесь, что конечная точка движется по прямой линии, и определите скорость движения. -------------------------------------------------- ------------------------- * / void servo_service (void) {sei (); статический плавающий альфа, бета, гамма; for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {if (abs (site_now [i] [j] - site_expect [i] [j])> =abs (temp_speed [i] [j])) site_now [i] [j] + =temp_speed [i] [j]; иначе site_now [я] [j] =site_expect [я] [j]; } cartesian_to_polar (альфа, бета, гамма, site_now [i] [0], site_now [i] [1], site_now [i] [2]); polar_to_servo (я, альфа, бета, гамма); } rest_counter ++;} / * - установить одну из конечных точек ожидаемого сайта - эта функция одновременно установит temp_speed [4] [3] - неблокирующая функция --------------- -------------------------------------------------- ---------- * / void set_site (int leg, float x, float y, float z) {float length_x =0, length_y =0, length_z =0; if (x! =KEEP) length_x =x - site_now [leg] [0]; if (y! =KEEP) length_y =y - site_now [leg] [1]; if (z! =KEEP) length_z =z - site_now [leg] [2]; длина с плавающей запятой =sqrt (pow (length_x, 2) + pow (length_y, 2) + pow (length_z, 2)); temp_speed [leg] [0] =length_x / length * move_speed * speed_multiple; temp_speed [leg] [1] =length_y / length * move_speed * speed_multiple; temp_speed [leg] [2] =length_z / length * move_speed * speed_multiple; если (x! =KEEP) site_expect [нога] [0] =x; если (y! =KEEP) site_expect [leg] [1] =y; if (z! =KEEP) site_expect [leg] [2] =z;} / * - ждать, когда одна из конечных точек переместится, чтобы ожидать сайт - функция блокировки ----------------- -------------------------------------------------- -------- * / void wait_reach (int leg) {while (1) if (site_now [leg] [0] ==site_expect [leg] [0]) if (site_now [leg] [1] ==site_expect [leg] [1]) if (site_now [leg] [2] ==site_expect [leg] [2]) break;} / * - ждать, когда все конечные точки переместятся, чтобы ожидать сайт - функция блокировки ---- -------------------------------------------------- --------------------- * / void wait_all_reach (void) {for (int i =0; i <4; i ++) wait_reach (i);} / * - переход от декартовой системы координат к полярной - математическая модель 2/2 ------------------------------------- -------------------------------------- * / void cartesian_to_polar (изменчивое число с плавающей запятой и альфа, изменчивое число с плавающей точкой и бета , volatile float &gamma, volatile float x, volatile float y, volatile float z) {// вычислить wz степень float v, w; w =(x> =0? 1:-1) * (sqrt (pow (x, 2) + pow (y, 2))); v =w - длина_c; альфа =atan2 (z, v) + acos ((pow (length_a, 2) - pow (length_b, 2) + pow (v, 2) + pow (z, 2)) / 2 / length_a / sqrt (pow (v , 2) + pow (z, 2))); бета =acos ((pow (length_a, 2) + pow (length_b, 2) - pow (v, 2) - pow (z, 2)) / 2 / length_a / length_b); // вычислить степень x-y-z gamma =(w> =0)? атан2 (у, х):атан2 (-у, -х); // степень трансформации pi-> 180 alpha =alpha / pi * 180; бета =бета / пи * 180; gamma =gamma / pi * 180;} / * - переход с полярного на микросерво - соответствие математической модели факту - ошибки, сохраненные в eeprom, будут добавлены ----------------- -------------------------------------------------- -------- * / void polar_to_servo (int leg, float alpha, float beta, float gamma) {if (leg ==0) {alpha =90 - alpha; бета =бета; гамма + =90; } иначе, если (нога ==1) {альфа + =90; бета =180 - бета; гамма =90 - гамма; } иначе, если (нога ==2) {альфа + =90; бета =180 - бета; гамма =90 - гамма; } иначе, если (нога ==3) {альфа =90 - альфа; бета =бета; гамма + =90; } серво [нога] [0] .write (альфа); серво [нога] [1] .write (бета); серво [нога] [2] .write (гамма);}  

Подключите выводы светодиода

• Вот и все, четвероногий готов!
• Загрузите программу.
• Подключите сервопривод в соответствии с выводами, определенными в программе.

Код

• паук
• spider_fix.ino

паук Arduino

 / * Включает -------------------------------------------- ---------------------- * / # include  // для определения и управления сервоприводами # include  // для установки таймер для управления всеми сервоприводами # определить ledred 46 # определить ledblue 44 # определить ledgreen 45 / * сервоприводы ------------------------------ -------------------------------------- * /// определяем 12 сервоприводов на 4 ноги Сервопривод [ 4] [3]; // определяем порты сервоприводовconst int servo_pin [4] [3] ={{2, 3, 4}, {20, 6, 7}, {8, 9, 17}, {16, 12 , 13}}; / * Размер робота --------------------------------------- ------------------ * / const float length_a =55; const float length_b =77,5; const float length_c =27,5; const float length_side =71; const float z_absolute =-28; / * Константы движения -------------------------------------------- -------- * / const float z_default =-50, z_up =-30, z_boot =z_absolute; const float x_default =62, x_offset =0; const float y_start =0, y_step =40; const float y_default =x_default; / * переменные для движения ---------------------- ------------------------------ * / volatile float site_now [4] [3]; // текущие координаты конца каждого legvolatile float site_expect [4] [3]; // ожидаемые координаты конца каждого плавучего хода temp_speed [4] [3]; // скорость каждой оси необходимо пересчитывать перед каждым движением. float move_speed; // скорость движения на плаву speed_multiple =1; // скорость движения multiconst float spot_turn_speed =4; const float leg_move_speed =8; const float body_move_speed =3; const float stand_seat_speed =1; volatile int rest_counter; //+1/0.02с, для автоматического // функции отдыха параметрconst float KEEP =255; // определяем PI для вычисленияconst float pi =3.1415926; / * Константы для поворота ------------- ------------------------------------------- * /// temp lengthconst float temp_a =sqrt (pow (2 * x_default + length_side, 2) + pow (y_step, 2)); const float temp_b =2 * (y_start + y_step) + length_side; const float temp_c =sqrt (pow (2 * x_default + length_side , 2) + pow (2 * y_start + y_step + length_side, 2)); const float temp_alpha =acos ((pow (temp_a, 2) + pow (temp_b, 2) - pow (temp_c, 2)) / 2 / temp_a / temp_b); // сайт для Turnconst float turn_x1 =(temp_a - length_side) / 2; const float turn_y1 =y_start + y_step / 2; const float turn_x0 =turn_x1 - temp_b * cos (temp_alpha); const float turn_y0 =temp_b * sin (temp_alpha) - turn_y1 - length_side; / * ---------------------------------------- ----------------------------------- * // * - функция настройки -------- -------------------------------------------------- ----------------- * / void setup () {pi nMode (ledred, ВЫХОД); pinMode (ledblue, ВЫХОД); pinMode (ledgreen, ВЫХОД); // запускаем серийник для отладки Serial.begin (115200); Serial.println («Робот начинает инициализацию»); // инициализировать параметр по умолчанию set_site (0, x_default - x_offset, y_start + y_step, z_boot); set_site (1, x_default - x_offset, y_start + y_step, z_boot); set_site (2, x_default + x_offset, y_start, z_boot); set_site (3, x_default + x_offset, y_start, z_boot); для (int я =0; я <4; я ++) {для (int j =0; j <3; j ++) {site_now [я] [j] =site_expect [я] [j]; }} // запускаем сервосервис FlexiTimer2 ::set (20, servo_service); FlexiTimer2 ::start (); Serial.println («Сервосервис запущен»); // инициализируем сервоприводы servo_attach (); Serial.println («Сервоприводы инициализированы»); Serial.println ("Инициализация робота завершена");} void servo_attach (void) {for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .attach (servo_pin [i] [j]); задержка (100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); задержка (100); } }}/* - loop function ---------------------------------------------------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stand(); задержка (2000); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Step forward"); step_forward(5); задержка (2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Step back"); step_back(5); задержка (2000); analogWrite(ledgreen,0); analogWrite(ledred,255); analogWrite(ledblue,255); Serial.println("Turn left"); turn_left(5); задержка (2000); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Turn right"); turn_right(5); задержка (2000); analogWrite(ledgreen,255); analogWrite(ledred,255); analogWrite(ledblue,0); Serial.println("Hand wave"); hand_wave(3); задержка (2000); Serial.println("Hand wave"); hand_shake(3); задержка (2000); int x=100; for(int i=0;i<5;i++) { analogWrite(ledgreen,255); analogWrite(ledred,255);//white analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,255);//yellow analogWrite(ledred,255); analogWrite(ledblue,0); delay(x); analogWrite(ledgreen,255);//cyan analogWrite(ledred,0); analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,0); analogWrite(ledred,255);//purple analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,0); analogWrite(ledred,255);//red analogWrite(ledblue,0); delay(x); analogWrite(ledgreen,0);//blue analogWrite(ledred,0); analogWrite(ledblue,255); delay(x); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,0); //green delay(x); } analogWrite(ledgreen,0); analogWrite(ledred,0); analogWrite(ledblue,0); //Serial.println("Body dance"); //body_dance(10); // delay(2000); //Serial.println("Sit"); // sit(); delay(1000);}/* - sit - blocking function ---------------------------------------------------------------------------*/void sit(void){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach();}/* - stand - blocking function ---------------------------------------------------------------------------*/void stand(void){ move_speed =stand_seat_speed; for (int leg =0; leg <4; leg++) { set_site(leg, KEEP, KEEP, z_default); } wait_all_reach();}/* - spot turn to left - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/void turn_left(unsigned int step){ move_speed =spot_turn_speed; while (step--> 0) { if (site_now[3][1] ==y_start) { //leg 3&1 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_default); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start, z_up); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&2 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_default); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_up); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - spot turn to right - blocking function - parameter step steps wanted to turn ---------------------------------------------------------------------------*/void turn_right(unsigned int step){ move_speed =spot_turn_speed; while (step--> 0) { if (site_now[2][1] ==y_start) { //leg 2&0 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_up); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&3 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x1 + x_offset, turn_y1, z_default); set_site(1, turn_x0 + x_offset, turn_y0, z_up); set_site(2, turn_x1 - x_offset, turn_y1, z_default); set_site(3, turn_x0 - x_offset, turn_y0, z_default); wait_all_reach(); set_site(1, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x1 - x_offset, turn_y1, z_default); set_site(1, turn_x0 - x_offset, turn_y0, z_default); set_site(2, turn_x1 + x_offset, turn_y1, z_default); set_site(3, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(3, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - go forward - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/void step_forward(unsigned int step){ move_speed =leg_move_speed; while (step--> 0) { if (site_now[2][1] ==y_start) { //leg 2&1 move set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default + x_offset, y_start, z_default); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 0&3 move set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - go back - blocking function - parameter step steps wanted to go ---------------------------------------------------------------------------*/void step_back(unsigned int step){ move_speed =leg_move_speed; while (step--> 0) { if (site_now[3][1] ==y_start) { //leg 3&0 move set_site(3, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(1, x_default + x_offset, y_start, z_default); set_site(2, x_default - x_offset, y_start + y_step, z_default); set_site(3, x_default - x_offset, y_start + y_step, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start, z_default); wait_all_reach(); } else { //leg 1&2 move set_site(1, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); move_speed =leg_move_speed; set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}// add by RegisHsuvoid body_left(int i){ set_site(0, site_now[0][0] + i, KEEP, KEEP); set_site(1, site_now[1][0] + i, KEEP, KEEP); set_site(2, site_now[2][0] - i, KEEP, KEEP); set_site(3, site_now[3][0] - i, KEEP, KEEP); wait_all_reach();}void body_right(int i){ set_site(0, site_now[0][0] - i, KEEP, KEEP); set_site(1, site_now[1][0] - i, KEEP, KEEP); set_site(2, site_now[2][0] + i, KEEP, KEEP); set_site(3, site_now[3][0] + i, KEEP, KEEP); wait_all_reach();}void hand_wave(int i){ float x_tmp; float y_tmp; float z_tmp; move_speed =1; if (site_now[3][1] ==y_start) { body_right(15); x_tmp =site_now[2][0]; y_tmp =site_now[2][1]; z_tmp =site_now[2][2]; move_speed =body_move_speed; for (int j =0; j  i / 4) move_speed =body_dance_speed * 2; if (j> i / 2) move_speed =body_dance_speed * 3; set_site(0, KEEP, y_default - 20, KEEP); set_site(1, KEEP, y_default + 20, KEEP); set_site(2, KEEP, y_default - 20, KEEP); set_site(3, KEEP, y_default + 20, KEEP); wait_all_reach(); set_site(0, KEEP, y_default + 20, KEEP); set_site(1, KEEP, y_default - 20, KEEP); set_site(2, KEEP, y_default + 20, KEEP); set_site(3, KEEP, y_default - 20, KEEP); wait_all_reach(); } move_speed =body_dance_speed; head_down(30);}/* - microservos service /timer interrupt function/50Hz - when set site expected,this function move the end point to it in a straight line - temp_speed[4][3] should be set before set expect site,it make sure the end point move in a straight line,and decide move speed. ---------------------------------------------------------------------------*/void servo_service(void){ sei(); static float alpha, beta, gamma; for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { if (abs(site_now[i][j] - site_expect[i][j])>=abs(temp_speed[i][j])) site_now[i][j] +=temp_speed[i][j]; else site_now[i][j] =site_expect[i][j]; } cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); polar_to_servo(i, alpha, beta, gamma); } rest_counter++;}/* - set one of end points' expect site - this founction will set temp_speed[4][3] at same time - non - blocking function ---------------------------------------------------------------------------*/void set_site(int leg, float x, float y, float z){ float length_x =0, length_y =0, length_z =0; if (x !=KEEP) length_x =x - site_now[leg][0]; if (y !=KEEP) length_y =y - site_now[leg][1]; if (z !=KEEP) length_z =z - site_now[leg][2]; float length =sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2)); temp_speed[leg][0] =length_x / length * move_speed * speed_multiple; temp_speed[leg][1] =length_y / length * move_speed * speed_multiple; temp_speed[leg][2] =length_z / length * move_speed * speed_multiple; if (x !=KEEP) site_expect[leg][0] =x; if (y !=KEEP) site_expect[leg][1] =y; if (z !=KEEP) site_expect[leg][2] =z;}/* - wait one of end points move to expect site - blocking function ---------------------------------------------------------------------------*/void wait_reach(int leg){ while (1) if (site_now[leg][0] ==site_expect[leg][0]) if (site_now[leg][1] ==site_expect[leg][1]) if (site_now[leg][2] ==site_expect[leg][2]) break;}/* - wait all of end points move to expect site - blocking function ---------------------------------------------------------------------------*/void wait_all_reach(void){ for (int i =0; i <4; i++) wait_reach(i);}/* - trans site from cartesian to polar - mathematical model 2/2 ---------------------------------------------------------------------------*/void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z){ //calculate w-z degree float v, w; w =(x>=0 ? 1 :-1) * (sqrt(pow(x, 2) + pow(y, 2))); v =w - length_c; alpha =atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2))); beta =acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b); //calculate x-y-z degree gamma =(w>=0) ? atan2(y, x) :atan2(-y, -x); //trans degree pi->180 alpha =alpha / pi * 180; beta =beta / pi * 180; gamma =gamma / pi * 180;}/* - trans site from polar to microservos - mathematical model map to fact - the errors saved in eeprom will be add ---------------------------------------------------------------------------*/void polar_to_servo(int leg, float alpha, float beta, float gamma){ if (leg ==0) { alpha =90 - alpha; beta =beta; gamma +=90; } else if (leg ==1) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==2) { alpha +=90; beta =180 - beta; gamma =90 - gamma; } else if (leg ==3) { alpha =90 - alpha; beta =beta; gamma +=90; } servo[leg][0].write(alpha); servo[leg][1].write(beta); servo[leg][2].write(gamma);}

spider_fix.inoArduino

// Locate the initial position of legs // RegisHsu 2015-09-09#include  Servo servo[4][3];//define servos' portsconst int servo_pin[4][3] ={ {10,11,2}, {3,4,5}, {6,7,8}, {9, 12, 13} };void setup(){ //initialize all servos for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].attach(servo_pin[i][j]); задержка (20); } }}void loop(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].write(90); задержка (20); }}} 

Изготовленные на заказ детали и корпуса

Схема