Bras robotisé avec Servo-moteurs (5 / 6 étapes)

Étape 5: Branchez-le et écrire du code

Tout d’abord, vous aurez besoin de quelques bibliothèques :
AFMotor - cette bibliothèque a été écrite par Adafruit et rend l’utilisation de la flasque arrière un jeu d’enfant. Lire la documentation associée.
PID_V1 - il s’agit d’une bibliothèque incroyable avec tout aussi étonnant de documentation.

Maintenant, regardez à travers le code et assurez-vous que votre bras robotisé est correctement câblé (faire attention aux broches potentiomètre). J’ai eu une alimentation de benchtop câblée à la flasque arrière donc le bouclier devenait 6V à jusqu'à 3 a, puis l’Arduino a été alimenté seulement via le câble USB.  Les deux fils de chaque moteur sont câblés aux blocs M3 et M4 sur le flasque arrière du moteur.  Les trois fils de potentiomètre de chaque moteur, on va à la terre, l’autre va à alimentation 5V sur l’Arduino, et l’autre est un fil de signal qui est branché sur une entrée analogique sur l’Arduino. Il va être à souder n’importe quelle configuration vous accédez.

Maintenant, copiez et collez ce code commenté assez bien :

/*
Contrôle de bras de robot à l’aide de servos piratés
y est vertical, + x est à droite
par FAB Roberts 2012-05
*/

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

Instancier les deux moteurs
AF_DCMotor shoulder(3) ;
AF_DCMotor elbow(4) ;

déclarer les épingles
int ElbowPin = A1 ;  potentiomètre sur le moteur de coude
int ShoulderPin = A0 ; potentiomètre sur le moteur de l’épaule

INITIALISER DES CONSTANTES
double Kp_Elbow = 20 ;    C’est le gain proportionnel
double Kp_Shoulder = 20 ;
double Ki_Elbow = 0,1 ;    C’est le gain intégral
double Ki_Shoulder = 0,1 ;
double Kd_Elbow = 1.0 ;  C’est le gain dérivé
double Kd_Shoulder = 0,75 ;

double Elbow_neg = 970 ;      limites communes du bras robotique en utilisant la règle de la main droite pour signe
double Elbow_pos = 31 ;
double Shoulder_neg = 210 ;
double Shoulder_pos = 793 ;

const double a1 = 200 ;     longueur de l’épaule-à-coude « OS » (mm)
const double a2 = 220 ;     longueur de coude-à-poignet « OS » (mm) - support c plus long

double highY = 350 ; objectifs de la ligne de démarcation
double lowY = 250 ;
double constantX = 200 ;

elbowup booléen = false ; true = coude vers le haut, false = coude vers le bas

VARIABLES
double rawElbowAngle = 0.0 ;      initialiser tous les angles à 0
double rawShoulderAngle = 0.0 ;

double elbowAngle = 0.0 ;      initialiser tous les angles à 0
double shoulderAngle = 0.0 ;

double theta1 = 0.0 ;  angles de la cible tel que déterminé par le biais de IK
double theta2 = 0.0 ;

double actualX = 0.0 ;
double actualY = 0.0 ;
double y = 0.0 ;

double c2 = 0.0 ; est btwn -1 et 1
double s2 = 0.0 ;

double pwmTemp = 0.0 ;

double pwmElbow = 100.0 ;  entre 0 et 255
double pwmShoulder = 100.0 ;

Syntaxe: PID (& entrée, &, & point de consigne, Kp, Ki, Kd, sortie Direction)
PID elbowPID (elbowAngle, pwmElbow & theta2, Kp_Elbow, Ki_Elbow, Kd_Elbow, DIRECT) ;
PID shoulderPID (shoulderAngle, pwmShoulder & theta1, Kp_Shoulder, Ki_Shoulder, Kd_Shoulder, DIRECT) ;

void setup() {}
Serial.Begin(115200) ;           mettre en place une bibliothèque série

elbowPID.SetSampleTime(5) ;  (ms) combien de fois nouveau PID calc est effectuée, la valeur par défaut est 1000
shoulderPID.SetSampleTime(5) ;

