1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305
|
/***********************************************************
*EVITEMENT AUTOMATIQUE OBSTACLE MINI CAR UNO SANS CONTROLE *
************************************************************/
#include <HCSR04.h>
#include <SoftwareSerial.h>
int inputPin = A3; // ultrasonic module ECHO to A3
int outputPin = A4; // ultrasonic module TRIG to A4
#define Lpwm_pin 5 // broche de contrôle de vitesse ---- ENA de la carte de commande du moteur
#define Rpwm_pin 10 // broche de contrôle de vitesse ---- ENB de la carte de commande du moteur
int pinLB = 7; // broche de contrôle de rotation ---- IN1 de la carte de commande du moteur
int pinLF = 6; // broche de contrôle de rotation ---- IN2 de la carte de commande du moteur
int pinRB = 9; // broche de contrôle de rotation ---- IN3 de la carte de commande du moteur
int pinRF = 8; // broche de contrôle de rotation ---- IN4 de la carte de commande du moteur
unsigned char Lpwm_val = 110; // vitesse de roue gauche initialisée à 150
unsigned char Rpwm_val = 110; // vitesse de roue droite initialisée à 150
int Car_state = 0; // l'état de fonctionnement de la voiture
int servopin = 3; // définition de la broche 3 du port numérique, connexion à la ligne de signal du servomoteur
int myangle; // définition d'une variable d'angle
int pulsewidth; // defining variable of pulse width
unsigned char DuoJiao = 85; // initialized angle of motor at 60°
String messageRecu = ""; // stockage du message venant du bluetooth
//DEFINITION CAPTEUR BLUETOOTH Ne pas oublier de debrancher au chargement du programme
SoftwareSerial hc06(0,1);// RX, TX
unsigned char Bluetooth_val; //définition de la valeur val
void servopulse(int servopin, int myangle) // définition d'une fonction d'impulsion
{
pulsewidth = (myangle * 11) + 500; // conversion de l'angle en valeur de largeur d'impulsion à 400-2380
digitalWrite(servopin, HIGH); // augmenter le niveau de l'interface moteur au maximum
delayMicroseconds(pulsewidth); // retarder la microseconde de la valeur de largeur d'impulsion
digitalWrite(servopin, LOW); // réduire au minimum le niveau d'interface moteur
delay(20 - pulsewidth / 1000);
}
void Set_servopulse(int set_val)
{
for (int i = 0; i <= 10; i++) // donner au moteur suffisamment de temps pour se tourner vers le point d'attribution
servopulse(servopin, set_val); // appel de la fonction d'impulsion
}
void M_Control_IO_config(void)
{
pinMode(pinLB, OUTPUT); // pin 2
pinMode(pinLF, OUTPUT); // pin 4
pinMode(pinRB, OUTPUT); // pin 7
pinMode(pinRF, OUTPUT); // pin 8
pinMode(Lpwm_pin, OUTPUT); // pin 5 (PWM)
pinMode(Rpwm_pin, OUTPUT); // pin 10(PWM)
}
void Set_Speed(unsigned char Left, unsigned char Right) // fonction de réglage de la vitesse
{
analogWrite(Lpwm_pin, Left);
analogWrite(Rpwm_pin, Right);
}
void advance() // Avancer
{
digitalWrite(pinRB, LOW); // making motor move towards right rear
digitalWrite(pinRF, HIGH);
digitalWrite(pinLB, LOW); // making motor move towards left rear
digitalWrite(pinLF, HIGH);
Car_state = 1;
}
void turnR() // tourner à droite (double roue)
{
digitalWrite(pinRB, LOW); // faire touner le moteur vers l'arrière droit
digitalWrite(pinRF, HIGH);
digitalWrite(pinLB, HIGH);
digitalWrite(pinLF, LOW); // faire tourner le moteur vers l'avant gauche
Car_state = 4;
}
void turnL() // tourner à gauche (double roue)
{
digitalWrite(pinRB, HIGH);
digitalWrite(pinRF, LOW ); // faire avancer le moteur vers l'avant droit
digitalWrite(pinLB, LOW); // faire avancer le moteur vers l'avant gauche
digitalWrite(pinLF, HIGH);
Car_state = 3;
}
void stopp() //stop
{
digitalWrite(pinRB, HIGH);
digitalWrite(pinRF, HIGH);
digitalWrite(pinLB, HIGH);
digitalWrite(pinLF, HIGH);
Car_state = 5;
}
void back() //back up
{
digitalWrite(pinRB, HIGH); // faisant tourner le moteur vers l'arrière droit
digitalWrite(pinRF, LOW);
digitalWrite(pinLB, HIGH); // faisant tourner le moteur vers l'arrière gauche
digitalWrite(pinLF, LOW);
Car_state = 2;
}
void Self_Control(void) // évitement d'obstacles par ultrasons autonome
{
int H;
Set_servopulse(DuoJiao);
H = Ultrasonic_Ranging(1);
delay(100);
if (Ultrasonic_Ranging(1) < 35)
{
stopp();
delay(100);
back();
delay(50);
}
if (Ultrasonic_Ranging(1) < 50)
{
stopp();
delay(100);
Set_servopulse(5);
int L = ask_pin_L(2);
delay(100);
Set_servopulse(177);
int R = ask_pin_R(3);
delay(100);
if (ask_pin_L(2) > ask_pin_R(3))
{
back();
delay(100);
turnL();
delay(400);
stopp();
delay(50);
Set_servopulse(DuoJiao);
H = Ultrasonic_Ranging(1);
delay(500);
}
if (ask_pin_L(2) <= ask_pin_R(3))
{
back();
delay(100);
turnR();
delay(400);
stopp();
delay(50);
Set_servopulse(DuoJiao);
H = Ultrasonic_Ranging(1);
delay(300);
}
if (ask_pin_L(2) < 35 && ask_pin_R(3) < 35)
{
stopp();
delay(50);
back();
delay(50);
}
}
else
{
advance();
}
}
int Ultrasonic_Ranging(unsigned char Mode)// fonction de détection de distance par ultrasons , MODE = 1 , affichage , pas d'affichage dans une autre situation
{
int old_distance;
digitalWrite(outputPin, LOW);
delayMicroseconds(2);
digitalWrite(outputPin, HIGH);
delayMicroseconds(20);
digitalWrite(outputPin, LOW);
int distance = pulseIn(inputPin, HIGH); // lecture de la durée de haut niveau
distance = distance / 58; // Transforme le temps d'impulsion en distance
if (Mode == 1) {
Serial.print("\n H = ");
Serial.print(distance, DEC);
return distance;
}
else return distance;
}
int ask_pin_L(unsigned char Mode)
{
int old_Ldistance;
digitalWrite(outputPin, LOW);
delayMicroseconds(2);
digitalWrite(outputPin, HIGH);
delayMicroseconds(20);
digitalWrite(outputPin, LOW);
int Ldistance = pulseIn(inputPin, HIGH);
Ldistance = Ldistance / 58; // Transforme le temps d'impulsion en distance
if (Mode == 2) {
Serial.print("\n L = ");
Serial.print(Ldistance, DEC);
return Ldistance;
}
else return Ldistance;
}
int ask_pin_R(unsigned char Mode)
{
int old_Rdistance;
digitalWrite(outputPin, LOW);
delayMicroseconds(2);
digitalWrite(outputPin, HIGH); //
delayMicroseconds(20);
digitalWrite(outputPin, LOW);
int Rdistance = pulseIn(inputPin, HIGH);
Rdistance = Rdistance / 58; // Transforme le temps d'impulsion en distance
if (Mode == 3) {
Serial.print("\n R = ");
Serial.print(Rdistance, DEC);
return Rdistance;
}
else return Rdistance;
}
void setup()
{
pinMode(servopin, OUTPUT); //setting motor interface as output
M_Control_IO_config(); //motor controlling the initialization of IO
Set_Speed(Lpwm_val, Rpwm_val); //setting initialized speed
Set_servopulse(DuoJiao); //setting initialized motor angle
pinMode(inputPin, INPUT); //starting receiving IR remote control signal
pinMode(outputPin, OUTPUT); //IO of ultrasonic module
Serial.begin(9600); //initialized serial port , using Bluetooth as serial port, setting baud
stopp(); //stop
Serial.println("TEST Deplacement ");
advance(); delay(1000); stopp();
back(); delay(1000); stopp();
turnL(); delay(1000); stopp();
turnR(); delay(1000); stopp();
delay(1000); stopp();
Serial.println("TEST Servo ");
Set_servopulse(DuoJiao);delay(1000);
Set_servopulse(5);delay(1000);
Set_servopulse(177);delay(1000);
Set_servopulse(DuoJiao);delay(1000);
Serial.println("BOB READY ");
}
/*void loop()
{
Self_Control();
}*/
void loop()
{
while (Serial.available())
{
delay (3); // lecture car par car
char c = Serial.read();
messageRecu += c;
}
if (messageRecu.length() > 0)
{
if ((messageRecu.indexOf("recule") != -1)) { // marche arrière
Serial.println ("recule");
stopp(); back();delay(300);stopp();
messageRecu = "";
}
else if ((messageRecu.indexOf("gauche") != -1)) { // tourne à gauche brièvement
Serial.println ("a gauche");
turnL(); delay(200); stopp(); messageRecu = "sto";
}
else if ((messageRecu.indexOf("droite") != -1)) { //tourne à droite brièvement
Serial.println ("a droite");
turnR(); delay(200); stopp(); messageRecu = "sto";
}
else if ((messageRecu.indexOf("sto") != -1)) { // moteur stop
Serial.println ("stop"); stopp(); messageRecu = "";
}
else if (((messageRecu.indexOf("demi-tour") != -1)) || ((messageRecu.indexOf("retourne") != -1)) || ((messageRecu.indexOf("reviens") != -1))) { // moteur stop
Serial.println ("demi-tour");
turnL(); delay(1000); stopp(); messageRecu = "avance";
}
else if ((messageRecu.indexOf("bâbord") != -1)) { // moteur stop
Serial.println ("quart-de-tour à gauche");
turnL(); delay(500); stopp(); messageRecu = "";
}
else if ((messageRecu.indexOf("tribord") != -1)) { // moteur stop
Serial.println ("quart-de-tour à droite");
turnR(); delay(500); stopp(); messageRecu = "";
}
else if ((messageRecu.indexOf("repos") != -1)) { // marche avant
stopp();
Serial.println (messageRecu);
}
else if ((messageRecu.indexOf("avance") != -1)) { // marche avant
Self_Control(); //les instructions à répéter tant que la condition est vérifiée
Serial.println (messageRecu);
}
else if ((messageRecu.indexOf("viens ici") != -1)) { // marche avant
Self_Control(); //les instructions à répéter tant que la condition est vérifiée
Serial.println (messageRecu);
}
}
} |
Partager