Revision 5:34ed652f8c31, committed 2020-07-21

Comitter:

Nanaud

Date:

Tue Jul 21 19:33:38 2020 +0000

Parent:

4:ad9b7355332e

Child:

6:ea6b30c4bb01

Commit message:

Commande Bluetooth test moteur

Changed in this revision

--- a/codeurs.cpp	Sat Jul 18 17:09:08 2020 +0000
+++ b/codeurs.cpp	Tue Jul 21 19:33:38 2020 +0000
@@ -3,28 +3,13 @@
 
 // Variables globales
 long cpt_cdgA=0; // Codeur de gauche
-//long cpt_cdgB=0;
 long cpt_cddA=0; // Codeur de droite
-//long cpt_cddB=0;
 
 void cdgaRise(){
     if(cdgB) cpt_cdgA++;
     else cpt_cdgA--;
 }
 
-/*
-void cdgbRise(){
-    if(cdgA) cpt_cdgB--;
-    else cpt_cdgB++;    
-}
-*/
-
 void cddaRise(){
     
 }
-
-/*
-void cddbRise(){
-    
-}
-*/
\ No newline at end of file

--- a/debug.h	Sat Jul 18 17:09:08 2020 +0000
+++ b/debug.h	Tue Jul 21 19:33:38 2020 +0000
@@ -19,5 +19,6 @@
 void cmdChoice(char *cmd);
 
 void StringToAngle(char *cmd);
+void StringToVitesse(char *cmd);
 void affUltrasons();
 void affCodeurs();

--- a/debugBT.cpp	Sat Jul 18 17:09:08 2020 +0000
+++ b/debugBT.cpp	Tue Jul 21 19:33:38 2020 +0000
@@ -16,7 +16,8 @@
         buffer[i]=bt.getc();
         pc.printf("%c",buffer[i]);
         bt.printf("%c",buffer[i]);
-        i++;
+
+        if((buffer[i] >= 48 && buffer[i] <=57) || (buffer[i]>=97 && buffer[i]<=122) || buffer[i]==13) i++;
     }
 
     if(buffer[i-1]=='\r') {
@@ -31,6 +32,10 @@
                 StringToAngle(cmd);
                 cmdType=0;
                 break;
+            case 2 : // Commande test vitesse
+                StringToVitesse(cmd);
+                cmdType=0;
+                break;
             default : // Commande par défaut
                 cmdChoice(cmd);
         }

--- a/debugPC.cpp	Sat Jul 18 17:09:08 2020 +0000
+++ b/debugPC.cpp	Tue Jul 21 19:33:38 2020 +0000
@@ -7,6 +7,8 @@
 Ticker ticker_affcd;
 bool aff_US[6];
 bool aff_cd[4];
+bool m_dis = true;    // 0 : START // 1 : STOP
+bool m_avance = false;   // 0 : BACKWARD // 1 : FORWARD
 int cmdType=0;
 
 void serialIT() // avec Tera Term
@@ -33,6 +35,10 @@
                 StringToAngle(cmd);
                 cmdType=0;
                 break;
+            case 2 : // Commande test vitesse
+                StringToVitesse(cmd);
+                cmdType=0;
+                break;
             default : // Commande par défaut
                 cmdChoice(cmd);
         }
@@ -66,6 +72,9 @@
         "cdga", //10
         "cdgb", //11
         "tagl", //12
+        "mten", //13
+        "mtdr", //14
+        "mtvt", //15
         0
     };
 
@@ -88,6 +97,9 @@
             pc.printf("cdon : Affichage resultats codeurs\n\r");
             pc.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
             pc.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
+            pc.printf("mten : Activation/Desactivation des moteurs\r\n");
+            pc.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
+            pc.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
             pc.printf("\n\r");
             // bt
             bt.printf("\n\n\r###HELP###\n\r");
@@ -97,6 +109,9 @@
             bt.printf("cdon : Affichage resultats codeurs\n\r");
             bt.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
             bt.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
+            bt.printf("mten : Activation/Desactivation des moteurs\r\n");
+            bt.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
+            bt.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
             bt.printf("\n\r");
             break;
         case 1:     //usao
@@ -167,6 +182,41 @@
             bt.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
             cmdType=1;
             break;
