Étape 11 : Code d’Arduino
#include
Servo armServo ; Servo baseServo ;
déclarer des variables pour les broches moteur int motorPin1 = 8 ; DANS 1 int motorPin2 = 9 ; DANS 2 int motorPin3 = 10 ; Dans 3 int motorPin4 = 11 ; 4
motorSpeed int = 6000 ; variable pour définir la vitesse de moteur pas à pas / / expérience avec cela ; trop petit ne fonctionnera pas. int count = 0 ; nombre d’étapes fait int countsperrev = 68 ; nombre de pas par tour pour ce moteur
recherche int [8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001} ;
int motorPos = 0 ; Utilisé pour suivre le moteur
déclarer des variables pour les encodeurs rotatifs int encPins [] = {2, 3, 7, 4, 12, 13} ;
volatile int lastEncoded [] = {0, 0, 0} ; volatils encoderValue long [] = {90, 180, 0} ;
void setup() {baseServo.attach(6) ; armServo.attach(5) ; //declare le moteur épingle comme sorties pinMode (motorPin1, sortie); pinMode (motorPin2, sortie); pinMode (motorPin3, sortie); pinMode (motorPin4, sortie) ; Serial.Begin(9600) ;
déclarant les goupilles de molette pour (int j’ai = 0; j’ai 6 <; i ++) {pinMode (encPins [i], entrée); digitalWrite (encPins [i], HIGH); résistance pullup //turn sur} ;
appel updateEncoder() lorsque tout haut/bas changé vu //on interruption 0 (broche 3), interrompre 1 (axe 2) ou interrompre 4 (broche 7) attachInterrupt (1, update1, changement) ; attachInterrupt (0, MAJ2, changement) ; attachInterrupt (4, update3, changement) ;
}
void loop() {//check premier encodeur rotatif et mise à jour basent servo. si encoderValue (encoderValue [0] < 0) [0] = 0; if (encoderValue [0] > 180) encoderValue [0] = 180 ; baseServo.write(encoderValue[0]) ;
vérifier la deuxième molette et mise à jour de servo de bras. Si encoderValue (encoderValue [1] < 90) [1] = 90 ; Si encoderValue (encoderValue [1] > 270) [1] = 270 ; armServo.write(int(encoderValue[1]/2)) ;
vérifier la dernière encodeur rotatif et décaler le moteur si sa plus faible que la position de la molette. De cette façon elle se déplacera seulement une étape de chaque boucle. Si (motorPos < int(encoderValue[2]/4)) {motorPos ; clockwise();} else if (motorPos > int(encoderValue[2]/4)) {motorPos--; anticlockwise();} //Delay si le retard de la Stepping motor n’est pas utilisé. else {delay(15);}
}
//Set épingles à ULN2003 élevé dans l’ordre de 1 à 4 //delay « motorSpeed » entre chaque broche réglage (pour déterminer la vitesse) / / / Sub anticlockwise() {pour (int i = 0; j’ai < 8; i ++) {setOutput(i) ; delayMicroseconds(motorSpeed);}}
void clockwise() {pour (int i = 7; j’ai > = 0; i--) {setOutput(i) ; delayMicroseconds(motorSpeed);}}
void setOutput(int out) {digitalWrite (motorPin2, bitRead (recherche [out], 1)); digitalWrite (motorPin3, bitRead (recherche [out], 2)), digitalWrite (motorPin1, bitRead (recherche [out], 0)); digitalWrite (motorPin4, bitRead (recherche [out], 3));}
//Rotary encodeur interrompre fonctions / / /
void update1() {updateEncoder(0);}
void update2() {updateEncoder(1);}
void update3() {updateEncoder(2);}
void updateEncoder (int p) {int MSB = digitalRead(encPins[p]) ; //MSB = plus important int bit LSB = digitalRead(encPins[p+3]) ; //LSB = bit le moins significatif
if(MSB == LSB) {encoderValue [p] ++;} else {encoderValue [p]--;}
}
