Bibliothèque des éléments du robot

Dependencies:   CMPS03 CNY70 GP2A PID Pixy RC_Servo VMA306 mbed

Files at this revision

API Documentation at this revision

Comitter:
haarkon
Date:
Tue Jun 05 13:55:04 2018 +0000
Parent:
6:ebc5b5ad56d4
Commit message:
Unstable first attempt of all system in one test

Changed in this revision

CMPS03.lib Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
Pixy.lib Show annotated file Show diff for this revision Revisions of this file
RC_Servo.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r ebc5b5ad56d4 -r 6d58adc26b78 CMPS03.lib
--- a/CMPS03.lib	Tue Jun 05 08:06:57 2018 +0000
+++ b/CMPS03.lib	Tue Jun 05 13:55:04 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/FRC-Hackathon/code/CMPS03/#3e9586433ce5
+https://os.mbed.com/teams/FRC-Hackathon/code/CMPS03/#ab9eadf7537a
diff -r ebc5b5ad56d4 -r 6d58adc26b78 PID.lib
--- a/PID.lib	Tue Jun 05 08:06:57 2018 +0000
+++ b/PID.lib	Tue Jun 05 13:55:04 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/FRC-Hackathon/code/PID/#1e8c73d85648
+https://os.mbed.com/teams/FRC-Hackathon/code/PID/#4553677e8b99
diff -r ebc5b5ad56d4 -r 6d58adc26b78 Pixy.lib
--- a/Pixy.lib	Tue Jun 05 08:06:57 2018 +0000
+++ b/Pixy.lib	Tue Jun 05 13:55:04 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/FRC-Hackathon/code/Pixy/#d395fe0fb0e0
+https://os.mbed.com/teams/FRC-Hackathon/code/Pixy/#5982d904f7aa
diff -r ebc5b5ad56d4 -r 6d58adc26b78 RC_Servo.lib
--- a/RC_Servo.lib	Tue Jun 05 08:06:57 2018 +0000
+++ b/RC_Servo.lib	Tue Jun 05 13:55:04 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/FRC-Hackathon/code/RC_Servo/#cf65fc8b0de1
+https://os.mbed.com/teams/FRC-Hackathon/code/RC_Servo/#014d36c33b73
diff -r ebc5b5ad56d4 -r 6d58adc26b78 main.cpp
--- a/main.cpp	Tue Jun 05 08:06:57 2018 +0000
+++ b/main.cpp	Tue Jun 05 13:55:04 2018 +0000
@@ -9,8 +9,10 @@
 
 #define PI  3.1415926535898
 
+#define NSpeed  100
+
 Serial      pc          (PA_2, PA_3, 921600);
-//PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
+PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
 
 CMPS03      boussole    (PC_4);
 
@@ -21,13 +23,14 @@
 GP2A        ld1         (PA_4, 20, 150, 55);
 GP2A        sd1         (PB_0, 5, 80, 10);
 
-RC_Servo    ballon      (PA_10, 0);
-RC_Servo    verrou      (PA_11, 0);
+RC_Servo    ballon      (PB_10, 0);
+RC_Servo    verrou      (PA_15, 0);
 
 VMA306      UltraSon    (PB_15, PA_6, PB_14, PC_7, PB_13, PB_2);
 
 PIXY        Pixy        (PA_0, PA_1, 230400);
 
+DigitalIn   bp          (PC_13);
 
 DigitalOut  led1        (PA_5);
 DigitalOut  led2        (PD_2);
@@ -40,7 +43,7 @@
     double              pos = 0.5;
     T_pixyCCBloc        CCBuf;
     T_pixyNMBloc        NMBuf;
-    double              x, y, theta;
+    double              x, y, theta, vG, vD;
 
     pc.printf ("\n\rHelloWorld\n");
     led1 = 1;
@@ -101,51 +104,71 @@
         }
 
         // Square danse !!!
-        /*motor.getPosition (&x,&y, &theta);
+        motor.getPosition (&x,&y, &theta);
+        motor.getSpeed (&vG, &vD);
+
+        pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf\n", etatmvt, x, y, 180*theta/PI, vG, vD);
+
         switch (etatmvt) {
             case 0 :
                 // On avance de 50cm (coordonnés X = 500, Y = 0)
-                motor.setSpeed (400,400);
-                if (x >= 500) etatmvt = 1;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (x > 300) {
+                    etatmvt = 1;
+                }
                 break;
             case 1 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
-                motor.setSpeed (400,-400);
-                if (theta <= -PI/2.0) etatmvt = 2;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta < (-PI/2.0)) {
+                    etatmvt = 2;
+                }
                 break;
             case 2 :
                 // On avance de 50cm (coordonnés X = 500, Y = -500)
-                motor.setSpeed (400,400);
-                if (y <= -500) etatmvt = 3;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (y < -300)  {
+                    etatmvt = 3;
+                }
                 break;
             case 3 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
-                motor.setSpeed (400,-400);
-                if (theta >= 0) etatmvt = 4;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta > 0)  {
+                    etatmvt = 4;
+                }
                 break;
             case 4 :
                 // On avance de 50cm (coordonnés X = 0, Y = -500)
-                motor.setSpeed (400,400);
-                if (x <= 0) etatmvt = 5;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (x < 0)  {
+                    etatmvt = 5;
+                }
                 break;
             case 5 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
-                motor.setSpeed (400,-400);
-                if (theta <= PI/2.0) etatmvt = 6;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta < (PI/2.0))  {
+                    etatmvt = 6;
+                }
                 break;
             case 6 :
                 // On avance de 50cm (coordonnés X = 0, Y = 0)
-                motor.setSpeed (400,400);
-                if (y <= 0) etatmvt = 7;
+                motor.setSpeed (NSpeed,NSpeed);
+                if (y > 0)  {
+                    etatmvt = 7;
+                }
                 break;
             case 7 :
                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
-                motor.setSpeed (400,-400);
-                if (theta <= 0) etatmvt = 0;
+                motor.setSpeed (NSpeed,-NSpeed);
+                if (theta < 0) {
+                    etatmvt = 0;
+                }
                 break;
         }
-*/
-        pc.printf ("\n");
+
+        pc.printf ("\n\n");
         led1 = !led1;
         led2 = !led2;
         wait (0.2);