+        case 13:    // mten
+            if(m_dis) {
+                mot_en();
+                m_dis=!m_dis;
+                pc.printf("Motors Enable\r\n");
+                bt.printf("Motors Enable\r\n");
+            } else {
+                mot_dis();
+                m_dis=!m_dis;
+                pc.printf("Motors Disable\r\n");
+                bt.printf("Motors Disable\r\n");
+            }
+            break;
+        case 14:    // mtdr
+            if(m_avance) {
+                pc.printf("Changement de direction : En avant\r\n");
+                bt.printf("Changement de direction : En avant\r\n");
+                motGauche_fwd();
+                motDroite_fwd();
+                m_avance=!m_avance;
+            } else {
+                pc.printf("Changement de direction : En arriere\r\n");
+                bt.printf("Changement de direction : En arriere\r\n");
+                motGauche_bck();
+                motDroite_bck();
+                m_avance=!m_avance;
+            }
+            break;
+        case 15:    // mtvt
+            pc.printf("Cmd Vitesse robot\r\n");
+            pc.printf("Vitesse en mm/s (sous la forme xxxx): ");
+            bt.printf("Cmd Vitesse robot\r\n");
+            bt.printf("Vitesse en mm/s (sous la forme xxxx): ");
+            cmdType=2;
+            break;
         default:
             pc.printf("Commande invalide\n\r");
             bt.printf("Commande invalide\n\r");
@@ -191,6 +241,32 @@
     }
 }
 
