Arduino Quadruped

Over dit project

Arduino-gebaseerde viervoeter! Quadruped staat voor vierpotige bot, die er in feite uitziet als een vierpotige spin, dus laten we leren hoe de spin loopt en proberen deze te repliceren met Arduino.

Benodigdheden:

Stap 1:Benodigde componenten

• 1 X Arduino Mega of Arduino Uno
• 1 X geboorde printplaat
• 12 X servomotoren (9g)
• 1 X HC-SR04 ultrasone sensor
• 4 X RGB-LED
• Kartonnen

Stap 2:CG behouden

zwaartepunt (CG) is de belangrijkste factor tijdens het lopen. Het zwaartepunt blijft in het midden van het lichaam om in evenwicht te blijven als het zwaartepunt op bepaalde limieten uit het midden beweegt, wordt het evenwicht aangetast en leidt dit tot vallen

Dus laten we eens kijken hoe je je zwaartepunt behoudt tijdens het lopen.

Als elk been zich in 45 graden bevindt, is het zwaartepunt perfect in het midden, maar als we een been verplaatsen, zal het zwaartepunt naar die kant verschuiven, zodat het aan die kant valt.

Dus om dit te voorkomen, worden de beide uiteinden van de benen in een hoek van meer dan 45 graden gehouden op basis van de botgrootte, zodat de drie benen een driehoek vormen, waar het zwaartepunt erin zit en het vierde been vrij is om te bewegen en het zwaartepunt blijft binnen een driehoek.

Stap 3:Loopprocedure

• Dit is de startpositie, met twee benen (C, D) aan één kant uitgestrekt en de andere twee benen (A, B) naar binnen getrokken.
• Het been rechtsboven (B) gaat omhoog en strekt zich uit, ver voor de robot uit.
• Alle benen schuiven naar achteren, waardoor het lichaam naar voren beweegt.
• Het linkerachterbeen (D) gaat omhoog en stapt naast het lichaam naar voren. Deze positie is het spiegelbeeld van de startpositie.
• Het linkerbovenbeen (B) gaat omhoog en strekt zich uit, ver voor de robot uit.
• Nogmaals, alle benen schuiven naar achteren, waardoor het lichaam naar voren beweegt.
• Het rechterachterbeen gaat omhoog (B) en stapt terug in het lichaam, waardoor we teruggaan naar de startpositie.

Stap 4:Plannen voor viervoeters

Stap 5:opbouw van het lichaam

Construeer de body volgens PDF.

Stap 6:Circuitverbinding

Maak je eigen schild volgens uw behoefte arduino mega heeft 15 pwm pin, gebruik 12 van hen voor servo-aansluitingen en 3 voor RBG led en twee pinnen voor ultrasone sensor

Stap 7:initialisatie van servo

• Upload het programma naar arduino mega en begin met het monteren van het been volgens de afbeelding

#include  Servo servo[4][3];//define servo's poortconst int servo_pin[4][3] ={ {10,11,2}, {3,4 ,5}, {6,7,8}, {9, 12, 13} };void setup(){ // initialiseer alle servo's voor (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].attach(servo_pin[i][j]); vertraging(20); } }}void loop(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].write(90); vertraging(20); } }} 

