Diff: debugPC.cpp

Revision:

5:34ed652f8c31

Parent:

4:ad9b7355332e

Child:

6:ea6b30c4bb01

--- 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);
 }