|
//Définition des ports pour controler les moteurs
|
|
//Moteur gauche :
|
|
#define IN1a 27
|
|
#define IN2a 14
|
|
#define IN3a 12
|
|
#define IN4a 13
|
|
//Moteur droit :
|
|
#define IN1b 5
|
|
#define IN2b 18
|
|
#define IN3b 19
|
|
#define IN4b 21
|
|
|
|
//Temps d'attente entre deux impulsions :
|
|
int delayTime = 3;
|
|
|
|
//Code éxécuté au démarrage (paramétrage) :
|
|
void setup() {
|
|
//On indique que les ports de controle des moteurs sont des sorties (output)
|
|
pinMode(IN1a, OUTPUT);
|
|
pinMode(IN2a, OUTPUT);
|
|
pinMode(IN3a, OUTPUT);
|
|
pinMode(IN4a, OUTPUT);
|
|
|
|
pinMode(IN1b, OUTPUT);
|
|
pinMode(IN2b, OUTPUT);
|
|
pinMode(IN3b, OUTPUT);
|
|
pinMode(IN4b, OUTPUT);
|
|
|
|
|
|
Serial.begin(115200); //Démarrage d'une communication Série avec l'ordinateur s'il est connecté
|
|
|
|
delay(2000);
|
|
|
|
// DEBUT DU CODE
|
|
// Fonctions disponibles : forward(<distance en cm>) | backward(<distance en cm>) | left(<rotation en degré>) | right(<rotation en degré>)
|
|
|
|
|
|
|
|
// FIN DU CODE
|
|
}
|
|
|
|
//Code éxécuté en boucle jusqu'à l'extinction du robot
|
|
void loop() {
|
|
}
|
|
|
|
|
|
|
|
//Fonction pour convertir une longueur en cm vers un nombre de pas moteur.
|
|
int convertLengthToSteps(float length) {
|
|
float result = length * 512 / (4 * 3.1415);
|
|
return int(result);
|
|
}
|
|
|
|
//fonction pour convertir une rotation en degrés vers un nombre de pas moteur
|
|
int convertRotToSteps(int rotation) {
|
|
int result = convertLengthToSteps(rotation * 3.1415 / 180 * 7.8);
|
|
return result;
|
|
}
|
|
|
|
|
|
void forward(int length) {
|
|
for (int i = 0; i < convertLengthToSteps(length); i++) {
|
|
digitalWrite(IN4a, HIGH);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, HIGH);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, HIGH);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, HIGH);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, HIGH);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, HIGH);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, HIGH);
|
|
digitalWrite(IN4b, HIGH);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
}
|
|
}
|
|
|
|
void backward(int length) {
|
|
for (int i = 0; i < convertLengthToSteps(length); i++) {
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, HIGH);
|
|
digitalWrite(IN4b, HIGH);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, HIGH);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, HIGH);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, HIGH);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, HIGH);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, HIGH);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, HIGH);
|
|
delay(delayTime);
|
|
}
|
|
}
|
|
|
|
void right(int deg) {
|
|
for (int i = 0; i < convertRotToSteps(deg); i++) {
|
|
digitalWrite(IN4a, HIGH);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, HIGH);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, HIGH);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, HIGH);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, HIGH);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, HIGH);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, HIGH);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, HIGH);
|
|
delay(delayTime);
|
|
}
|
|
}
|
|
|
|
void left(int deg) {
|
|
for (int i = 0; i < convertRotToSteps(deg); i++) {
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, HIGH);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, HIGH);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, HIGH);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, HIGH);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, HIGH);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, HIGH);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
digitalWrite(IN4a, HIGH);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
digitalWrite(IN4b, HIGH);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
delay(delayTime);
|
|
}
|
|
}
|
|
|
|
|
|
void stopMotors(void) {
|
|
digitalWrite(IN4a, LOW);
|
|
digitalWrite(IN3a, LOW);
|
|
digitalWrite(IN2a, LOW);
|
|
digitalWrite(IN1a, LOW);
|
|
|
|
digitalWrite(IN4b, LOW);
|
|
digitalWrite(IN3b, LOW);
|
|
digitalWrite(IN2b, LOW);
|
|
digitalWrite(IN1b, LOW);
|
|
}
|