+void StringToVitesse(char *cmd)
+{
+    for(int y=0; y<Lcmd ; y++) {
+        bt.printf("%d ",cmd[y]);
+    }
+    bt.printf("\r\n");
+
+    float cmd3 = 0;
+    cmd3 += (cmd[0]-48)*1000;
+    cmd3 += (cmd[1]-48)*100;
+    cmd3 += (cmd[2]-48)*10;
+    cmd3 += (cmd[3]-48)*1;
+
+    if (cmd3>=0 && cmd3<=9999) {
+        cmd3 = cmd3 / 1000;
+        pc.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
+        bt.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
+        drvGauche.moveLinSpeed(cmd3);
+        drvDroite.moveLinSpeed(cmd3);
+    } else {
+        pc.printf("\r\nVitesse incorrect\r\n\n");
+        bt.printf("\r\nVitesse incorrect\r\n\n");
+        cmd3 = 0;
+    }
+}
+
 void affUltrasons()
 {
     // pc
@@ -216,7 +292,7 @@
 {
     if(aff_cd[0]) pc.printf("CdgA = %d\n\r", cpt_cdgA);
     //if(aff_cd[1]) pc.printf("CdgB = %d\n\r", cpt_cdgB);
-    
+
     if(aff_cd[0]) bt.printf("CdgA = %d\n\r", cpt_cdgA);
     //if(aff_cd[1]) bt.printf("CdgB = %d\n\r", cpt_cdgB);
 }

--- a/motors.cpp	Sat Jul 18 17:09:08 2020 +0000
+++ b/motors.cpp	Tue Jul 21 19:33:38 2020 +0000
@@ -1,49 +1,60 @@
 // Nom du fichier : motors.cpp
 #include "pins.h"
 
-void drv_init(){
+void drv_init()
+{
     mot_dis();
-    //mot_en();
+    motGauche_fwd();
+    motDroite_fwd();
+    drvGauche.moveLinSpeed(0.1);
+    drvDroite.moveLinSpeed(0.1);
     mode = 0b111; // M0, M1 et M2 sont à 1
 }
 
 // ENABLE/DISABLE // Les deux modules ont le même enable
-void mot_en(){
+void mot_en()
+{
     drvGauche.setEnable(START);
     //drvDroite.setEnable(START);
 }
 
-void mot_dis(){
+void mot_dis()
+{
     drvGauche.setEnable(STOP);
     //drvDroite.setEnable(STOP);
 }
 
 // FORWARD
-void motGauche_fwd(){
+void motGauche_fwd()
+{
     drvGauche.setDir(FORWARD);
 }
 
-void motDroite_fwd(){
+void motDroite_fwd()
+{
     drvDroite.setDir(BACKWARD);
 }
 
 // BACKWARD
-void motGauche_bck(){
+void motGauche_bck()
+{
     drvGauche.setDir(BACKWARD);
 }
 
-void motDroite_bck(){
+void motDroite_bck()
+{
     drvDroite.setDir(FORWARD);
 }
 
 // TESTS
 
-void test_drv(){
+void test_drv()
+{
     mot_en();
     motGauche_fwd();
     motDroite_fwd();
-    drvGauche.moveLinSpeed(0.035);
-    drvDroite.moveLinSpeed(0.035);  
+    drvGauche.moveLinSpeed(0.250); // 0.035
+    drvDroite.moveLinSpeed(0.250); // 0.035
     wait(2);
     motGauche_bck();
     motDroite_bck();
@@ -51,6 +62,4 @@
     mot_dis();
 }
 
-void testAngle(int cmdAngle){
-    
-}
\ No newline at end of file
+void testAngle(int cmdAngle){}

--- a/motors.h	Sat Jul 18 17:09:08 2020 +0000
+++ b/motors.h	Tue Jul 21 19:33:38 2020 +0000
@@ -11,4 +11,6 @@
 void motDroite_bck();
 
 void test_drv();
-void testAngle(int cmdAngle);
\ No newline at end of file
+void testAngle(int cmdAngle);
+double mot_acc(double vit,double distanceParcourue, double distAfaire);
+double mot_dec(double vit,double distanceParcourue, double distAfaire);
\ No newline at end of file

--- a/odo_asserv.cpp	Sat Jul 18 17:09:08 2020 +0000
+++ b/odo_asserv.cpp	Tue Jul 21 19:33:38 2020 +0000
@@ -1,11 +1,5 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
-#define Pi 3.14159265359
-
-#define capt 1000 // Nb d'impulsions
-#define ecart 245 // Distance en mm entre les deux roues codeuses
-#define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
-#define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)
 
 // Variables globales
 // cpt_cdgA est le compteur d'impulsion du codeur de gauche
@@ -15,11 +9,11 @@
 
 void odometrie(){
 
-    float distG = (cpt_cdgA * perimetreRoueCodeuse) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
-    float distD = (cpt_cddA * perimetreRoueCodeuse) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    float distG = (cpt_cdgA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    float distD = (cpt_cddA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
     
     float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
-    float rayon = (ecart /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
+    float rayon = (ecartCodeuse /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
     
     float dTheta = distRobot / rayon; // Changement d'orientation => Commande
     float posX0 = posX - rayon*cos(theta); 

--- a/odo_asserv.h	Sat Jul 18 17:09:08 2020 +0000
+++ b/odo_asserv.h	Tue Jul 21 19:33:38 2020 +0000
@@ -1,2 +1,3 @@
 // Nom du fichier : odo.h
 
+

--- a/pins.cpp	Sat Jul 18 17:09:08 2020 +0000
+++ b/pins.cpp	Tue Jul 21 19:33:38 2020 +0000
@@ -16,14 +16,15 @@
 #define DIR1 PC_2
 #define DIR2 PH_1
 #define EN PC_3
-#define rRoue 70 // peut être 71 mm
-#define nbPas 1000
+#define diametreRoue 72 //51.45
+#define rayonRoue (diametreRoue/2)
+#define nbPas 6400 //1000
 BusOut mode(PB_0, PC_1, PC_0); // LSB ... MSB
-DRV8825 drvGauche(EN,DIR1,STEP1,rRoue,nbPas); 
-DRV8825 drvDroite(EN,DIR2,STEP2,rRoue,nbPas); 
+DRV8825 drvGauche(EN,DIR1,STEP1,rayonRoue,nbPas); 
+DRV8825 drvDroite(EN,DIR2,STEP2,rayonRoue,nbPas); 
 
 // Codeurs (Réf : LPJ3806-1000BM-G5-24E)
 InterruptIn cdgA(PA_8); // Codeur de gauche
-InterruptIn cdgB(PA_9);
+DigitalIn cdgB(PA_9);
 InterruptIn cddA(PA_0); // Codeur de droite
-InterruptIn cddB(PA_1);
+DigitalIn cddB(PA_1);

--- a/pins.h	Sat Jul 18 17:09:08 2020 +0000
+++ b/pins.h	Tue Jul 21 19:33:38 2020 +0000
@@ -10,6 +10,15 @@
 #include "codeurs.h"
 #include "odo_asserv.h"
 
+// #define
+#define Pi 3.14159265359
+#define NbPulseCodeur 1000
+#define ecartCodeuse 245 // Distance en mm entre les deux roues codeuses
+#define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
+#define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)
+#define vtSAT 0.250 // m/s 
+#define perimetreE Pi*ecartCodeuse
+
 //Capteurs à ultrasons
 extern DigitalOut trigger;
 extern InterruptIn echo1;
@@ -26,6 +35,6 @@
 
 // Codeurs
 extern InterruptIn cdgA;
-extern InterruptIn cdgB;
+extern DigitalIn cdgB;
 extern InterruptIn cddA;
-extern InterruptIn cddB;
\ No newline at end of file
+extern DigitalIn cddB;
\ No newline at end of file