Stap 8:Laatste stap

 /* Inclusief ----------------------------------------- -------------------------*/#include  //om servo's te definiëren en te besturen#include // een timer instellen om alle servo's te beheren#define ledred 46#define ledblue 44#define ledgreen 45/* Servo's --------------------------- -----------------------------------------*///definieer 12 servo's voor 4 benenServo servo [4][3];//definieer de poorten van servo'sconst int servo_pin [4][3] ={{2, 3, 4}, {20, 6, 7}, {8, 9, 17}, { 16, 12, 13} };/* Grootte van de 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_absoluut =-28;/* Constanten voor beweging ----------------------------------------- -----------*/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;/* variabelen voor beweging ---------------- ------------------------------------*/vluchtige float site_now[4][3]; // realtime coördinaten van het einde van elke legvolatile float site_expect [4][3]; // verwachte coördinaten van het einde van elke legfloat temp_speed [4][3]; //de snelheid van elke as moet herberekend worden voor elke motionfloat move_speed; // beweging speedfloat speed_multiple =1; // bewegingssnelheid multipleconst float spot_turn_speed =4;const float leg_move_speed =8;const float body_move_speed =3;const float stand_seat_speed =1;vluchtige int rest_counter; //+1/0.02s, voor automatische rust//functie parameterconst float KEEP =255;//define PI voor berekeningconst float pi =3.1415926;/* Constanten voor draaien ------------- -------------------------------------------*///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 voor 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;/* ---------------------------------------- -----------------------------------*//* - instellingsfunctie -------- -------------------------------------------------- -----------------*/void setup(){ pi nMode(ledred,OUTPUT);pinMode(ledblue,OUTPUT);pinMode(ledgroen,OUTPUT); //start serial voor debug Serial.begin (115200); Serial.println("Robot start initialisatie"); // initialiseer standaard 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 gestart"); // initialiseer servo's servo_attach (); Serial.println ("Servo's geïnitialiseerd"); Serial.println ("Robotinitialisatie voltooid");}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]); vertraging (100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); vertraging (100); } }}/* - lusfunctie ------------------------------------------ ---------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stellage(); vertraging (2000); analoogWrite(ledred,0); analoogWrite(ledblue,255); Serial.println("Stap vooruit"); step_forward(5); vertraging (2000); analoogWrite(ledblue,0); analoogWrite(ledgroen,255); Serial.println("Stap terug"); step_back(5); vertraging (2000); analoogWrite(ledgroen,0); analoogWrite(ledred,255); analoogWrite(ledblue,255); Serial.println("Sla linksaf"); turn_left(5); vertraging (2000); analoogWrite(ledgroen,255); analoogWrite(ledred,0); analoogWrite(ledblue,255); Serial.println("Sla rechtsaf"); turn_right(5); vertraging (2000); analoogWrite(ledgroen,255); analoogWrite(ledred,255); analoogWrite(ledblue,0); Serial.println ("Handgolf"); hand_wave (3); vertraging (2000); Serial.println ("Handgolf"); hand_shake (3); vertraging (2000); int x=100; for(int i=0;i<5;i++) {analogeWrite(ledgreen,255); analogWrite(ledred,255);//wit analogWrite(ledblue,255); vertraging (x); analogWrite(ledgreen,255);//geel analogWrite(ledred,255); analoogWrite(ledblue,0); vertraging (x); analogWrite(ledgreen,255);//cyaan analogWrite(ledred,0); analoogWrite(ledblue,255); vertraging (x); analoogWrite(ledgroen,0); analogWrite(ledred,255);//paars analogWrite(ledblue,255); vertraging(x); analoogWrite(ledgroen,0); analogWrite(ledred,255);//rood analogWrite(ledblue,0); vertraging (x); analogWrite(ledgreen,0);//blauw analogWrite(ledred,0); analoogWrite(ledblue,255); vertraging (x); analoogWrite(ledgroen,255); analoogWrite(ledred,0); analoogWrite(ledblue,0); // groene vertraging (x); } analoogWrite(ledgroen,0); analoogWrite(ledred,0); analoogWrite(ledblue,0); //Serial.println ("Lichaamsdans"); //body_dance (10);// vertraging (2000); //Serial.println("Sit");// sit(); delay(1000);}/* - sit - blokkeerfunctie ------------------------------------- --------------------------------------*/void sit(void){ move_speed =stand_seat_speed; for (int been =0; been <4; been++) { set_site(leg, KEEP, KEEP, z_boot); } wait_all_reach();}/* - stand - blokkeerfunctie ------------------------------------- --------------------------------------*/void stand(void){ move_speed =stand_seat_speed; for (int been =0; been <4; been++) { set_site(leg, KEEP, KEEP, z_default); } wait_all_reach();}/* - spot draai naar links - blokkeerfunctie - parameter stap stappen wilden draaien --------------------------- ------------------------------------------------*/ void turn_left (unsigned int step){ move_speed =spot_turn_speed; while (stap--> 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 draai naar rechts - blokkeerfunctie - parameter stap stappen wilden draaien ------------------------------ ---------------------------------------------*/void turn_right( unsigned int step){ move_speed =spot_turn_speed; while (stap--> 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(); } }}/* - ga vooruit - blokkeerfunctie - parameter stap stappen wilden gaan -------------------------------- -------------------------------------------*/void step_forward(unsigned int stap){ move_speed =leg_move_speed; while (stap--> 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(); } }}/* - ga terug - blokkeerfunctie - parameter stap stappen wilden gaan -------------------------------- -------------------------------------------*/void step_back (unsigned int stap){ move_speed =leg_move_speed; while (stap--> 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(); } }}// toevoegen door 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; zweven y_tmp; zweven z_tmp; move_speed =1; if (site_now[3][1] ==y_start) { body_right (15); x_tmp =site_nu[2][0]; y_tmp =site_nu[2][1]; z_tmp =site_nu[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-onderbrekingsfunctie/50Hz - wanneer de site wordt verwacht, verplaatst deze functie het eindpunt ernaartoe in een rechte lijn - temp_speed[4][3] moet worden ingesteld voordat de verwachte site wordt ingesteld , het zorgt ervoor dat het eindpunt in een rechte lijn beweegt en bepaalt de bewegingssnelheid. -------------------------------------------------- -------------------------*/void servo_service(void){ sei(); statische vlotter alfa, bèta, 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, alfa, bèta, gamma); } rest_counter++;}/* - stel een van de verwachte eindpunten in - deze functie stelt tegelijkertijd temp_speed[4][3] in - niet-blokkerende functie --------------- -------------------------------------------------- ----------*/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 lengte =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;}/* - wacht een van de eindpunten om de site te verwachten - blokkeerfunctie ----------------- -------------------------------------------------- --------*/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]) pauze;}/* - wacht alle eindpunten verplaatsen om site te verwachten - blokkeerfunctie ---- -------------------------------------------------- ---------------------*/void wait_all_reach(void){ for (int i =0; i <4; i++) wait_reach(i);}/* - transsite van cartesiaans naar polair - wiskundig model 2/2 ------------------------------------- --------------------------------------*/void cartesian_to_polar(vluchtige float &alpha, vluchtige float &beta , vluchtige vlotter &gamma, vluchtige vlotter x, vluchtige vlotter y, vluchtige vlotter z){ //bereken wz graad vlotter v, w; w =(x>=0 ? 1 :-1) * (sqrt(pow(x, 2) + pow(y, 2))); v =w - lengte_c; alpha =atan2(z, v) + acos((pow(lengte_a, 2) - pow(lengte_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / lengte_a / sqrt(pow(v , 2) + pow(z, 2))); beta =acos((pow(lengte_a, 2) + pow(lengte_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / lengte_a / lengte_b); // bereken x-y-z graad gamma =(w>=0) ? atan2(y, x) :atan2(-y, -x); //trans graden pi->180 alpha =alpha / pi * 180; bèta =bèta / pi * 180; gamma =gamma / pi * 180;}/* - trans-site van polaire naar microservo's - wiskundige modelkaart naar feit - de fouten die in eeprom zijn opgeslagen, worden toegevoegd ----------------- -------------------------------------------------- --------*/void polar_to_servo(int leg, float alpha, float beta, float gamma){ if (leg ==0) { alpha =90 - alpha; bèta =bèta; gamma +=90; } else if (been ==1) { alpha +=90; bèta =180 - bèta; gamma =90 - gamma; } else if (been ==2) { alpha +=90; bèta =180 - bèta; gamma =90 - gamma; } else if (been ==3) { alpha =90 - alpha; bèta =bèta; gamma +=90; } servo[been][0].write(alpha); servo[been][1].write(bèta); servo[leg][2].write(gamma);} 

