moniks_bot/server/commande_auto.ino
2024-06-25 11:19:53 +02:00

229 lines
5.4 KiB
C++

//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é
}
//Code éxécuté en boucle jusqu'à l'extinction du robot
void loop() {
//Exemple de commande :
for (int i=0; i<50; i++){
forward();
}
int rotation_steps = convertRotToSteps(46);
for (int i=0; i<rotation_steps; i++){
left();
}
}
//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(){
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(){
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(){
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(){
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);
}