Robotarm med servomotorer (5 / 6 steg)

Steg 5: Koppla in den och skriva någon kod

Nu, titta igenom koden och se till att din robotarm kopplats korrekt (betala uppmärksamhet till potentiometern stift). Jag hade en bänkmonterade strömförsörjning trådbunden till motor skölden så skölden fick 6V på upp till 3A, då Arduino var bara drivs genom USB-kabeln. De två kablarna från varje motor är anslutna till M3 och M4 block på motor skölden. Av tre potentiometer trådarna från varje motor, en går till marken, den andra går till 5V power på Arduino och den andra är en signal som blir ansluten till en analog ingång på Arduino. Det kommer att lödning oavsett vilken inställning du går med.

Nu, kopiera och klistra in denna ganska väl kommenterade kod:

/*
Robot arm kontroll med hackade servon
+ y vertikala, + x till höger
av Dustyn Roberts 2012-05
*/

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

Initiera båda motorer
AF_DCMotor shoulder(3);
AF_DCMotor elbow(4);

deklarera pins
int ElbowPin = A1; att potentiometer på armbågen motor
int ShoulderPin = A0; att potentiometer på axeln motor

INITIERA KONSTANTER
Double Kp_Elbow = 20; Detta är proportionella förstärkningen
Double Kp_Shoulder = 20;
Double Ki_Elbow = 0,1; Detta är den integrerad vinsten
Double Ki_Shoulder = 0,1;
Double Kd_Elbow = 1,0; Detta är den härledda vinsten
Double Kd_Shoulder = 0,75;

Double Elbow_neg = 970; gemensamma gränser av robotarm med höger regel för tecken
Double Elbow_pos = 31;
Double Shoulder_neg = 210;
Double Shoulder_pos = 793;

CONST double a1 = 200. axeln till armbåge "bone" längd (mm)
CONST double a2 = 220; armbåge-till-handled "bone" längd (mm) - längre c fäste

Double highY = 350; linjeritning mål
dubbel lowY = 250;
Double constantX = 200.

booleska elbowup = false; sant = armbåge upp, false = armbåge ned

VARIABLER
Double rawElbowAngle = 0.0; initiera alla vinklar 0
Double rawShoulderAngle = 0.0;

Double elbowAngle = 0.0; initiera alla vinklar 0
Double shoulderAngle = 0.0;

Double theta1 = 0.0; Target vinklar som bestäms genom IK
Double theta2 = 0.0;

Double actualX = 0.0;
dubbel faktiskt = 0,0;
dubbla y = 0,0;

dubbel c2 = 0,0; är btwn -1 och 1
dubbla s2 = 0,0;

Double pwmTemp = 0.0;

Double pwmElbow = 100,0; mellan 0 och 255
Double pwmShoulder = 100,0;

Syntax: PID (& Input, & utgång, & Setpoint Kp, Ki, Kd, riktning)
PID elbowPID (& elbowAngle, och pwmElbow, och theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, direkt);
PID shoulderPID (& shoulderAngle, och pwmShoulder, och theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, direkt);

void setup() {
Serial.BEGIN(115200); Ställ in följetong bibliotek

elbowPID.SetSampleTime(5); (ms) hur ofta nya PID calc görs, standard är 1000
shoulderPID.SetSampleTime(5);

elbow.setSpeed(pwmElbow); som hastigheten till pwmElbow
shoulder.setSpeed(pwmShoulder); som hastigheten till pwmElbow

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

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

move_to_start(); komma till startposition
}

void loop() {
line_y();
}

void move_to_start() {
göra {
get_angles (constantX, lowY);
drive_joints(); enheten leder till dess faktiska är lika med förväntat
}
medan (abs (theta1 - shoulderAngle) > 2 & & abs (theta2 - elbowAngle) > 2); borgen när det är nära
}

void line_y() {
för (y = lowY; y < highY; y +=.5) {/ / dra rak linje
get_angles(constantX,y);
drive_joints();
}
för (y = highY, y > lowY; y-=.5) {/ / dra rak linje
get_angles (constantX, y);
drive_joints();
}
}

Med tanke på theta1, theta2 lösa för mål (Px, Py) (framåt kinematik)
void get_xy() {
actualX = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2));
Faktiskt = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2));
}

Med tanke på målet (Px, Py) lösa för theta1, theta2 (omvänd kinematik)
void get_angles (dubbel Px, dubbel Py) {
först hitta theta2 där c2 = cos(theta2) och s2 = sin(theta2)
C2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2); är btwn -1 och 1

om (elbowup == false) {
S2 = sqrt (1 - pow(c2,2)); rot kan vara + eller -, och var och en motsvarar till en annan orientering
}
annat if (elbowup == true) {
S2 = - sqrt (1 - pow(c2,2));
}
theta2 = degrees(atan2(s2,c2)); löser för vinkeln i grader och platser i rätt kvadranten

nu hitta theta1 där c1 = cos(theta1) och s1 = sin(theta1)
theta1 = grader (ARCTAN2 (-a2 * s2 * Px + (a1 + a2 * c2) * Py, (a1 + a2 * c2) * Px + a2 * s2 * Py));
}

void drive_joints() {
läsa de faktiska värdena från alla krukor
rawElbowAngle = analogRead(ElbowPin);
rawShoulderAngle = analogRead(ShoulderPin);

begränsa robotarm att ignorera av intervallvärden
elbowAngle = begränsa (rawElbowAngle, Elbow_pos, Elbow_neg);
shoulderAngle = begränsa (rawShoulderAngle, Shoulder_neg, Shoulder_pos);

nu karta vinklarna så att de motsvarar korrekt
elbowAngle = karta (elbowAngle, Elbow_neg, Elbow_pos,-110.0, 85,0);
shoulderAngle = karta (shoulderAngle, Shoulder_neg, Shoulder_pos, 5.0, 135.0);

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

ENHETEN ARMBÅGAR: CW är framåt, CCW är bakåt
pwmTemp = abs(pwmElbow);
elbow.setSpeed(pwmTemp); nu Använd de nya PID utdata ställa in hastigheten

om (pwmElbow < 0) {
Elbow.Run(forward); slå på den
}
annars om (pwmElbow > 0) {
Elbow.Run(Backward); slå på den
}
annat elbow.run(RELEASE); stoppad

DRIVE axel: CCW är framåt, CW är bakåt
pwmTemp = abs(pwmShoulder);
shoulder.setSpeed(pwmTemp); ställa in hastigheten

om (pwmShoulder < 0) {
Shoulder.Run(Backward); slå på den
}
annars om (pwmShoulder > 0) {
Shoulder.Run(forward); slå på den
}
annat shoulder.run(RELEASE); stoppad

get_xy();
seriella skriva ut data här om så önskas
Serial.Print(actualX);
Serial.Print (",");
Serial.println(actualY);
}