main.cpp

00001 #include "mbed.h"
00002 #include "Nucleo_Encoder_16_bits.h"
00003 #include "math.h"
00004 #include "PID.h"
00005 
00006 PID         motor       (TIM4, TIM3, PA_9, PA_8, PC_9, PC_8, PC_6, PC_5);
00007 
00008 Serial      pc          (PA_2, PA_3, 921600);                                   // Create a serial link to PC for communication
00009 
00010 DigitalOut              led1        (PA_5);                                     // Added Led1 for test purpose
00011 DigitalOut              led2        (PD_2);                                     // Added Led2 for test purpose
00012 DigitalOut              disquette   (PA_12);                                    // Added baloon destructor command (without it, you might see baloon destructor motor be set to full speed)
00013 
00014 main ()
00015 {
00016     int                 etatmvt = 0;
00017     double              x, y, theta, vG, vD;
00018 
00019     pc.printf ("\n\rHelloWorld\n");
00020 //    led1 = 1;
00021     led2 = 0;
00022     disquette = 0;
00023 
00024     motor.resetPosition();
00025 
00026     wait (5);
00027 
00028     while (1) {
00029 
00030         // Square danse !!!
00031         motor.getPosition (&x,&y, &theta);
00032         motor.getSpeed (&vG, &vD);
00033 
00034         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);
00035 
00036         switch (etatmvt) {
00037             case 0 :
00038                 // On avance de 50cm (coordonnés X = 500, Y = 0)
00039                 motor.setSpeed (300,300);
00040                 if (x > 500) {
00041                     etatmvt = 1;
00042                     pc.printf("\n");
00043                 }
00044                 break;
00045             case 1 :
00046                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
00047                 motor.setSpeed (150,-150);
00048                 if (theta < (-PI/2.0)) {
00049                     etatmvt = 2;
00050                     pc.printf("\n");
00051                 }
00052                 break;
00053             case 2 :
00054                 // On avance de 50cm (coordonnés X = 500, Y = -500)
00055                 motor.setSpeed (300,300);
00056                 if (y < -500)  {
00057                     etatmvt = 3;
00058                     pc.printf("\n");
00059                 }
00060                 break;
00061             case 3 :
00062                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
00063                 motor.setSpeed (150,-150);
00064                 if (theta > 0)  {
00065                     etatmvt = 4;
00066                     pc.printf("\n");
00067                 }
00068                 break;
00069             case 4 :
00070                 // On avance de 50cm (coordonnés X = 0, Y = -500)
00071                 motor.setSpeed (300,300);
00072                 if (x < 0)  {
00073                     etatmvt = 5;
00074                     pc.printf("\n");
00075                 }
00076                 break;
00077             case 5 :
00078                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
00079                 motor.setSpeed (200,-200);
00080                 if (theta < (PI/2.0))  {
00081                     etatmvt = 6;
00082                     pc.printf("\n");
00083                 }
00084                 break;
00085             case 6 :
00086                 // On avance de 50cm (coordonnés X = 0, Y = 0)
00087                 motor.setSpeed (300,300);
00088                 if (y > 0)  {
00089                     etatmvt = 7;
00090                     pc.printf("\n");
00091                 }
00092                 break;
00093             case 7 :
00094                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
00095                 motor.setSpeed (200,-200);
00096                 if (theta < 0) {
00097                     etatmvt = 8;
00098                     pc.printf("\n");
00099                 }
00100                 break;
00101             case 8 :
00102                 // On avance de 50cm (coordonnés X = 500, Y = 0)
00103                 motor.setSpeed (300,300);
00104                 if (x > 500) {
00105                     etatmvt = 9;
00106                     pc.printf("\n");
00107                 }
00108                 break;
00109             case 9 :
00110                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI/2
00111                 motor.setSpeed (-150,150);
00112                 if (theta > (PI/2.0)) {
00113                     etatmvt = 10;
00114                     pc.printf("\n");
00115                 }
00116                 break;
00117             case 10 :
00118                 // On avance de 50cm (coordonnés X = 500, Y = -500)
00119                 motor.setSpeed (300,300);
00120                 if (y > 500)  {
00121                     etatmvt = 11;
00122                     pc.printf("\n");
00123                 }
00124                 break;
00125             case 11 :
00126                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = -PI (Attention comme -PI = +PI, on teste le rebouclage)
00127                 motor.setSpeed (-150,150);
00128                 if (theta < 0)  {
00129                     etatmvt = 12;
00130                     pc.printf("\n");
00131                 }
00132                 break;
00133             case 12 :
00134                 // On avance de 50cm (coordonnés X = 0, Y = -500)
00135                 motor.setSpeed (300,300);
00136                 if (x < 0)  {
00137                     etatmvt = 13;
00138                     pc.printf("\n");
00139                 }
00140                 break;
00141             case 13 :
00142                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = PI/2
00143                 motor.setSpeed (-200,200);
00144                 if (theta > (-PI/2.0))  {
00145                     etatmvt = 14;
00146                     pc.printf("\n");
00147                 }
00148                 break;
00149             case 14 :
00150                 // On avance de 50cm (coordonnés X = 0, Y = 0)
00151                 motor.setSpeed (300,300);
00152                 if (y < 0)  {
00153                     etatmvt = 15;
00154                     pc.printf("\n");
00155                 }
00156                 break;
00157             case 15 :
00158                 // On tourne à droite de 90° (sens antitrigo => négatif) => Angle = 0
00159                 motor.setSpeed (-200,200);
00160                 if (theta > 0) {
00161                     etatmvt = 0;
00162                     pc.printf("\n");
00163                 }
00164                 break;
00165         }
00166         led1 = !led1;
00167         led2 = !led2;
00168         wait (0.005);
00169     }
00170 }