Arduino-Vierbeiner

Notwendige Werkzeuge und Maschinen

Lötkolben (generisch)

Apps und Onlinedienste

Arduino-IDE

Über dieses Projekt

Arduino-basierter Vierbeiner! Quadruped steht für vierbeiniger Bot, der im Grunde wie eine vierbeinige Spinne aussieht. Lassen Sie uns also lernen, wie die Spinne läuft und versuchen, sie mit Arduino zu replizieren.

Zubehör:

Schritt 1:Erforderliche Komponenten

• 1 X Arduino Mega oder Arduino Uno
• 1 X gebohrte Leiterplatte
• 12 X Servomotoren (9g)
• 1 X HC-SR04 Ultraschallsensor
• 4 X RGB-LED
• Karton

Schritt 2:CG beibehalten

Der Schwerpunkt (CG) ist der Hauptfaktor beim Gehen. Der Schwerpunkt bleibt in der Körpermitte, um das Gleichgewicht zu halten Wenn sich der Schwerpunkt an bestimmten Grenzen aus der Mitte bewegt, wird das Gleichgewicht beeinträchtigt und führt zum Herunterfallen

Sehen wir uns also an, wie man den CG beim Gehen aufrecht erhält.

Wenn jedes Bein in einem 45-Grad-Winkel ist, ist der Schwerpunkt perfekt in der Mitte. Wenn wir jedoch ein Bein bewegen, verschiebt sich der Schwerpunkt auf diese Seite, sodass es zu einem Sturz auf diese Seite kommt.

Um dies zu vermeiden, werden die Beine an beiden Enden basierend auf der Bot-Größe in einem Winkel von mehr als 45 Grad gehalten, sodass die drei Beine ein Dreieck bilden, in dem sich der Schwerpunkt darin befindet und der vierte Bein frei beweglich ist und der Schwerpunkt CG bleibt in einem Dreieck.

Schritt 3:Gehverfahren

• Dies ist die Ausgangsposition, mit zwei Beinen (C, D) auf einer Seite ausgestreckt und den anderen beiden Beinen (A, B) nach innen gezogen.
• Das rechte obere Bein (B) hebt sich und streckt sich weit vor dem Roboter aus.
• Alle Beine verschieben sich nach hinten und bewegen den Körper nach vorne.
• Das hintere linke Bein (D) hebt und tritt neben dem Körper nach vorne. Diese Position ist das Spiegelbild der Ausgangsposition.
• Das obere linke Bein (B) hebt und streckt sich weit vor dem Roboter aus.
• Wieder verschieben sich alle Beine nach hinten und bewegen den Körper nach vorne.
• Das hintere rechte Bein hebt (B) und tritt zurück in den Körper, was uns in die Ausgangsposition zurückbringt.

Schritt 4:Pläne für Vierbeiner

Schritt 5:Aufbau der Karosserie

Konstruieren Sie den Körper nach PDF.

Schritt 6:Stromkreisverbindung

Stellen Sie Ihr eigenes Schild nach Ihren Anforderungen her Arduino Mega hat 15 PWM-Pins, verwenden Sie 12 davon für Servoverbindungen und 3 für RBG-LED und zwei beliebige Pins für Ultraschallsensor

Schritt 7:Initialisierung des Servos

• Laden Sie das Programm auf Arduino Mega hoch und beginnen Sie mit der Montage des Beins gemäß Bild

#include  Servo Servo[4][3]; // Definiere die Ports der Servosconst int servo_pin[4][3] ={ {10,11,2}, {3,4 ,5}, {6,7,8}, {9, 12, 13} };void setup () {// alle Servos initialisieren für (int i =0; i <4; i++) { for (int j =0; j <3; j++) {servo[i][j].attach(servo_pin[i][j]); Verzögerung (20); aufrechtzuerhalten. Verzögerung (20); } }} 