Sluit de led-pinnen aan

• Dat is het, je viervoeter is klaar!
• Upload het programma.
• Sluit de servo aan volgens de pinnen die in het programma zijn gedefinieerd.

Code

• spin
• spider_fix.ino

spinArduino

 /* Inclusief -------------------------------------------- ----------------------*/#include  //om servo's te definiëren en te besturen#include //om een timer om alle servo's te beheren#define ledred 46#define ledblue 44#define ledgreen 45/* Servo's ------------------------------ --------------------------------------*///definieer 12 servo's voor 4 potenServo servo[ 4][3];//definieer de poorten van servo'sconst int servo_pin [4][3] ={{2, 3, 4}, {20, 6, 7}, {8, 9, 17}, {16, 12 , 13} };/* Grootte van de robot --------------------------------------- ------------------*/const float lengte_a =55;const float lengte_b =77,5;const float lengte_c =27,5;const float lengte_side =71;const float z_absolute =-28;/* Constanten voor beweging -------------------------------------------- --------*/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;/* variabelen voor beweging ---------------------- ------------------------------*/vluchtige float site_now[4][3]; // realtime coördinaten van het einde van elke legvolatile float site_expect [4][3]; // verwachte coördinaten van het einde van elke legfloat temp_speed [4][3]; //de snelheid van elke as moet herberekend worden voor elke motionfloat move_speed; // beweging speedfloat speed_multiple =1; // bewegingssnelheid multipleconst float spot_turn_speed =4;const float leg_move_speed =8;const float body_move_speed =3;const float stand_seat_speed =1;vluchtige int rest_counter; //+1/0.02s, voor automatische rust//functie parameterconst float KEEP =255;//define PI voor berekeningconst float pi =3.1415926;/* Constanten voor draaien ------------- -------------------------------------------*///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 voor 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;/* ---------------------------------------- -----------------------------------*//* - instellingsfunctie -------- -------------------------------------------------- -----------------*/void setup(){ pi nMode(ledred,OUTPUT);pinMode(ledblue,OUTPUT);pinMode(ledgroen,OUTPUT); //start serial voor debug Serial.begin (115200); Serial.println("Robot start initialisatie"); // initialiseer standaard 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 gestart"); // initialiseer servo's servo_attach (); Serial.println ("Servo's geïnitialiseerd"); 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]); vertraging (100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); vertraging (100); } }}/* - loop function ---------------------------------------------------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stand(); vertraging (2000); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Step forward"); step_forward(5); vertraging (2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Step back"); step_back(5); vertraging (2000); analogWrite(ledgreen,0); analogWrite(ledred,255); analogWrite(ledblue,255); Serial.println("Turn left"); turn_left(5); vertraging (2000); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Turn right"); turn_right(5); vertraging (2000); analogWrite(ledgreen,255); analogWrite(ledred,255); analogWrite(ledblue,0); Serial.println("Hand wave"); hand_wave(3); vertraging (2000); Serial.println("Hand wave"); hand_shake(3); vertraging (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); } }}

Aangepaste onderdelen en behuizingen

Schema's