// Tester par GL le 19 Aout 2022 //Avec compensation vitesse moteur gauche ( M1 et M2 ) #include //Define the pin of ultrasonic obstacle avoidance sensor const int Trig = A2; //A2 is defined as the pin Trig connected to the ultrasonic sensor const int Echo = A3; //A3 is defined as the pin Echo connected to the ultrasonic sensor MotorDriver m; int vit=180; int comp = vit /3; // Compensation vitesse pour les moteurs M1 et M2 int vitcomp= (vit - comp); ; int tempo = 1000; int tempo10 = 1000; int dure= 2; int distance = 0; //Variables for storing ultrasonic sensor measurements char serialData; char cmd; void arret() { m.motor(1,RELEASE,0); m.motor(2,RELEASE,0); m.motor(3,RELEASE,0); m.motor(4,RELEASE,0); } void avant() { m.motor(1,FORWARD,vitcomp); m.motor(2,FORWARD,vitcomp); m.motor(3,FORWARD,vit); m.motor(4,FORWARD,vit); } void arriere() { m.motor(1,BACKWARD,vitcomp); m.motor(2,BACKWARD,vitcomp); m.motor(3,BACKWARD,vit); m.motor(4,BACKWARD,vit); } void LatDroite() { m.motor(1,FORWARD,vitcomp); m.motor(2,BACKWARD,vitcomp); m.motor(3,FORWARD,vit); m.motor(4,BACKWARD,vit); } void LatGauche() { m.motor(1,BACKWARD,vitcomp); m.motor(2,FORWARD,vitcomp); m.motor(3,BACKWARD,vit); m.motor(4,FORWARD,vit); } void BiaisDroite() { m.motor(1,FORWARD,vitcomp); m.motor(2,RELEASE,0); m.motor(3,FORWARD,vit); m.motor(4,RELEASE,0); } void BiaisGauche() { m.motor(1,RELEASE,0); m.motor(2,FORWARD,vitcomp); m.motor(3,RELEASE,0); m.motor(4,FORWARD,vit); } void TourneDroite() { m.motor(1,FORWARD,vitcomp); m.motor(2,FORWARD,vitcomp); m.motor(3,RELEASE,0); m.motor(4,RELEASE,0); } void TourneGauche() { m.motor(1,RELEASE,0); m.motor(2,RELEASE,0); m.motor(3,FORWARD,vit); m.motor(4,FORWARD,vit); } void DemiTourDroite() { m.motor(1,FORWARD,vitcomp); m.motor(2,FORWARD,vitcomp); m.motor(3,BACKWARD,vit); m.motor(4,BACKWARD,vit); } void DemiTourGauche() { m.motor(1,BACKWARD,vitcomp); m.motor(2,BACKWARD,vitcomp); m.motor(3,FORWARD,vit); m.motor(4,FORWARD,vit); } int SR04(int Trig,int Echo) { float cm = 0; digitalWrite(Trig,LOW); //Trig is set to low level delayMicroseconds(2); //Wait 2 microseconds digitalWrite(Trig,HIGH); //Trig is set to high level delayMicroseconds(15); //Wait 15 microseconds digitalWrite(Trig,LOW); //Trig is set to low cm = pulseIn(Echo,HIGH)/58.8; //Convert the ranging time to CM cm = (int(cm * 100.0))/100.0; //Leave 2 as a decimal //Serial.print("Distance:"); //Character Distance displayed in serial port monitor window: //Serial.print(cm); //Serial.println("cm"); return cm; //Returns cm value ranging data } void AvoidingObstacles() { if((distance > 20 ) || cmd == 'F')//If the distance is greater than 20cm or bluetooth receives a command equal to F { delay(100);//Delay of 100 ms if(distance > 20)//Again, determine if the distance is really greater than 20cm { avant(); } else //Otherwise the distance is less than 20 { arriere(); delay(500); TourneGauche(); delay(500); } } } void HC05() { if(Serial.available() == -1) { arret(); cmd='S'; } if(Serial.available() > 0) //Determine if the received data is greater than 0 { serialData = Serial.read(); //Receiving function if ('F' == serialData ) cmd = 'F'; //Avant else if('B' == serialData ) cmd = 'B'; //Arriere else if('L' == serialData ) cmd = 'L'; //Tourne Gauche else if('R' == serialData ) cmd = 'R'; //Tourne Droite else if('S' == serialData ) cmd = 'S'; //Stop else if('A' == serialData ) cmd = 'A'; //Lateral Droite else if('C' == serialData ) cmd = 'C'; //Lateral Gauche else if('D' == serialData ) cmd = 'D'; //Demi Tour else if('J' == serialData ) cmd = 'J'; //Biais Droite else if('K' == serialData ) cmd = 'K'; //Biais Gauche else if( serialData == '+' && vit < 245)//If you receive a string plus, the speed increases { comp += 5; //We're going to increase the velocity by 10 at a time } else if( serialData == '-' && vit > 30)//When I receive a string -- the speed decreases { comp -= 5; //I'm going to subtract 10 at a time } } if('F' == cmd) //If Bluetooth receives the string F, the dolly moves forward and enables obstacle avoidance { avant(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); //AvoidingObstacles();//The ultrasonic obstacle avoidance function is called to realize the obstacle avoidance function } else if('A' == serialData) { LatDroite(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } else if('J' == serialData) { BiaisDroite(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } else if('K' == serialData) { BiaisGauche(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } else if('C' == serialData) { LatGauche(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } else if('B' == cmd) //Bluetooth receives string B, car backs up { arriere(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } else if('L' == cmd) //Bluetooth received the string L, car left translation { TourneGauche(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } else if('R' == cmd) //Bluetooth received the string R, car right translation { TourneDroite(); //right translation delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } else if('S' == serialData) //When the string S is received, the cart stops moving { arret(); } else if('D' == serialData) { DemiTourDroite(); delay(tempo10); cmd ='S'; serialData = 'S'; arret(); } } void setup() { Serial.begin(9600); pinMode(Trig,OUTPUT);//The Trig pin connected to the ultrasound is set to output mode pinMode(Echo,INPUT);//The Echo pin connected to the ultrasound is set to input mode void Motor(int Dri,int Speed1,int Speed2,int Speed3,int Speed4); int SR04(int Trig,int Echo); void AvoidingObstacles(); void HC05(); } void loop() { //distance = SR04(Trig,Echo); //Acquisition of ultrasonic distance HC05(); //Call the Bluetooth car control function }