Arduino Cuadrúpedo

Acerca de este proyecto

¡Cuadrúpedo basado en Arduino! Quadruped significa bot de cuatro patas, que básicamente parece una araña de cuatro patas, así que aprendamos cómo camina la araña e intentemos replicarla con Arduino.

Suministros:

Paso 1:componentes necesarios

• 1 X Arduino Mega o Arduino Uno
• 1 X PCB perforado
• 12 X servomotores (9g)
• 1 X Sensor ultrasónico HC-SR04
• 4 LED X RGB
• Cartón

Paso 2:mantenimiento de CG

El centro de gravedad (CG) es el factor principal al caminar. El centro de gravedad permanece en el centro del cuerpo para mantener el equilibrio si el CG se mueve fuera del centro en ciertos límites, entonces el equilibrio se verá afectado y provocará una caída

Así que veamos cómo mantener el CG mientras camina.

Si cada pierna está en 45 grados, entonces el CG será perfectamente principal en el centro, pero si movimos cualquier pierna, el CG se desplazará hacia ese lado y provocará una caída de ese lado.

Entonces, para evitar esto, las patas de los extremos se mantienen en un ángulo superior a 45 grados según el tamaño del bot, por lo que las tres patas formarán un triángulo, donde el CG estará dentro y la cuarta pata estará libre para moverse y CG. permanecerá dentro de un triángulo.

Paso 3:procedimiento para caminar

• Esta es la posición inicial, con dos piernas (C, D) extendidas hacia afuera en un lado y las otras dos piernas (A, B) hacia adentro.
• La pierna superior derecha (B) se levanta y se extiende, muy por delante del robot.
• Todas las piernas se mueven hacia atrás, moviendo el cuerpo hacia adelante.
• La pierna trasera izquierda (D) se eleva y avanza a lo largo del cuerpo. Esta posición es la imagen especular de la posición inicial.
• La pierna superior izquierda (B) se eleva y se extiende, muy por delante del robot.
• Nuevamente, todas las piernas se mueven hacia atrás, moviendo el cuerpo hacia adelante.
• La pierna trasera derecha se eleva (B) y retrocede hacia el cuerpo, llevándonos de regreso a la posición inicial.

Paso 4:planes para cuadrúpedos

Paso 5:Construcción del cuerpo

Construya el cuerpo según PDF.

Paso 6:Conexión del circuito

Haga su propio escudo de acuerdo con sus requisitos arduino mega tiene 15 pwm pin, use 12 de ellos para conexiones de servo y 3 para LED RBG y dos pines para sensor ultrasónico

Paso 7:Inicialización del Servo

• Cargue el programa en arduino mega y comience a ensamblar la pierna de acuerdo con la imagen

  #include  Servo servo [4] [3]; // define los puertos de los servosconst int servo_pin [4] [3] ={{10,11,2}, {3,4 , 5}, {6,7,8}, {9, 12, 13}}; void setup () {// inicializar todos los servos para (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .attach (servo_pin [i] [j]); retraso (20); }}} bucle vacío (void) {for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .write (90); retraso (20); }}}  