Schritt 8:Letzter Schritt

 /* Enthält ------------------------------------------ -------------------------*/#include  //um Servos zu definieren und zu steuern#include // um einen Timer zu setzen um alle Servos zu verwalten#define ledred 46#define ledblue 44#define ledgreen 45/* Servos -------------------------- ------------------------------------------* /// 12 Servos für 4 . definieren legServo servo[4][3]; // Definiere die Ports der Servosconst int servo_pin[4][3] ={ {2, 3, 4}, {20, 6, 7}, {8, 9, 17}, { 16, 12, 13} };/* Größe des Roboters ------------------------------------ ---------------------*/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;/* Konstanten für die Bewegung ------------------------------------------ -----------*/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;/* Variablen für Bewegung ---------------- ------------------------------------*/volatile float site_now[4][3]; //Echtzeitkoordinaten des Endes jedes legvolatile float site_expect[4][3]; // erwartete Koordinaten des Endes jedes Legfloat temp_speed[4][3]; // Die Geschwindigkeit jeder Achse muss vor jeder Bewegung neu berechnet werden float move_speed; // Bewegungsgeschwindigkeitfloat speed_multiple =1; // Bewegungsgeschwindigkeit 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, für automatische Pause//Funktionsparameterconst float KEEP =255;//define PI für Berechnungconst float pi =3.1415926;/* Konstanten für Drehung ------------- -------------------------------------------------------* ///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 für 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-Funktion -------- -------------------------------------------------- -----------------*/void setup(){ weiter nMode(ledred,OUTPUT);pinMode(ledblue,OUTPUT);pinMode(ledgreen,OUTPUT); // Serial zum Debuggen starten Serial.begin (115200); Serial.println ( "Roboter startet Initialisierung"); //Standardparameter initialisieren 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]; } } // Servo-Dienst starten FlexiTimer2::set (20, servo_service); FlexiTimer2::start(); Serial.println ( "Servodienst gestartet"); // Servos initialisieren servo_attach (); Serial.println ( "Servos initialisiert"); Serial.println ("Roboter-Initialisierung abgeschlossen");}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]); Verzögerung (100); }}}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) {servo[i][j].detach(); Verzögerung (100); } }}/* - Schleifenfunktion ------------------------------------------ ---------------------------------*/void loop () { analogWrite (ledred, 255); Serial.println ("Stand"); Stand(); Verzögerung (2000); analogWrite(ledred,0); analogWrite (ledblau,255); Serial.println ( "Schritt vorwärts"); step_forward(5); Verzögerung (2000); analogWrite (ledblau, 0); analogWrite (ledgrün,255); Serial.println ("Schritt zurück"); step_back(5); Verzögerung (2000); analogWrite (ledgrün, 0); analogWrite (ledred,255); analogWrite (ledblau,255); Serial.println ("Links abbiegen"); turn_left(5); Verzögerung (2000); analogWrite (ledgrün,255); analogWrite(ledred,0); analogWrite (ledblau,255); Serial.println ( "Rechts abbiegen"); turn_right(5); Verzögerung (2000); analogWrite (ledgrün,255); analogWrite (ledred,255); analogWrite (ledblau, 0); Serial.println ("Handwelle"); hand_wave(3); Verzögerung (2000); Serial.println ("Handwelle"); hand_shake(3); Verzögerung (2000); int x =100; for(int i=0;i<5;i++) {analogWrite(ledgreen,255); analogWrite (ledred,255); // weiß analogWrite (ledblue,255); Verzögerung(x); analogWrite (ledgreen,255); // gelb analogWrite (ledred,255); analogWrite (ledblau, 0); Verzögerung(x); analogWrite (ledgreen,255); //cyan analogWrite (ledred,0); analogWrite (ledblau,255); Verzögerung(x); analogWrite (ledgrün, 0); analogWrite (ledred,255); // lila analogWrite (ledblue,255); Verzögerung(x); analogWrite (ledgrün, 0); analogWrite (ledred,255); // rot analogWrite (ledblue,0); Verzögerung(x); analogWrite (ledgreen, 0); // blau analogWrite (ledred, 0); analogWrite (ledblau,255); Verzögerung(x); analogWrite (ledgrün,255); analogWrite(ledred,0); analogWrite (ledblau, 0); // grüne Verzögerung (x); } analogWrite (ledgrün, 0); analogWrite(ledred,0); analogWrite (ledblau, 0); //Serial.println("Körpertanz"); // body_dance (10); // Verzögerung (2000); //Serial.println("Sitz"); // sit(); delay(1000);}/* - sitzen - Sperrfunktion -------------------------------------- --------------------------------------*/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 - Sperrfunktion ------------------------------------------------- --------------------------------------*/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 links abbiegen - Blockierfunktion - Parameterschritt Schritte wollte abbiegen -------------------------- ------------------------------------------------*/ 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(); aufrechtzuerhalten. } else {//leg 0&2 set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_up); set_site(1, turn_x1 + x_offset, turn_y1, z_default); set_site(2, turn_x0 - x_offset, turn_y0, z_default); set_site(3, turn_x1 - x_offset, turn_y1, z_default); wait_all_reach(); set_site(0, turn_x0 + x_offset, turn_y0, z_default); wait_all_reach(); set_site(0, turn_x0 - x_offset, turn_y0, z_default); set_site(1, turn_x1 - x_offset, turn_y1, z_default); set_site(2, turn_x0 + x_offset, turn_y0, z_default); set_site(3, turn_x1 + x_offset, turn_y1, z_default); wait_all_reach(); set_site(2, turn_x0 + x_offset, turn_y0, z_up); wait_all_reach(); set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_up); set_site(3, x_default + x_offset, y_start, z_default); wait_all_reach(); set_site(2, x_default + x_offset, y_start, z_default); wait_all_reach(); } }}/* - Spot rechts abbiegen - Sperrfunktion - Parameterschritt Schritte wollte abbiegen ------------------------------------------ ---------------------------------------------------------*/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(); aufrechtzuerhalten. aufrechtzuerhalten. Else { // Leg 1 &3 verschieben 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(); } }}/* - vorwärts gehen - Funktion blockieren - Parameterschritt Schritte wollten gehen -------------------------------- -------------------------------------------------------*/void step_forward(unsigned int Schritt) {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(); Bewegungsgeschwindigkeit =Beinbewegungsgeschwindigkeit; 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(); aufrechtzuerhalten. Else {//leg 0&3 set_site(0, x_default + x_offset, y_start, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); wait_all_reach(); set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); move_speed =body_move_speed; set_site(0, x_default - x_offset, y_start + y_step, z_default); set_site(1, x_default - x_offset, y_start + y_step, z_default); set_site(2, x_default + x_offset, y_start, z_default); set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); wait_all_reach(); Bewegungsgeschwindigkeit =Beinbewegungsgeschwindigkeit; 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(); } }}/* - zurück - Sperrfunktion - Parameterschritt Schritte wollte gehen -------------------------------- -------------------------------------------------------*/void step_back(unsigned int Schritt) {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(); Bewegungsgeschwindigkeit =Beinbewegungsgeschwindigkeit; 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(); aufrechtzuerhalten. aufrechtzuerhalten. Else { // Leg 1 &2 verschieben 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(); Bewegungsgeschwindigkeit =Beinbewegungsgeschwindigkeit; 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(); } }} // von RegisHsuvoid hinzufügen body_left (int i) { set_site (0, site_now [0][0] + i, KEEP, KEEP); set_site(1, site_now[1][0] + i, BEHALTEN, BEHALTEN); set_site(2, site_now[2][0] – i, BEHALTEN, BEHALTEN); set_site(3, site_now[3][0] – i, BEHALTEN, BEHALTEN); 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, BEHALTEN, BEHALTEN); set_site(2, site_now[2][0] + i, BEHALTEN, BEHALTEN); set_site(3, site_now[3][0] + i, BEHALTEN, BEHALTEN); wait_all_reach();}void hand_wave(int i){ float x_tmp; float y_tmp; float z_tmp; Bewegungsgeschwindigkeit =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-Dienst / Timer-Interrupt-Funktion / 50 Hz - Wenn die Site erwartet wird, verschiebt diese Funktion den Endpunkt in einer geraden Linie dorthin - temp_speed[4][3] sollte vor dem Setzen der erwarteten Site eingestellt werden Stellen Sie sicher, dass sich der Endpunkt in einer geraden Linie bewegt, und bestimmen Sie die Bewegungsgeschwindigkeit. -------------------------------------------------- --------------------------*/void servo_service (void) { sei(); statische Gleitkommazahl 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++;}/* - setze einen der Endpunkte der erwarteten Site - diese Funktion setzt gleichzeitig temp_speed[4][3] - nicht blockierende Funktion -------------- -------------------------------------------------- ----------*/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-Länge =sqrt(pow(Länge_x, 2) + pow(Länge_y, 2) + pow(Länge_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;}/* - warten, dass sich einer der Endpunkte zum erwarteten Standort bewegt - Blockierfunktion ----------------- -------------------------------------------------- --------*/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;}/* - warte, dass sich alle Endpunkte bewegen, um die Site zu erwarten - Blockierfunktion ---- -------------------------------------------------- ---------------------*/void wait_all_reach(void){ for (int i =0; i <4; i++) wait_reach(i);}/* - Transsite von Kartesisch zu Polar - Mathematisches Modell 2/2 ------------------------------------------------- --------------------------------------*/void cartesian_to_polar(volatile float &alpha, volatile float &beta , volatile float &gamma, volatile float x, volatile float y, volatile float z) {// berechnen wz Grad float v, w; w =(x>=0 ? 1 :-1) * (sqrt(pow(x, 2) + pow(y, 2))); v =w - Länge_c; alpha =atan2(z, v) + acos((pow(Länge_a, 2) - pow(Länge_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / Länge_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); // x-y-z-Grad-Gamma berechnen =(w>=0) ? atan2(y, x):atan2(-y, -x); // trans Grad pi -> 180 alpha =alpha / pi * 180; Beta =Beta / Pi * 180; gamma =gamma / pi * 180;}/* - Trans-Site von Polar zu Mikroservos - Mathematische Modellkarte zu Tatsache - die im eeprom gespeicherten Fehler werden hinzugefügt ----------------- -------------------------------------------------- --------*/void polar_to_servo (int leg, float alpha, float beta, float gamma) { if (leg ==0) { alpha =90 - alpha; Beta =Beta; Gamma +=90; aufrechtzuerhalten. Sonst wenn (Bein ==1) {Alpha +=90; Beta =180 - Beta; Gamma =90 - Gamma; aufrechtzuerhalten. Sonst wenn (Bein ==2) {Alpha +=90; Beta =180 - Beta; Gamma =90 - Gamma; aufrechtzuerhalten. Sonst wenn (Bein ==3) {Alpha =90 - Alpha; Beta =Beta; Gamma +=90; } servo[Bein][0].write(alpha); servo[Bein][1].write(beta); servo[bein][2].write(gamma);} 

