Étape 7: Electronique : Télécharger code
Télécharger le code suivant à la carte Arduino Uno. Vous pouvez trouver des instructions sur la façon de mettre en place votre arduino ici : https://www.arduino.cc/en/Guide/HomePage
début du code
const int DIR_A = 2, //Motor goupille de bouclier A DIR
DIR_B = 4, //Motor goupille de bouclier B DIR
PWM_A = 3, //Motor goupille de bouclier A PWM
PWM_B = 5, //Motor goupille de bouclier A PWM
POT = A3, //Potentiometer broche
FORWARD = 8, //Forward broche
INVERSER = 9, //Reverse broche
GAUCHE = 11, //Left broche
DROITE = 10, //Right broche
CURRENT_A = A1, tige de sonde //Current
CURRENT_B = A0 ; NIP actuel de capteur B
int DIR, OLD_DIR ; Contrôleur d’entrée et entrée histoire
stop_all booléen = false ; Diminuer la vitesse à 0
Déclaration de variables
int MAX_SPEED = 230, MIN_SPEED = 60 ;
flotteur acc = 0 ;
float LEFT_SPEED = 0 ;
float RIGHT_SPEED = 0 ;
float COMP_A = 0 ;
float COMP_B = 0 ;
int LEFT_MOTOR_DIR = 0 ;
int RIGHT_MOTOR_DIR = 0 ;
double TIME_TO_MAX_SPEED = 5000 ;
int DELAY = 100 ;
void setup() {}
Définition des broches d’entrée et de sortie
pinMode (DIR_A, sortie) ;
pinMode (DIR_B, sortie) ;
pinMode (PWM_A, sortie) ;
pinMode (PWM_B, sortie) ;
pinMode (droit, INPUT_PULLUP) ;
pinMode (gauche, INPUT_PULLUP) ;
pinMode (avancer, INPUT_PULLUP) ;
pinMode (REVERSE, INPUT_PULLUP) ;
pinMode (entrée, POT) ;
pinMode (entrée, CURRENT_A) ;
pinMode (entrée, CURRENT_B) ;
Initialise un port série pour le suivi si nécessaire
Serial.Begin(9600) ;
DIR = (digitalRead(FORWARD) << 3)
+ (digitalRead(REVERSE) << 2)
+ (digitalRead(LEFT) << 1)
+ (digitalRead(RIGHT)) ;
OLD_DIR = DIR ;
Diminuer la fréquence PWM pour réduire pleurnicher moteur
setPwmFrequency(3,1) ;
setPwmFrequency(5,1) ;
}
void loop() {}
Lit l’entrée de contrôleur
AVANT : B0111
INVERSER : B1011
GAUCHE : B1101
DROIT : B1110
DIR = (digitalRead(FORWARD) << 3)
+ (digitalRead(REVERSE) << 2)
+ (digitalRead(LEFT) << 1)
+ (digitalRead(RIGHT)) ;
LIT LE POTENTIOMÈTRE ET MAPPE AU MINIMUM ET UNE VITESSE MAXIMALE
int vitesse = map(analogRead(POT), 0, 1023, MIN_SPEED, MAX_SPEED) ;
LIT LE CAPTEUR DE COURANT POUR ÉQUILIBRER LA CHARGE VA DANS LES DEUX MOTEURS
I_A double = (((double)analogRead(CURRENT_A)) * 5/1023.0) - 2,5) / 0,1 ;
double I_B = (((double)analogRead(CURRENT_B)) * 5/1023.0) - 2,5) / 0,1 ;
SI AUCUNE TOUCHE N’EST ENFONCÉE, ARRÊTER LES MOTEURS
Si (DIR == B1111) {}
stop_all = true ;
}
Si {(stop_all)
LEFT_SPEED = (LEFT_SPEED < = 0) ? 0: LEFT_SPEED-(SPEED/TIME_TO_MAX_SPEED*DELAY*2) ;
RIGHT_SPEED = (RIGHT_SPEED < = 0) ? 0: RIGHT_SPEED-(SPEED/TIME_TO_MAX_SPEED*DELAY*2) ;
}
Si (LEFT_SPEED == 0 & & RIGHT_SPEED == 0 & & B1111) {}
stop_all = false ;
OLD_DIR = DIR ;
}
SI LES MOTEURS SONT INACTIFS, GUETTER UN CONTRÔLEUR D’ENTRÉE
Si (stop_all == false) {}
Si (DIR! = OLD_DIR & & digitalRead(FORWARD)! = B0) {}
DIR = OLD_DIR ;
}
{Switch(dir)}
affaire B0111 : //FORWARD
rémunération actuelle de capteur
Si {(abs(I_A-I_B) > 0.1)
Si (I_A > I_B) {}
COMP_A = (I_A-I_B) * VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
COMP_B = (I_A-I_B) *-VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
}
else {}
COMP_A = (I_A-I_B) *-VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
COMP_B = (I_A-I_B) * VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
}
}
else {}
COMP_A = 0 ;
COMP_B = 0 ;
}
calculer l’accélération
ACC = (vitesse-LEFT_SPEED) * 0,1 ;
ACC = (acc > = 6) ? 6: acc ;
ACC = (acc < = 1) ? 1: acc ;
Ajouter l’accélération et l’indemnisation aux moteurs gauche et droit
LEFT_SPEED = LEFT_SPEED + acc + COMP_B ;
RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A ;
LEFT_SPEED = (LEFT_SPEED > = vitesse) ? VITESSE : LEFT_SPEED ;
RIGHT_SPEED = (RIGHT_SPEED > = vitesse) ? VITESSE : RIGHT_SPEED ;
Établir les orientations moteurs, 1 = avant, -1 = marche arrière
LEFT_MOTOR_DIR = 1 ;
RIGHT_MOTOR_DIR = 1 ;
rupture ;
B0101-affaire : //steer lors du déplacement vers l’avant à gauche
LEFT_SPEED = LEFT_SPEED * 0,9 ;
RIGHT_SPEED = RIGHT_SPEED ;
rupture ;
affaire B0110 : //steer droit lors d’un déplacement vers l’avant
LEFT_SPEED = LEFT_SPEED ;
RIGHT_SPEED = RIGHT_SPEED * 0,9 ;
rupture ;
B1011-affaire : //REVERSE
rémunération actuelle de capteur
Si {(abs(I_A-I_B) > 0.1)
Si (I_A > I_B) {}
COMP_A = (I_A-I_B) * VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
COMP_B = (I_A-I_B) *-VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
}
else {}
COMP_A = (I_A-I_B) *-VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
COMP_B = (I_A-I_B) * VITESSE/TIME_TO_MAX_SPEED * RETARD * 0,1 ;
}
}
else {}
COMP_A = 0 ;
COMP_B = 0 ;
}
calculer l’accélération
ACC = (vitesse-LEFT_SPEED) * 0,1 ;
ACC = (acc > = 6) ? 6: acc ;
ACC = (acc < = 1) ? 1: acc ;
Ajouter l’accélération et l’indemnisation aux moteurs gauche et droit
LEFT_SPEED = LEFT_SPEED + acc + COMP_B ;
RIGHT_SPEED = RIGHT_SPEED + acc + COMP_A ;
LEFT_SPEED = (LEFT_SPEED > = vitesse) ? VITESSE : LEFT_SPEED ;
RIGHT_SPEED = (RIGHT_SPEED > = vitesse) ? VITESSE : RIGHT_SPEED ;
Établir les orientations moteurs, 1 = avant, -1 = marche arrière
LEFT_MOTOR_DIR = -1 ;
RIGHT_MOTOR_DIR = -1 ;
rupture ;
B1101-affaire : //LEFT
calculer l’accélération
ACC = (vitesse-LEFT_SPEED) * 0,1 ;
ACC = (acc > = 15) ? 8: acc ;
ACC = (acc < = 1) ? 1: acc ;
Ajouter l’accélération et l’indemnisation aux moteurs gauche et droit
LEFT_SPEED = LEFT_SPEED + acc ;
RIGHT_SPEED = RIGHT_SPEED + acc ;
LEFT_SPEED = (LEFT_SPEED > = vitesse) ? VITESSE : LEFT_SPEED ;
RIGHT_SPEED = (RIGHT_SPEED > = vitesse) ? VITESSE : RIGHT_SPEED ;
Établir les orientations moteurs, 1 = avant, -1 = marche arrière
LEFT_MOTOR_DIR = -1 ;
RIGHT_MOTOR_DIR = 1 ;
rupture ;
B1110 de cas : //RIGHT
calculer l’accélération
ACC = (vitesse-LEFT_SPEED) * 0,1 ;
ACC = (acc > = 15) ? 8: acc ;
ACC = (acc < = 1) ? 1: acc ;
Ajouter l’accélération et l’indemnisation aux moteurs gauche et droit
LEFT_SPEED = LEFT_SPEED + acc ;
RIGHT_SPEED = RIGHT_SPEED + acc ;
LEFT_SPEED = (LEFT_SPEED > = vitesse) ? VITESSE : LEFT_SPEED ;
RIGHT_SPEED = (RIGHT_SPEED > = vitesse) ? VITESSE : RIGHT_SPEED ;
Établir les orientations moteurs, 1 = avant, -1 = marche arrière
LEFT_MOTOR_DIR = 1 ;
RIGHT_MOTOR_DIR = -1 ;
rupture ;
}
}
Set moteurs directions
Si (RIGHT_MOTOR_DIR > 0) {}
digitalWrite (DIR_A, basse) ;
}
else {}
digitalWrite (DIR_A, élevé) ;
}
Si (LEFT_MOTOR_DIR > 0) {}
digitalWrite (DIR_B, basse) ;
}
else {}
digitalWrite (DIR_B, élevé) ;
}
régler la vitesse du moteur
LEFT_SPEED = LEFT_SPEED < = 0 ? 0: LEFT_SPEED ;
RIGHT_SPEED = RIGHT_SPEED < = 0 ? 0: RIGHT_SPEED ;
analogWrite (PWM_A, RIGHT_SPEED) ;
analogWrite (PWM_B, LEFT_SPEED) ;
OLD_DIR = DIR ;
Serial.Print(Speed) ;
Serial.Print("|") ;
Serial.Print("left:") ;
Serial.Print(LEFT_SPEED) ;
Serial.Print("|") ;
Serial.Print("right:") ;
Serial.println(RIGHT_SPEED) ;
Delay(Delay) ;
}
Définit la fréquence de la PWM
void setPwmFrequency (int pin, int diviseur) {}
mode d’octets ;
Si (broche == 5 || Broche == 6 || broche == 9 || broche == 10) {}
{Switch(Divisor)}
cas 1 :
mode = 0 x 01 ;
rupture ;
cas 8 :
mode = 0 x 02 ;
rupture ;
cas 64 :
mode = 0 x 03 ;
rupture ;
cas 256 :
mode = 0 x 04 ;
rupture ;
cas 1024 :
mode = 0 x 05 ;
rupture ;
par défaut :
retour ;
}
Si (broche == 5 || Broche == 6) {}
TCCR0B = TCCR0B & 0b11111000 | mode ;
}
else {}
TCCR1B = TCCR1B & 0b11111000 | mode ;
}
}
ElseIf (broche == 3 || broche == 11) {}
{Switch(Divisor)}
cas 1 :
mode = 0 x 01 ;
rupture ;
cas 8 :
mode = 0 x 02 ;
rupture ;
No 32 :
mode = 0 x 03 ;
rupture ;
cas 64 :
mode = 0 x 04 ;
rupture ;
cas 128 :
mode = 0 x 05 ;
rupture ;
cas 256 :
mode = 0 x 06 ;
rupture ;
cas 1024 :
mode = 0 x 7 ;
rupture ;
par défaut :
retour ;
}
TCCR2B = TCCR2B & 0b11111000 | mode ;
}
}
fin du code