Paso 8:paso final

  / * Incluye ----------------------------------------- ------------------------- * / # include  // para definir y controlar los servos # include  // para configurar un temporizador para administrar todos los servos # definir ledred 46 # definir ledblue 44 # definir ledgreen 45 / * Servos --------------------------- ----------------------------------------- * /// definir 12 servos para 4 legsServo servo [4] [3]; // define el portsconst int de los servos servo_pin [4] [3] ={{2, 3, 4}, {20, 6, 7}, {8, 9, 17}, { 16, 12, 13}}; / * Tamaño del robot ------------------------------------ --------------------- * / 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; / * Constantes de movimiento ----------------------------------------- ----------- * / 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; / * variables de movimiento ---------------- ------------------------------------ * / volatile float site_now [4] [3]; // coordenadas en tiempo real del final de cada flotante legvolátil site_expect [4] [3]; // coordenadas esperadas del final de cada legfloat temp_speed [4] [3]; // la velocidad de cada eje, necesita ser recalculada antes de cada movimiento flotador move_speed; // movimiento speedfloat speed_multiple =1; // velocidad de movimiento multipleconst 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.02s, para descanso automático // parámetro de funcionesconst float KEEP =255; // define PI para cálculoconst float pi =3.1415926; / * Constantes para turno ------------- ------------------------------------------- * /// 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); // sitio para 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; / * ---------------------------------------- ----------------------------------- * // * - función de configuración -------- -------------------------------------------------- ----------------- * / void setup () {pi nMode (ledred, SALIDA); pinMode (ledblue, SALIDA); pinMode (ledgreen, SALIDA); // iniciar serial para depurar Serial.begin (115200); Serial.println ("El robot inicia la inicialización"); // inicializar el parámetro predeterminado 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); for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {site_now [i] [j] =site_expect [i] [j]; }} // iniciar el servo servicio FlexiTimer2 ::set (20, servo_service); FlexiTimer2 ::inicio (); Serial.println ("Servicio de servo iniciado"); // inicializar servos servo_attach (); Serial.println ("Servos inicializados"); Serial.println ("Inicialización del robot completa");} 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]); retraso (100); }}} void servo_detach (void) {for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .detach (); retraso (100); }}} / * - función de bucle ------------------------------------------ --------------------------------- * / void loop () {analogWrite (ledred, 255); Serial.println ("Soporte"); estar(); retraso (2000); analogWrite (ledred, 0); analogWrite (ledblue, 255); Serial.println ("Paso adelante"); paso_hacia adelante (5); retraso (2000); analogWrite (ledblue, 0); analogWrite (verde led, 255); Serial.println ("Paso atrás"); step_back (5); retraso (2000); analogWrite (verde led, 0); analogWrite (ledred, 255); analogWrite (ledblue, 255); Serial.println ("Girar a la izquierda"); giro_izquierda (5); retraso (2000); analogWrite (verde led, 255); analogWrite (ledred, 0); analogWrite (ledblue, 255); Serial.println ("Gire a la derecha"); girar_derecha (5); retraso (2000); analogWrite (verde led, 255); analogWrite (ledred, 255); analogWrite (ledblue, 0); Serial.println ("saludo con la mano"); hand_wave (3); retraso (2000); Serial.println ("saludo con la mano"); apretón de manos (3); retraso (2000); int x =100; para (int i =0; i <5; i ++) {analogWrite (ledgreen, 255); analogWrite (ledred, 255); // blanco analogWrite (ledblue, 255); retraso (x); analogWrite (ledgreen, 255); // amarillo analogWrite (ledred, 255); analogWrite (ledblue, 0); retraso (x); analogWrite (ledgreen, 255); // cyan analogWrite (ledred, 0); analogWrite (ledblue, 255); retraso (x); analogWrite (verde led, 0); analogWrite (ledred, 255); // violeta analogWrite (ledblue, 255); retraso (x); analogWrite (verde led, 0); analogWrite (ledred, 255); // rojo analogWrite (ledblue, 0); retraso (x); analogWrite (ledgreen, 0); // azul analogWrite (ledred, 0); analogWrite (ledblue, 255); retraso (x); analogWrite (verde led, 255); analogWrite (ledred, 0); analogWrite (ledblue, 0); // retardo verde (x); } analogWrite (verde led, 0); analogWrite (ledred, 0); analogWrite (ledblue, 0); //Serial.println("Body dance "); // body_dance (10); // retraso (2000); //Serial.println("Sit");// sit (); delay (1000);} / * - sit - función de bloqueo ------------------------------------- -------------------------------------- * / void sit (void) {move_speed =stand_seat_speed; for (int leg =0; leg <4; leg ++) {set_site (leg, MANTENER, MANTENER, z_boot); } wait_all_reach ();} / * - stand - función de bloqueo ------------------------------------- -------------------------------------- * / soporte vacío (vacío) {move_speed =stand_seat_speed; for (int leg =0; leg <4; leg ++) {set_site (leg, MANTENER, MANTENER, z_default); } wait_all_reach ();} / * - giro puntual a la izquierda - función de bloqueo - paso del parámetro pasos que quería girar --------------------------- ------------------------------------------------ * / void turn_left (paso int sin firmar) {move_speed =spot_turn_speed; while (paso--> 0) {if (site_now [3] [1] ==y_start) {// tramo 3 &1 mover set_site (3, x_default + x_offset, y_start, z_up); esperar_todos_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); esperar_todos_reach (); set_site (3, turn_x0 + x_offset, turn_y0, z_default); esperar_todos_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); esperar_todos_reach (); set_site (1, turn_x0 + x_offset, turn_y0, z_up); esperar_todos_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); esperar_todos_reach (); set_site (1, x_default + x_offset, y_start, z_default); esperar_todos_reach (); } else {// tramo 0 y 2 mover set_site (0, x_default + x_offset, y_start, z_up); esperar_todos_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); esperar_todos_reach (); set_site (0, turn_x0 + x_offset, turn_y0, z_default); esperar_todos_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); esperar_todos_reach (); set_site (2, turn_x0 + x_offset, turn_y0, z_up); esperar_todos_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); esperar_todos_reach (); set_site (2, x_default + x_offset, y_start, z_default); esperar_todos_reach (); }}} / * - giro puntual a la derecha - función de bloqueo - paso de parámetro pasos que quería girar ------------------------------ --------------------------------------------- * / void turn_right ( paso int sin firmar) {move_speed =spot_turn_speed; while (paso--> 0) {if (site_now [2] [1] ==y_start) {// tramo 2 &0 mover set_site (2, x_default + x_offset, y_start, z_up); esperar_todos_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); esperar_todos_reach (); set_site (2, turn_x0 + x_offset, turn_y0, z_default); esperar_todos_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); esperar_todos_reach (); set_site (0, turn_x0 + x_offset, turn_y0, z_up); esperar_todos_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); esperar_todos_reach (); set_site (0, x_default + x_offset, y_start, z_default); esperar_todos_reach (); } else {// tramo 1 y 3 mover set_site (1, x_default + x_offset, y_start, z_up); esperar_todos_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); esperar_todos_reach (); set_site (1, turn_x0 + x_offset, turn_y0, z_default); esperar_todos_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); esperar_todos_reach (); set_site (3, turn_x0 + x_offset, turn_y0, z_up); esperar_todos_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); esperar_todos_reach (); set_site (3, x_default + x_offset, y_start, z_default); esperar_todos_reach (); }}} / * - avanzar - función de bloqueo - paso de parámetro pasos a seguir -------------------------------- ------------------------------------------- * / void step_forward (unsigned int paso) {move_speed =leg_move_speed; while (paso--> 0) {if (site_now [2] [1] ==y_start) {// tramo 2 &1 mover set_site (2, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (2, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (2, x_default + x_offset, y_start + 2 * y_step, z_default); esperar_todos_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); esperar_todos_reach (); move_speed =leg_move_speed; set_site (1, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (1, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (1, x_default + x_offset, y_start, z_default); esperar_todos_reach (); } else {// tramo 0 y 3 mover set_site (0, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (0, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (0, x_default + x_offset, y_start + 2 * y_step, z_default); esperar_todos_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); esperar_todos_reach (); move_speed =leg_move_speed; set_site (3, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (3, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (3, x_default + x_offset, y_start, z_default); esperar_todos_reach (); }}} / * - retroceder - función de bloqueo - paso de parámetro pasos a seguir -------------------------------- ------------------------------------------- * / void step_back (unsigned int paso) {move_speed =leg_move_speed; while (paso--> 0) {if (site_now [3] [1] ==y_start) {// tramo 3 &0 mover set_site (3, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (3, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (3, x_default + x_offset, y_start + 2 * y_step, z_default); esperar_todos_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); esperar_todos_reach (); move_speed =leg_move_speed; set_site (0, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (0, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (0, x_default + x_offset, y_start, z_default); esperar_todos_reach (); } else {// tramo 1 y 2 mover set_site (1, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (1, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (1, x_default + x_offset, y_start + 2 * y_step, z_default); esperar_todos_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); esperar_todos_reach (); move_speed =leg_move_speed; set_site (2, x_default + x_offset, y_start + 2 * y_step, z_up); esperar_todos_reach (); set_site (2, x_default + x_offset, y_start, z_up); esperar_todos_reach (); set_site (2, x_default + x_offset, y_start, z_default); esperar_todos_reach (); }}} // agregar por RegisHsuvoid body_left (int i) {set_site (0, site_now [0] [0] + i, KEEP, KEEP); set_site (1, site_now [1] [0] + i, MANTENER, MANTENER); set_site (2, site_now [2] [0] - i, MANTENER, MANTENER); set_site (3, site_now [3] [0] - i, MANTENER, MANTENER); wait_all_reach ();} void body_right (int i) {set_site (0, site_now [0] [0] - i, MANTENER, MANTENER); set_site (1, site_now [1] [0] - i, MANTENER, MANTENER); set_site (2, site_now [2] [0] + i, MANTENER, MANTENER); set_site (3, site_now [3] [0] + i, MANTENER, MANTENER); esperar_todos_reach ();} void hand_wave (int i) {flotar x_tmp; float y_tmp; float z_tmp; move_speed =1; if (site_now [3] [1] ==y_start) {body_right (15); x_tmp =sitio_ahora [2] [0]; y_tmp =site_now [2] [1]; z_tmp =site_now [2] [2]; move_speed =body_move_speed; para (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, MANTENER, y_default - 20, MANTENER); set_site (1, MANTENER, y_default + 20, MANTENER); set_site (2, MANTENER, y_default - 20, MANTENER); set_site (3, MANTENER, y_default + 20, MANTENER); esperar_todos_reach (); set_site (0, MANTENER, y_default + 20, MANTENER); set_site (1, MANTENER, y_default - 20, MANTENER); set_site (2, MANTENER, y_default + 20, MANTENER); set_site (3, MANTENER, y_default - 20, MANTENER); esperar_todos_reach (); } move_speed =body_dance_speed; head_down (30);} / * - servicio de microservos / función de interrupción del temporizador / 50Hz - cuando se espera establecer el sitio, esta función mueve el punto final a él en línea recta - temp_speed [4] [3] debe establecerse antes de establecer el sitio esperado , asegúrese de que el punto final se mueva en línea recta y decida la velocidad de movimiento. -------------------------------------------------- ------------------------- * / void servo_service (void) {sei (); flotador estático alfa, 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]; } cartesiano_a_polar (alfa, beta, gamma, sitio_ahora [i] [0], sitio_ahora [i] [1], sitio_ahora [i] [2]); polar_to_servo (i, alfa, beta, gamma); } rest_counter ++;} / * - establece uno de los puntos finales 'sitio esperado - esta función establecerá temp_speed [4] [3] al mismo tiempo - función sin bloqueo --------------- -------------------------------------------------- ---------- * / void set_site (int leg, float x, float y, float z) {float length_x =0, length_y =0, length_z =0; if (x! =MANTENER) length_x =x - site_now [leg] [0]; if (y! =MANTENER) length_y =y - site_now [tramo] [1]; if (z! =MANTENER) length_z =z - site_now [tramo] [2]; longitud flotante =sqrt (pow (longitud_x, 2) + pow (longitud_y, 2) + pow (longitud_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! =MANTENER) site_expect [tramo] [0] =x; if (y! =MANTENER) site_expect [tramo] [1] =y; if (z! =KEEP) site_expect [leg] [2] =z;} / * - espera que uno de los puntos finales se mueva para esperar el sitio - función de bloqueo ----------------- -------------------------------------------------- -------- * / 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;} / * - esperar que todos los puntos finales se muevan para esperar la función de bloqueo del sitio ---- -------------------------------------------------- --------------------- * / void wait_all_reach (void) {for (int i =0; i <4; i ++) wait_reach (i);} / * - sitio trans de cartesiano a polar - modelo matemático 2/2 ------------------------------------- -------------------------------------- * / void cartesian_to_polar (flotante volátil y alfa, flotante volátil y beta , flotación volátil y gamma, flotación volátil x, flotación volátil y, flotación volátil z) {// calcular wz grado flotación v, w; w =(x> =0? 1:-1) * (sqrt (pow (x, 2) + pow (y, 2))); v =w - longitud_c; alpha =atan2 (z, v) + acos ((pow (longitud_a, 2) - pow (longitud_b, 2) + pow (v, 2) + pow (z, 2)) / 2 / longitud_a / sqrt (pow (v , 2) + pow (z, 2))); beta =acos ((pow (longitud_a, 2) + pow (longitud_b, 2) - pow (v, 2) - pow (z, 2)) / 2 / longitud_a / longitud_b); // calcular x-y-z grados gamma =(w> =0)? atan2 (y, x):atan2 (-y, -x); // grado trans pi-> 180 alfa =alfa / pi * 180; beta =beta / pi * 180; gamma =gamma / pi * 180;} / * - sitio trans de polar a microservos - mapa del modelo matemático al hecho - los errores guardados en eeprom se agregarán ----------------- -------------------------------------------------- -------- * / void polar_to_servo (int leg, float alpha, float beta, float gamma) {if (leg ==0) {alpha =90 - alpha; beta =beta; gamma + =90; } más si (pierna ==1) {alfa + =90; beta =180 - beta; gamma =90 - gamma; } más si (pierna ==2) {alfa + =90; beta =180 - beta; gamma =90 - gamma; } más si (pierna ==3) {alfa =90 - alfa; beta =beta; gamma + =90; } servo [leg] [0] .write (alfa); servo [pierna] [1] .escribir (beta); servo [leg] [2] .write (gamma);}  

