Arduino Quadruped

Ferramentas e máquinas necessárias

Ferro de soldar (genérico)

Aplicativos e serviços online

Arduino IDE

Sobre este projeto

Suprimentos:

Etapa 1:componentes necessários

• 1 X Arduino Mega ou Arduino Uno
• 1 X PCB perfurado
• 12 servo motores X (9g)
• 1 sensor ultrassônico X HC-SR04
• 4 X RGB LED
• papelão

Etapa 2:Manutenção de CG

Etapa 3:Procedimento de caminhada

• Esta é a posição inicial, com duas pernas (C, D) estendidas de um lado e as outras duas pernas (A, B) puxadas para dentro.
• A perna superior direita (B) se levanta e se estende, muito à frente do robô.
• Todas as pernas se movem para trás, movendo o corpo para a frente.
• A perna esquerda posterior (D) se levanta e avança ao lado do corpo. Esta posição é a imagem espelhada da posição inicial.
• A perna superior esquerda (B) se levanta e se estende, muito à frente do robô.
• Novamente, todas as pernas se movem para trás, movendo o corpo para a frente.
• A perna direita posterior se eleva (B) e recua no corpo, trazendo-nos de volta à posição inicial.

Etapa 4:Planos para quadrúpede

Etapa 5:construção do corpo

Etapa 6:Conexão do circuito

Etapa 7:Inicialização do Servo

• Faça upload do programa para o arduino mega e comece a montar a perna de acordo com a imagem

  #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 () {// inicializar todos os servos para (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .attach (servo_pin [i] [j]); atraso (20); }}} void loop (void) {for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .write (90); atraso (20); }}}  

Etapa 8:Etapa final

  / * Inclui ----------------------------------------- ------------------------- * / # include  // para definir e controlar servos # include  // para definir um cronômetro para gerenciar todos os servos # define ledred 46 # define ledblue 44 # define ledgreen 45 / * Servos --------------------------- ----------------------------------------- * /// define 12 servos para 4 pernasServo servo [4] [3]; // define portasconst de servos int servo_pin [4] [3] ={{2, 3, 4}, {20, 6, 7}, {8, 9, 17}, { 16, 12, 13}}; / * Tamanho do robô ------------------------------------ --------------------- * / 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 para movimento ----------------------------------------- ----------- * / 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; / * variáveis ​​para movimento ---------------- ------------------------------------ * / volatile float site_now [4] [3]; // coordenadas em tempo real do final de cada legvolátil float site_expect [4] [3]; // coordenadas esperadas do final de cada legfloat temp_speed [4] [3]; // a velocidade de cada eixo, precisa ser recalculada antes de cada movimentofloat move_speed; // movimento speedfloat speed_multiple =1; // velocidade de movimento 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 // functions 'parameterconst float KEEP =255; // define PI para cálculoconst float pi =3.1415926; / * Constantes para curva ------------- ------------------------------------------- * /// 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_início + y_step + comprimento_lado, 2)); const float temp_alpha =acos ((pow (temp_a, 2) + pow (temp_b, 2) - pow (temp_c, 2)) / 2 / temp_a / temp_b); // local 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; / * ---------------------------------------- ----------------------------------- * // * - função de configuração -------- -------------------------------------------------- ----------------- * / void setup () {pi nMode (ledred, OUTPUT); pinMode (ledblue, OUTPUT); pinMode (ledgreen, OUTPUT); // inicia o serial para depuração Serial.begin (115200); Serial.println ("O robô inicia a inicialização"); // inicializa o parâmetro padrão 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]; }} // inicia o serviço servo FlexiTimer2 ::set (20, servo_service); FlexiTimer2 ::start (); Serial.println ("Serviço servo iniciado"); // inicializa servos servo_attach (); Serial.println ("Servos inicializados"); Serial.println ("inicialização do robô concluída");} 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]); atraso (100); }}} void servo_detach (void) {for (int i =0; i <4; i ++) {for (int j =0; j <3; j ++) {servo [i] [j] .detach (); atraso (100); }}} / * - função de loop ------------------------------------------ --------------------------------- * / void loop () {analogWrite (ledred, 255); Serial.println ("Stand"); ficar de pé(); atraso (2000); analogWrite (ledred, 0); analogWrite (ledblue, 255); Serial.println ("Avançar"); step_forward (5); atraso (2000); analogWrite (ledblue, 0); analogWrite (ledgreen, 255); Serial.println ("Recuar"); passo_back (5); atraso (2000); analogWrite (ledgreen, 0); analogWrite (ledred, 255); analogWrite (ledblue, 255); Serial.println ("Vire à esquerda"); turn_left (5); atraso (2000); analogWrite (ledgreen, 255); analogWrite (ledred, 0); analogWrite (ledblue, 255); Serial.println ("Vire à direita"); turn_right (5); atraso (2000); analogWrite (ledgreen, 255); analogWrite (ledred, 255); analogWrite (ledblue, 0); Serial.println ("Onda manual"); hand_wave (3); atraso (2000); Serial.println ("Onda manual"); aperto de mão (3); atraso (2000); int x =100; para (int i =0; i <5; i ++) {analogWrite (ledgreen, 255); analogWrite (ledred, 255); // branco analogWrite (ledblue, 255); atraso (x); analogWrite (ledgreen, 255); // amarelo analogWrite (ledred, 255); analogWrite (ledblue, 0); atraso (x); analogWrite (ledgreen, 255); // ciano analogWrite (ledred, 0); analogWrite (ledblue, 255); atraso (x); analogWrite (ledgreen, 0); analogWrite (ledred, 255); // roxo analogWrite (ledblue, 255); atraso (x); analogWrite (ledgreen, 0); analogWrite (ledred, 255); // vermelho analogWrite (ledblue, 0); atraso (x); analogWrite (ledgreen, 0); // azul analogWrite (ledred, 0); analogWrite (ledblue, 255); atraso (x); analogWrite (ledgreen, 255); analogWrite (ledred, 0); analogWrite (ledblue, 0); // atraso verde (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);} / * - sentar - função de bloqueio ------------------------------------- -------------------------------------- * / void sit (void) {move_speed =stand_seat_speed; para (perna interna =0; perna <4; perna ++) {set_site (perna, MANTER, MANTER, z_boot); } wait_all_reach ();} / * - stand - função de bloqueio ------------------------------------- -------------------------------------- * / void stand (void) {move_speed =stand_seat_speed; para (perna interna =0; perna <4; perna ++) {set_site (perna, KEEP, KEEP, z_default); } wait_all_reach ();} / * - virar à esquerda - função de bloqueio - etapas de parâmetro que deseja virar --------------------------- ------------------------------------------------ * / void turn_left (passo interno sem sinal) {move_speed =spot_turn_speed; while (etapa--> 0) {if (site_now [3] [1] ==y_início) {// trecho 3 e 1 mover set_site (3, x_default + x_offset, y_início, 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 {// perna 0 e 2 mover 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 (); }}} / * - virar à direita - função de bloqueio - etapas de parâmetro que deseja virar ------------------------------ --------------------------------------------- * / void turn_right ( passo interno sem sinal) {move_speed =spot_turn_speed; while (etapa--> 0) {if (site_now [2] [1] ==y_início) {// trecho 2 e 0 mover set_site (2, x_default + x_offset, y_início, 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 {// perna 1 e 3 mover 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 (); }}} / * - avançar - função de bloqueio - etapas de etapa de parâmetro desejadas -------------------------------- ------------------------------------------- * / void step_forward (unsigned int etapa) {move_speed =leg_move_speed; while (etapa--> 0) {if (site_now [2] [1] ==y_início) {// perna 2 e 1 mover set_site (2, x_default + x_offset, y_início, z_up); wait_all_reach (); set_site (2, x_default + x_offset, y_início + 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 {// perna 0 e 3 move set_site (0, x_default + x_offset, y_start, z_up); wait_all_reach (); set_site (0, x_default + x_offset, y_início + 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_início + 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 (); }}} / * - voltar - função de bloqueio - etapas de etapa de parâmetro desejadas -------------------------------- ------------------------------------------- * / void step_back (unsigned int etapa) {move_speed =leg_move_speed; while (etapa--> 0) {if (site_now [3] [1] ==y_início) {// trecho 3 e 0 mover set_site (3, x_default + x_offset, y_início, z_up); wait_all_reach (); set_site (3, x_default + x_offset, y_início + 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_início + 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 {// perna 1 e 2 mover 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_início + 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 (); }}} // adicionar por RegisHsuvoid body_left (int i) {set_site (0, site_now [0] [0] + i, KEEP, KEEP); set_site (1, site_now [1] [0] + i, MANTER, MANTER); set_site (2, site_now [2] [0] - i, MANTER, MANTER); set_site (3, site_now [3] [0] - i, MANTER, MANTER); wait_all_reach ();} void body_right (int i) {set_site (0, site_now [0] [0] - i, MANTER, MANTER); set_site (1, site_now [1] [0] - i, MANTER, MANTER); set_site (2, site_now [2] [0] + i, MANTER, MANTER); set_site (3, site_now [3] [0] + i, MANTER, MANTER); 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; 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, 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);} 

