// Rob2_Blue2 //----------------------------------------------------------------------- // // Programme du Robot2 en mode bluetooth V3 // // Version compatible du logiciel "Arduino Bluetooth Controlled Joystick" // de "Uncia Robotics" côté Android // //----------------------------------------------------------------------- // // A utiliser avec : // Carte Adafruit configurée sous 3,3 V // Carte Seeedstudio Bluetooth avec connection sur A1, BT_Tx sur 0 et BT_Rx sur 1 // Carte LCD Keypad DFRobot modifiee pour tourner sous 3,3V // Carte Chipkit Uno32 // // Pour le telephone j'utilise "Arduino Bluetooth Controlled Joystick" // // (C) 2014-2020 Eric CHAMOUARD // // Contact : ouaib.site.eric@free.fr // // (Libre de droits pour utilisation non commerciale // et à vos propres risques) // //----------------------------------------------------------------------- // Version : // Interprétation de la trame joystick // Todo : // Timeout : fonction implantée mais non testée (calibrer millis sur ChipKit32) // La position du joystick est recue sur deux entiers donnant l'angle (A) et la puissance (P) // Dans un plan x,y : // // A=90, P=100 // A=180, P=100 A=0, P=0 A=0, P=100 // A=270, P100 // // Le programme applique alors les consignes suivantes aux moteurs G (gauche) et D (droite) : // // G=255, D=255 // G=-255, D=255 G=0, D=0 G=255, D=-255 // G=-255, D=-255 // // Options //#define fastboot // Permet (si pas commenté) un boot plus rapide #define NomRobot "Robot_Ex1" // Nom du robot à programmer unsigned long timeout=300; // Timeout commande en milisecondes // Librairies #include #include #include "utility/Adafruit_PWMServoDriver.h" #include #include // Motor shield avec adresse par defaut Adafruit_MotorShield AFMS = Adafruit_MotorShield(); // Objets moteurs (M1 et M2 sont les moteurs de gauche) Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1); Adafruit_DCMotor *myMotor2 = AFMS.getMotor(2); Adafruit_DCMotor *myMotor3 = AFMS.getMotor(3); Adafruit_DCMotor *myMotor4 = AFMS.getMotor(4); // Objet LCD LCDKeypad lcd; // La led de status bluetooth et la pin ou il est connecte #define Led 13 #define BTstatus A1 // Definition liées au protocole #define STX 0x02 #define ETX 0x03 // Definition du symboleBluetooth byte CharBlue[8]= { B01000, B01100, B11010, B01100, B01100, B11010, B01100, B01000, }; void setup() { int i, k; Serial.begin(9600); // S'est avéré nécessaire, je ne sais pas pourquoi... AFMS.begin(); // Bibliotheque avec frequence par defaut de 1,6 kHz // Valeurs de depart (arret du robot) myMotor1->setSpeed(0); myMotor2->setSpeed(0); myMotor3->setSpeed(0); myMotor4->setSpeed(0); myMotor1->run(RELEASE); myMotor2->run(RELEASE); myMotor3->run(RELEASE); myMotor4->run(RELEASE); // Init de l'afficheur et message d'accueil lcd.begin(16, 2); lcd.clear(); lcd.print(" Robot 2"); lcd.setCursor(0,1); lcd.print(" Mode Blue3"); delay(1000); #ifndef fastboot delay(1000); for(k=0; k<2; k++) { lcd.scrollDisplayLeft(); delay(400); } for(i=0;i<1;i++) { for (k=0; k<5; k++) { lcd.scrollDisplayRight(); delay(400); } for(k=0; k<5; k++) { lcd.scrollDisplayLeft(); delay(400); } } for(k=0; k<14; k++) { lcd.scrollDisplayLeft(); delay(400); } #endif lcd.clear(); // Creation du caractere bluetooth lcd.createChar(0, CharBlue); // Setup bluetooth et led associée pinMode(Led, OUTPUT); digitalWrite(Led, LOW); setupBlueToothConnection(); } void setupBlueToothConnection() { //Serial.begin(38400); // Vitesse par defaut du bluetooth (avant reprogrammation) Serial.begin(115200); // Vitesse par defaut du bluetooth (après reprogrammation) Serial.print("\r\n+STWMOD=0\r\n"); // Bluetooth work in slave mode Serial.print("\r\n+STNA="); Serial.print(NomRobot); Serial.print("\r\n"); // Nom du peripherique Bluetooth Serial.print("\r\n+STOAUT=1\r\n"); // Autorisation des apparaeil appairés de se connecter Serial.print("\r\n+STAUTO=0\r\n"); // Interdiction de l'auto-connection delay(2000); // Délai requis Serial.print("\r\n+INQ=1\r\n"); // Appareil Bluetooth interrogeable delay(2000); // Délai requis Serial.flush(); //Serial.print("\r\n+STBD=115200\r\n"); // Passage à 115200 Bauds (à faire une seule fois) } void loop() { int i=0, j=0, angle, strength, k, G, D; float rad, x, y; char recvChar, trame[8]; unsigned long timelast; while(1) { if(digitalRead(BTstatus)) { // Affichage du status bluetooth digitalWrite(Led, HIGH); // La led de la carte UNO32 lcd.setCursor(0,0); lcd.write(0); // L'afficheur lcd.print(" on "); timelast=millis(); // Initialisation du time out if(Serial.available()>0) { recvChar=Serial.read(); // Traitement du caractere reçu // Rangement du caractère dans le buffer if(j>7) { // Erreur car trame trop longue j=0; } else { // Rangement du caractère trame[j]=recvChar; ++j; } // En debug /*if(j>0) { lcd.setCursor(j-1,1); lcd.write(recvChar); }*/ // Test d'une fin de trame if(recvChar=='#') { if(j==8) { // Fin d'une trame // Test des valeurs recues if(trame[0]>='0' && trame[0]<='3' && trame[1]>='0' && trame[1]<='9' && trame[2]>='0' && trame[2]<='9' && trame[3]>='0' && trame[3]<='1' && trame[4]>='0' && trame[4]<='9' && trame[5]>='0' && trame[5]<='9' && trame[6]>='0' && trame[6]<='3') { angle=(trame[0]-'0')*100+(trame[1]-'0')*10+trame[2]-'0'; strength=(trame[3]-'0')*100+(trame[4]-'0')*10+trame[5]-'0'; /*lcd.setCursor(0,1); lcd.print(angle); lcd.print(" "); lcd.setCursor(4,1); lcd.print(strength); lcd.print(" ");*/ if(angle>=0 && angle <360 && strength>=0 && strength<=100) { // Tout est OK on programme les moteurs // Calcul des valeurs moteurs gauche et moteurs droite rad=3.14159*(float)angle/180.; x=cos(rad)*(float)strength*255./100.; y=sin(rad)*(float)strength*255./100.; G=y+x; D=y-x; /*if(G>255)G=255; if(G<-255)G=-255; if(D>255)D=255; if(D<-255)D=-255; lcd.setCursor(0,1); lcd.print(G); lcd.print(" "); lcd.setCursor(5,1); lcd.print(D); lcd.print(" ");*/ if(G>255)G=255; if(G<-255)G=-255; if(D>255)D=255; if(D<-255)D=-255; // Application des valeurs aux moteurs if(G>=0) { myMotor1->run(FORWARD); myMotor2->run(FORWARD); myMotor1->setSpeed(G); myMotor2->setSpeed(G); } else { myMotor1->run(BACKWARD); myMotor2->run(BACKWARD); myMotor1->setSpeed(-G); myMotor2->setSpeed(-G); } if(D>=0) { myMotor3->run(FORWARD); myMotor4->run(FORWARD); myMotor3->setSpeed(D); myMotor4->setSpeed(D); } else { myMotor3->run(BACKWARD); myMotor4->run(BACKWARD); myMotor3->setSpeed(-D); myMotor4->setSpeed(-D); } } } j=0; } else { // Trame incomplète j=0; } // Reset du timeout timelast=millis(); } } // Test du timeout if((millis()-timelast)>timeout) { // Arret d'urgence myMotor1->run(FORWARD); myMotor2->run(FORWARD); myMotor3->run(FORWARD); myMotor4->run(FORWARD); myMotor1->setSpeed(0); myMotor2->setSpeed(0); myMotor3->setSpeed(0); myMotor4->setSpeed(0); } } else { // Affichage du status bluetooth digitalWrite(Led, HIGH); // La led de la carte UNO32 lcd.setCursor(0,0); lcd.write(0); // L'afficheur lcd.print(" off"); // Arret des moteurs myMotor1->run(FORWARD); myMotor2->run(FORWARD); myMotor3->run(FORWARD); myMotor4->run(FORWARD); myMotor1->setSpeed(0); myMotor2->setSpeed(0); myMotor3->setSpeed(0); myMotor4->setSpeed(0); } } }