Étape 5: Code Arduino - robot fonctionne
Script de ligne de suiveur robot opération
Bouba
***/
/ * sortie de moteurs broches * /
const int M. = 6 ; Ce moteur broche avant
const int ML = 5 ; Mâle vers l’avant moteur gauche
/ * entrée sonde broches * /
const int [] sensorPins = {A0, A1, A2} ; droite, Centre, gauche
/ * autres broches d’entrée et de sortie * /
const int ledPin = 13 ; conduit indique si le mode de recherche est sur
const int potPin = A5 ; pour le réglage gauche droite erreur
/ * baies pour le stockage de capteur et calculée données * /
int sensorReadings [3] ; lectures du capteur à droite, Centre, gauche
flotteur sensorValues [3] ; lectures du capteur à droite, Centre, gauche
int potAdjust = 0 ; solde de capteur dont la valeur du potentiomètre de réglage
/ * compteurs, aides etc. * /
int i ; compteur General en boucles for
int unknownCount = 0 ; compteur des États de ligne-inconnu successifs
s int ; valeur de PWM pour Vitesse
/ *** PARAMÈTRES *** /
const int turnThreshold = 15 ; Quelle différence entre gauche et droite sensorreadings commencer lisse tournant (1000 unités échelle formulaire blanc au noir)
const int quickturnThreshold = 20 ; Quelle différence entre gauche et droite sensorreadings commence raide virage
const int indicateThreshold = 2 ; sur quelle lecture pour examiner la ligne connu
const int moveLength = 55 ; Combien de temps un mouvement prenne après vitesse a été acquise (dépend de la suite à trois variables)
et avant le nouveau contrôle de l’état du capteur doit être appelée (en millisecondes)
temps nécessaire pour gagner de la vitesse = [(maxSpeed-startSpeed) / speedStep] (dans les millis) actuel: 9
const int maxSpeed = 160 ; Max valeur PWM pour moteurs
pour mieux partir, nous augmentons la valeur PWM dans le temps :
const int startSpeed = 80 ; PWM pour commencer, 1/2 max par exemple
const int speedStep = 10 ; combien les valeurs PWM à augmenter après chaque 1 milliseconde
const int pauseLength = 140 ; faire une pause entre les mouvements pour une meilleure lecture de capteur
int unknownMax = 25 ; après combien mesures successifs « ligne inconnue » de commencer à rechercher la ligne de routine
/ * CAPTEUR de CORRECTIONS pour le point blanc et « pas d’unité »: droite, Centre, gauche
COPIEZ CES VALEURS DE CALIBRATION PROGRAMME SÉRIE SORTIE * /
const int corrections [3] = {38,38,31} ;
const char unitSteps [3] = {0.06,0.01,0.02} ;
const char betweenReadings [2] = {219.30,233.33} ;
/**/
void setup() {}
/ * moteur broches * /
pinMode (MR, sortie) ;
pinMode (ML, sortie) ;
/ * capteur broches * /
pinMode (sensorPins [0], entrée) ;
pinMode (sensorPins [1], d’entrée) ;
pinMode (sensorPins [2], entrée) ;
pinMode (potPin, entrée) ;
} / / / installation
void loop() {}
readSensors() ;
If
(
sensorValues [0] + sensorValues [1] < 0,1 * betweenReadings [0] ||
sensorValues [1] + sensorValues [2] < 0,1 * betweenReadings [1] ||
sensorValues [0] < indicateThreshold ||
sensorValues [1] < indicateThreshold ||
sensorValues [2] < indicateThreshold
) {
unknownCount ++ ;
}
else {}
unknownCount = 0 ;
}
Si (unknownCount < = unknownMax) {}
rideLine() ;
}
else {}
searchLine() ;
}
} / / / loop
void readSensors() {//sets tableau sensorValues [] aux lectures du capteur avec corrections calculées
pour (i = 0; i < = 2; i ++) {}
sensorReadings [i] = analogRead(sensorPins[i]) ;
sensorValues [i] = (sensorReadings [i] - corrections[i]) / unitSteps [i] ;
}
potAdjust = (analogRead(potPin) - 45) * 10 ;
} / / /readSensors
void goStraight() {}
Augmentez la vitesse progressivement
pour (s = startSpeed; s < = maxSpeed; s = s + speedStep) {}
analogWrite (MR, s) ;
analogWrite (ML, s) ;
Delay(1) ;
}
pleine vitesse
analogWrite (MR, s) ;
analogWrite (ML, s) ;
Delay(moveLength) ;
digitalWrite (MR, faible) ;
digitalWrite (ML, faible) ;
arrêter
Delay(pauseLength) ;
}
void goLeft (int turnspeed) {//turnspeed = 0 -> rotation lisse, sinon laisser infuser tournant
digitalWrite (ML, faible) ;
Augmentez la vitesse progressivement
pour (s = startSpeed; s < = maxSpeed; s = s + speedStep) {}
analogWrite (MR, s) ;
{if(turnspeed==0)}
analogWrite (ML, s * 0,4) ;
}
Delay(1) ;
}
pleine vitesse
analogWrite (MR, s) ;
{if(turnspeed==0)}
analogWrite (ML, s * 0,4) ;
}
Delay(moveLength) ;
digitalWrite (MR, faible) ;
digitalWrite (ML, faible) ;
arrêter
Delay(pauseLength) ;
}
void goRight (int turnspeed) {//turnspeed = 0 -> rotation lisse, sinon laisser infuser tournant
digitalWrite (MR, faible) ;
Augmentez la vitesse progressivement
pour (s = startSpeed; s < = maxSpeed; s = s + speedStep) {}
analogWrite (ML, s) ;
{if(turnspeed==0)}
analogWrite (MR, s * 0,4) ;
}
Delay(1) ;
}
pleine vitesse
analogWrite (ML, s) ;
{if(turnspeed=0)}
analogWrite (MR, s * 0,4) ;
}
Delay(moveLength) ;
digitalWrite (ML, faible) ;
digitalWrite (MR, faible) ;
arrêter
Delay(pauseLength) ;
}
void rideLine() {}
indiquer la différence entre droite et gauche des capteurs, prenant en compte les paramètres de potentiomètre, tourner en conséquence
Si (sensorValues [0] - sensorValues [2] > turnThreshold + potAdjust & & sensorValues [0] - sensorValues [2] < quickturnThreshold + potAdjust) {}
goRight(0) ;
}
ElseIf (sensorValues [0] - sensorValues [2] > quickturnThreshold + potAdjust) {}
goRight(1) ;
}
ElseIf (sensorValues [2] - sensorValues [0] > turnThreshold - potAdjust & & sensorValues [2] - sensorValues [0] < quickturnThreshold - potAdjust) {}
goLeft(0) ;
}
ElseIf (sensorValues [2] - sensorValues [0] > quickturnThreshold - potAdjust) {}
goLeft(1) ;
}
else {}
goStraight() ;
}
} / / /rideLine()
void searchLine() {}
digitalWrite (ledPin, HIGH) ; indiquer le mode de recherche avec LED
int searchVar = 1 ; variable d’assistance pour augmenter le rayon de mouvement de recherche
tout en
(
sensorValues [0] + sensorValues [1] < 0,1 * betweenReadings [0] & &
sensorValues [1] + sensorValues [2] < 0,1 * betweenReadings [1] & &
sensorValues [0] < indicateThreshold & &
sensorValues [1] < indicateThreshold & &
sensorValues [2] < indicateThreshold
) {
Si (searchVar < = 5) {//go droite pendant un certain temps, dans le cas où effectivement ne pas perdu
goStraight() ;
}
else {//rightward mouvement avec une augmentation de rayon en insérant des séquences droites
pour (i = 0; i < 7; i ++) {}
goRight(1) ;
}
pour (i = 0; i < searchVar/5; i ++) {}
goStraight() ;
}
}
searchVar ++ ; indiquer l’État systématique recherche terminée
readSensors() ; obtenir de nouvelles lectures
}
unknownCount = 0 ; réinitialiser le compteur
digitalWrite (ledPin, basse) ; indiquer la fin du mode de recherche
} / / /searchLine()