• That's it, your quadruped is ready!
• Upload the program.
• Connect the servo according to the pins defined in the program.

Código

• spider
• spider_fix.ino

spiderArduino

 /* Includes ------------------------------------------------------------------*/#include  //to define and control servos#include //to set a timer to manage all servos#define ledred 46#define ledblue 44#define ledgreen 45/* Servos --------------------------------------------------------------------*///define 12 servos for 4 legsServo servo[4][3];//define servos' portsconst int servo_pin[4][3] ={ {2, 3, 4}, {20, 6, 7}, {8, 9, 17}, {16, 12, 13} };/* Size of the 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;/* Constants for movement ----------------------------------------------------*/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 for movement ---------------------- ------------------------------*/volatile float site_now[4][3]; //real-time coordinates of the end of each legvolatile float site_expect[4][3]; //expected coordinates of the end of each legfloat temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movementfloat move_speed; //movement speedfloat speed_multiple =1; //movement speed 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, for automatic rest//functions' parameterconst float KEEP =255;//define PI for calculationconst float pi =3.1415926;/* Constants for turn --------------------------------------------------------*///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);//site for 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;/* ---------------------------------------------------------------------------*//* - setup function ---------------------------------------------------------------------------*/void setup(){ pi nMode(ledred,OUTPUT);pinMode(ledblue,OUTPUT);pinMode(ledgreen,OUTPUT); //start serial for debug Serial.begin(115200); Serial.println("Robot starts initialization"); //initialize default parameter 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]); atraso (100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); atraso (100); } }}/* - loop function ---------------------------------------------------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stand(); atraso (2000); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Step forward"); step_forward(5); atraso (2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Step back"); step_back(5); atraso (2000); analogWrite(ledgreen,0); analogWrite(ledred,255); analogWrite(ledblue,255); Serial.println("Turn left"); turn_left(5); atraso (2000); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Turn right"); turn_right(5); atraso (2000); analogWrite(ledgreen,255); analogWrite(ledred,255); analogWrite(ledblue,0); Serial.println("Hand wave"); hand_wave(3); atraso (2000); Serial.println("Hand wave"); hand_shake(3); atraso (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); }}} 

Peças personalizadas e gabinetes

Esquemas