Verbinden Sie die LED-Pins

• Das war's, dein Vierbeiner ist fertig!
• Laden Sie das Programm hoch.
• Verbinden Sie das Servo gemäß den im Programm definierten Pins.

Code

• Spinne
• spider_fix.ino

SpinneArduino

 /* Enthält ------------------------------------------ ----------------------*/#include  // zum Definieren und Steuern von Servos#include  // zum Einstellen von a Timer um alle Servos zu verwalten#define ledred 46#define ledblue 44#define ledgreen 45/* Servos ------------------------------------------ --------------------------------------* /// 12 Servos für 4 Beine definieren Servoservo[ 4][3]; // Definiere die Ports der Servosconst int servo_pin[4][3] ={ {2, 3, 4}, {20, 6, 7}, {8, 9, 17}, {16, 12 , 13} };/* Größe des Roboters -------------------------------------- ------------------*/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;/* Konstanten für die Bewegung -------------------------------------------------------- --------*/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;/* Variablen für Bewegung ---------------------- ------------------------------------------*/volatile float site_now[4][3]; //Echtzeitkoordinaten des Endes jedes legvolatile float site_expect[4][3]; // erwartete Koordinaten des Endes jedes Legfloat temp_speed[4][3]; // Die Geschwindigkeit jeder Achse muss vor jeder Bewegung neu berechnet werden float move_speed; // Bewegungsgeschwindigkeitfloat speed_multiple =1; // Bewegungsgeschwindigkeit 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, für automatische Pause//Funktionsparameterconst float KEEP =255;//Define PI für Berechnungconst float pi =3.1415926;/* Konstanten für Drehung ------------- -------------------------------------------------------* ///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 für 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-Funktion -------- -------------------------------------------------- -----------------*/void setup(){ weiter nMode(ledred,OUTPUT);pinMode(ledblue,OUTPUT);pinMode(ledgreen,OUTPUT); // Serial zum Debuggen starten Serial.begin (115200); Serial.println ( "Roboter startet Initialisierung"); //Standardparameter initialisieren 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]; } } // Servo-Dienst starten 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]); Verzögerung (100); } }}void servo_detach(void){ for (int i =0; i <4; i++) { for (int j =0; j <3; j++) { servo[i][j].detach(); Verzögerung (100); } }}/* - loop function ---------------------------------------------------------------------------*/void loop(){ analogWrite(ledred,255); Serial.println("Stand"); stand(); delay(2000); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Step forward"); step_forward(5); delay(2000); analogWrite(ledblue,0); analogWrite(ledgreen,255); Serial.println("Step back"); step_back(5); delay(2000); analogWrite(ledgreen,0); analogWrite(ledred,255); analogWrite(ledblue,255); Serial.println("Turn left"); turn_left(5); delay(2000); analogWrite(ledgreen,255); analogWrite(ledred,0); analogWrite(ledblue,255); Serial.println("Turn right"); turn_right(5); delay(2000); analogWrite(ledgreen,255); analogWrite(ledred,255); analogWrite(ledblue,0); Serial.println("Hand wave"); hand_wave(3); delay(2000); Serial.println("Hand wave"); hand_shake(3); delay(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); } }}

Kundenspezifische Teile und Gehäuse

Schaltpläne