FRC - Hackathon
programme de test de la bibliothèque d'asservissement PID
Dependencies: Encoder_Nucleo_16_bits mbed
Diff: main.cpp
- Revision:
- 1:81896d606b4e
- Parent:
- 0:86c5a1f6e21d
--- a/main.cpp Tue Jun 05 08:39:37 2018 +0000
+++ b/main.cpp Wed Jun 06 20:28:39 2018 +0000
@@ -3,11 +3,11 @@
#include "math.h"
#include "PID.h"
-PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5, PA_5);
+PID motor (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
Serial pc (PA_2, PA_3, 921600); // Create a serial link to PC for communication
-//DigitalOut led1 (PA_5); // Added Led1 for test purpose
+DigitalOut led1 (PA_5); // Added Led1 for test purpose
DigitalOut led2 (PD_2); // Added Led2 for test purpose
DigitalOut disquette (PA_12); // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed)
@@ -20,7 +20,7 @@
// led1 = 1;
led2 = 0;
disquette = 0;
-
+
motor.resetPosition();
wait (5);
@@ -30,54 +30,141 @@
// Square danse !!!
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", etatmvt, x, y, theta, vG, vD);
+
+ pc.printf ("\rEtape = %d : x = %5.1lf, y = %5.1lf, theta = %5.1lf - speedG = %5.1lf, speedD = %5.1lf", 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 (300,300);
+ if (x > 500) {
+ etatmvt = 1;
+ pc.printf("\n");
+ }
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 (150,-150);
+ if (theta < (-PI/2.0)) {
+ etatmvt = 2;
+ pc.printf("\n");
+ }
break;
case 2 :
// On avance de 50cm (coordonnés X = 500, Y = -500)
- motor.setSpeed (400,400);
- if (y <= -500) etatmvt = 3;
+ motor.setSpeed (300,300);
+ if (y < -500) {
+ etatmvt = 3;
+ pc.printf("\n");
+ }
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 (150,-150);
+ if (theta > 0) {
+ etatmvt = 4;
+ pc.printf("\n");
+ }
break;
case 4 :
// On avance de 50cm (coordonnés X = 0, Y = -500)
- motor.setSpeed (400,400);
- if (x <= 0) etatmvt = 5;
+ motor.setSpeed (300,300);
+ if (x < 0) {
+ etatmvt = 5;
+ pc.printf("\n");
+ }
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 (200,-200);
+ if (theta < (PI/2.0)) {
+ etatmvt = 6;
+ pc.printf("\n");
+ }
break;
case 6 :
// On avance de 50cm (coordonnés X = 0, Y = 0)
- motor.setSpeed (400,400);
- if (y <= 0) etatmvt = 7;
+ motor.setSpeed (300,300);
+ if (y > 0) {
+ etatmvt = 7;
+ pc.printf("\n");
+ }
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 (200,-200);
+ if (theta < 0) {
+ etatmvt = 8;
+ pc.printf("\n");
+ }
+ break;
+ case 8 :
+ // On avance de 50cm (coordonnés X = 500, Y = 0)
+ motor.setSpeed (300,300);
+ if (x > 500) {
+ etatmvt = 9;
+ pc.printf("\n");
+ }
+ break;
+ case 9 :
+ // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
+ motor.setSpeed (-150,150);
+ if (theta > (PI/2.0)) {
+ etatmvt = 10;
+ pc.printf("\n");
+ }
+ break;
+ case 10 :
+ // On avance de 50cm (coordonnés X = 500, Y = -500)
+ motor.setSpeed (300,300);
+ if (y > 500) {
+ etatmvt = 11;
+ pc.printf("\n");
+ }
+ break;
+ case 11 :
+ // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
+ motor.setSpeed (-150,150);
+ if (theta < 0) {
+ etatmvt = 12;
+ pc.printf("\n");
+ }
+ break;
+ case 12 :
+ // On avance de 50cm (coordonnés X = 0, Y = -500)
+ motor.setSpeed (300,300);
+ if (x < 0) {
+ etatmvt = 13;
+ pc.printf("\n");
+ }
+ break;
+ case 13 :
+ // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
+ motor.setSpeed (-200,200);
+ if (theta > (-PI/2.0)) {
+ etatmvt = 14;
+ pc.printf("\n");
+ }
+ break;
+ case 14 :
+ // On avance de 50cm (coordonnés X = 0, Y = 0)
+ motor.setSpeed (300,300);
+ if (y < 0) {
+ etatmvt = 15;
+ pc.printf("\n");
+ }
+ break;
+ case 15 :
+ // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
+ motor.setSpeed (-200,200);
+ if (theta > 0) {
+ etatmvt = 0;
+ pc.printf("\n");
+ }
break;
}
-
-// led1 = !led1;
+ led1 = !led1;
led2 = !led2;
- wait (0.2);
+ wait (0.005);
}
}
\ No newline at end of file