Robotarm met servomotoren (5 / 6 stap)

Stap 5: Stekker in het stopcontact en schrijf wat code

Nu, kijk door de code en zorg ervoor dat uw robotarm is correct aangesloten (aandacht besteden aan de potentiometer pinnen). Ik had een benchtop voeding wired aan de motor shield dus het schild kreeg 6V op tot 3A, dan de Arduino gewoon werd aangedreven door de USB-kabel.  De twee draden van elke motor zijn aangesloten op de M3 en M4 blokken op de motor schild.  Van de drie draden van de potentiometer van elke motor, gaat men op grond, de andere gaat aan de macht van de 5V op de Arduino en de andere is een signaal kabel die wordt aangesloten op een analoge ingang op de Arduino. Er zal worden solderen geen kwestie wat u gaan met setup.

Nu, kopieer en plak deze vrij goed becommentarieerde code:

/*
Robot arm controle met behulp van gehackte servos
+ y is verticale, + x bevindt zich rechts
door Dustyn Roberts 2012-05
*/

#include < AFMotor.h >
#include < math.h >
#include < PID_v1.h >

Instantiëren van beide motoren
AF_DCMotor shoulder(3);
AF_DCMotor elbow(4);

verklaar pinnen
int ElbowPin = a1 worden verkregen;  op potmeter op elleboog motor
int ShoulderPin = A0; op potmeter op schouder motor

INITIALISEREN VAN CONSTANTEN
dubbele Kp_Elbow = 20;    Dit is de proportionele winst
dubbele Kp_Shoulder = 20;
dubbele Ki_Elbow = 0,1;    Dit is de integrale winst
dubbele Ki_Shoulder = 0,1;
dubbele Kd_Elbow = 1,0;  Dit is de afgeleide winst
dubbele Kd_Shoulder = 0,75;

dubbele Elbow_neg = 970;      gezamenlijke grenzen van robotarm met behulp van de regel van de rechterhand voor teken
dubbele Elbow_pos = 31;
dubbele Shoulder_neg = 210;
dubbele Shoulder_pos = 793;

Const dubbele a1 = 200;     schouder-aan-elleboog "bot" lengte (mm)
Const dubbele a2 = 220;     elleboog-tot-pols "bot" lengte (mm) - langer c beugel

dubbele highY = 350; lijntekening doelstellingen
dubbele Lowy. = 250;
dubbele constantX = 200;

Boole elbowup = false; True = elleboog omhoog, false = elleboog naar beneden

VARIABELEN
dubbele rawElbowAngle = 0,0;      initialiseren van alle hoeken op 0
dubbele rawShoulderAngle = 0,0;

dubbele elbowAngle = 0,0;      initialiseren van alle hoeken op 0
dubbele shoulderAngle = 0,0;

dubbele theta1 = 0,0;  doel hoeken zoals bepaald door IK
dubbele theta2 = 0,0;

dubbele actualX = 0,0;
dubbele daadwerkelijk = 0,0;
dubbele y = 0,0;

dubbele c2 = 0,0; btwn -1 en 1 is
dubbele s2 = 0,0;

dubbele pwmTemp = 0,0;

dubbele pwmElbow = 100.0;  tussen 0 en 255
dubbele pwmShoulder = 100.0;

Syntaxis: PID (& Input, & Output, & Setpoint Kp, Ki, Kd, richting)
PID elbowPID (elbowAngle, pwmElbow, & theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, directe);
PID shoulderPID (shoulderAngle, pwmShoulder, & theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, directe);

VOID Setup {}
Serial.begin(115200);           Seriële bibliotheek heeft ingesteld

elbowPID.SetSampleTime(5);  (ms) hoe vaak nieuwe PID calc wordt uitgevoerd, de standaardwaarde is 1000
shoulderPID.SetSampleTime(5);

elbow.setSpeed(pwmElbow);     Zet de snelheid naar pwmElbow
shoulder.setSpeed(pwmShoulder);     Zet de snelheid naar pwmElbow