elbow.setSpeed(pwmElbow) ;     Réglez la vitesse sur pwmElbow
shoulder.setSpeed(pwmShoulder) ;     Réglez la vitesse sur pwmElbow

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

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

move_to_start() ;  arriver à la position de départ
}

void loop() {}
line_y() ;
}

void move_to_start() {}
{}
get_angles (constantX, lowY) ;
drive_joints() ;  articulations en voiture jusqu'à ce que le réel est égal à attendre
}
tandis que (abs (theta1 - shoulderAngle) > 2 & & abs (theta2 - elbowAngle) > 2) ; en liberté sous caution lorsqu’elle est étroite
}

void line_y() {}
pour (y = lowY; y < highY; y +=.5) {/ / dessine la ligne droite vers le haut
get_angles(constantX,y) ;
drive_joints() ;
}
pour (y = highY; y > lowY; y-=.5) {/ / Tracez une ligne droite
get_angles (constantX, y) ;
drive_joints() ;
}
}

Compte tenu de theta1, theta2 résoudre pour cible (Px, Py) (cinématique avant)
void get_xy() {}
actualX = a1*cos(radians(theta1)) + a2*cos(radians(theta1+theta2)) ;
actualY = a1*sin(radians(theta1)) + a2*sin(radians(theta1+theta2)) ;
}

Étant donné la cible (Px, Py) à régler pour theta1, theta2 (cinématique inverse)
void get_angles (double Px, Py le double) {}
tout d’abord trouver theta2 où c2 = cos(theta2) et s2 = sin(theta2)
C2 = (pow(Px,2) + pow(Py,2) - pow(a1,2) - pow(a2,2))/(2*a1*a2) ; est btwn -1 et 1

Si (elbowup == false) {}
S2 = sqrt (1 - pow(c2,2)) ;  racine carrée peut être + ou -, et chacun correspond à une orientation différente
}
ElseIf (elbowup == true) {}
S2 = - sqrt (1 - pow(c2,2)) ;
}
theta2 = degrees(atan2(s2,c2)) ;  résout l’angle en degrés et places dans bon quadrant

maintenant trouver theta1 où c1 = cos(theta1) et s1 = sin(theta1)
Theta1 = degrés (atan2 (-a2 * s2 * Px + (a1 + a2 * c2) * Py, (a1 + a2 * c2) * Px + a2 * s2 * Py)) ;
}

void drive_joints() {}
lire les valeurs réelles de tous les pots
rawElbowAngle = analogRead(ElbowPin) ;
rawShoulderAngle = analogRead(ShoulderPin) ;

contraindre le bras robot d’ignorer hors des plages
elbowAngle = contraindre (rawElbowAngle, Elbow_pos, Elbow_neg) ;
shoulderAngle = contraindre (rawShoulderAngle, Shoulder_neg, Shoulder_pos) ;

carte maintenant les angles afin qu’ils correspondent correctement
elbowAngle = carte (elbowAngle, Elbow_neg, Elbow_pos,-110.0, 85,0) ;
shoulderAngle = carte (shoulderAngle, Shoulder_neg, Shoulder_pos, 5,0, 135.0) ;

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

COUDE en voiture: CW est vers l’avant, CCW est descendante
pwmTemp = abs(pwmElbow) ;
elbow.setSpeed(pwmTemp) ;     Utilisez maintenant la nouvelle sortie de PID pour régler la vitesse

Si (pwmElbow < 0) {}
Elbow.Run(Forward) ;      mettre en marche
}
ElseIf (pwmElbow > 0) {}
Elbow.Run(Backward) ;      mettre en marche
}
d’autre elbow.run(RELEASE) ;      arrêté

LECTEUR épaule : CCW est vers l’avant, CW est descendante
pwmTemp = abs(pwmShoulder) ;
shoulder.setSpeed(pwmTemp) ;     régler la vitesse

Si (pwmShoulder < 0) {}
Shoulder.Run(Backward) ;      mettre en marche
}
ElseIf (pwmShoulder > 0) {}
Shoulder.Run(Forward) ;      mettre en marche
}
d’autre shoulder.run(RELEASE) ;      arrêté

get_xy() ;
série d’impression données ici si vous le souhaitez
Serial.Print(actualX) ;
Serial.Print (",") ;
Serial.println(actualY) ;
}