Conecte los pines led

• Eso es todo, ¡tu cuadrúpedo está listo!
• Sube el programa.
• Conecte el servo de acuerdo con los pines definidos en el programa.

Código

• araña
• spider_fix.ino

araña Arduino

 / * Incluye -------------------------------------------- ---------------------- * / # include  // para definir y controlar los servos # include  // para establecer un temporizador para gestionar todos los servos # definir ledred 46 # definir ledblue 44 # definir ledgreen 45 / * Servos ------------------------------ -------------------------------------- * /// definir 12 servos para 4 patas Servo servo [ 4] [3]; // definir los portsconst int de los servos servo_pin [4] [3] ={{2, 3, 4}, {20, 6, 7}, {8, 9, 17}, {16, 12 , 13}}; / * Tamaño del robot --------------------------------------- ------------------ * / 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; / * Constantes de movimiento -------------------------------------------- -------- * / 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; / * variables de movimiento ---------------------- ------------------------------ * / flotante volátil site_now [4] [3]; // coordenadas en tiempo real del final de cada flotante legvolátil site_expect [4] [3]; // coordenadas esperadas del final de cada legfloat temp_speed [4] [3]; // la velocidad de cada eje, necesita ser recalculada antes de cada movimiento flotador move_speed; // movimiento speedfloat speed_multiple =1; // velocidad de movimiento multipleconst 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.02s, para descanso automático // parámetro de funcionesconst float KEEP =255; // define PI para cálculoconst float pi =3.1415926; / * Constantes para turno ------------- ------------------------------------------- * /// 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); // sitio para 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; / * ---------------------------------------- ----------------------------------- * // * - función de configuración -------- -------------------------------------------------- ----------------- * / void setup () {pi nMode (ledred, SALIDA); pinMode (ledblue, SALIDA); pinMode (ledgreen, SALIDA); // iniciar serial para depurar Serial.begin (115200); Serial.println ("El robot inicia la inicialización"); // inicializar el parámetro predeterminado 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); for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {site_now [i] [j] =site_expect [i] [j]; } } //start servo service FlexiTimer2::set(20, servo_service); FlexiTimer2::start(); Serial.println("Servo service started"); //initialize servos servo_attach(); Serial.println("Servos initialized"); Serial.println("Robot initialization Complete");}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]); retraso (100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); retraso (100); } }}/* - loop function ---------------------------------------------------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stand(); retraso (2000); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Step forward"); step_forward(5); retraso (2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Step back"); step_back(5); retraso (2000); analogWrite(ledgreen,0); analogWrite(ledred,255); analogWrite(ledblue,255); Serial.println("Turn left"); turn_left(5); retraso (2000); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Turn right"); turn_right(5); retraso (2000); analogWrite(ledgreen,255); analogWrite(ledred,255); analogWrite(ledblue,0); Serial.println("Hand wave"); hand_wave(3); retraso (2000); Serial.println("Hand wave"); hand_shake(3); retraso (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]); delay(20); } }}void loop(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].write(90); delay(20); }}} 

Piezas y carcasas personalizadas

Esquemas

¡Despiértame! Altavoz de miedo de proximidad PIR de Halloween con efectos de iluminación