elbowPID.SetMode(AUTOMATIC);
shoulderPID.SetMode(AUTOMATIC);

elbowPID.SetOutputLimits(-255,255);
shoulderPID.SetOutputLimits(-255,255);

move_to_start();  krijgen tot de uitgangspositie
}

void loop {}
line_y();
}

ongeldig move_to_start() {}
{}
get_angles (constantX, Lowy.);
drive_joints();  Station gewrichten tot werkelijke gelijk is aan verwacht
}
terwijl (abs (theta1 - shoulderAngle) > 2 & & abs (theta2 - elbowAngle) > 2); borgtocht wanneer het is dicht
}

ongeldig line_y() {}
voor (y = lowY; y < highY; y +=.5) {/ / rechte lijn opstellen
get_angles(constantX,y);
drive_joints();
}
voor (y = highY; y > lowY; y-=.5) {/ / rechte lijn tekenen naar beneden
get_angles (constantX, y);
drive_joints();
}
}

Gezien de theta1, theta2 op te lossen voor doel (Px, Py) (forward kinematica)
ongeldig get_xy() {}
actualX = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
Daadwerkelijk = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
}

Gegeven doel (Px, Py) op te lossen voor theta1, theta2 (inverse kinematics)
VOID get_angles (Px dubbel, verdubbelen van Py) {}
eerst vinden theta2 waar c2 = cos(theta2) en s2 = sin(theta2)
C2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2); btwn -1 en 1 is

Als (elbowup == false) {}
S2 = sqrt (1 - pow(c2,2));  SQRT kunnen + of -, en elk komt overeen met een andere afdrukstand
}
else if (elbowup == true) {}
S2 = - sqrt (1 - pow(c2,2));
}
theta2 = degrees(atan2(s2,c2));  Lost voor de hoek in graden en plaatsen in het juiste kwadrant

nu vinden theta1 waar c1 = cos(theta1) en s1 = sin(theta1)
theta1 = graden (atan2 (-a2 * s2 * Px + (a1 + a2 * c2) * Py, (a1 + a2 * c2) * Px + a2 * s2 * Py));
}

ongeldig drive_joints() {}
de werkelijke waarden lezen van alle de potten
rawElbowAngle = analogRead(ElbowPin);
rawShoulderAngle = analogRead(ShoulderPin);

beperken van de robotarm om te negeren uit bereikwaarden
elbowAngle = beperken (rawElbowAngle, Elbow_pos, Elbow_neg);
shoulderAngle = beperken (rawShoulderAngle, Shoulder_neg, Shoulder_pos);

nu kaart de hoeken zodat ze overeenstemmen
elbowAngle = kaart (elbowAngle, Elbow_neg, Elbow_pos,-110.0, 85.0);
shoulderAngle = kaart (shoulderAngle, Shoulder_neg, Shoulder_pos, 5.0, 135.0);

«««elbowPID.Compute();
«««shoulderPID.Compute();

Station ELLEBOOG: CW vooruit, CCW is neerwaarts
pwmTemp = abs(pwmElbow);
elbow.setSpeed(pwmTemp);     nu de nieuwe PID output kunt de snelheid instellen

Als (pwmElbow < 0) {}
Elbow.run(forward);      zet hem op
}
anders als (pwmElbow > 0) {}
Elbow.run(BACKWARD);      zet hem op
}
anders elbow.run(RELEASE);      gestopt

Station schouder: CCW vooruit, CW is neerwaarts
pwmTemp = abs(pwmShoulder);
shoulder.setSpeed(pwmTemp);     de snelheid instellen

Als (pwmShoulder < 0) {}
Shoulder.run(BACKWARD);      zet hem op
}
anders als (pwmShoulder > 0) {}
Shoulder.run(forward);      zet hem op
}
anders shoulder.run(RELEASE);      gestopt

get_xy();
seriële gegevens hier desgewenst afdrukken
Serial.Print(actualX);
Serial.Print (",");
Serial.println(actualY);
}