![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
programme de test de la bibliothèque d'asservissement PID
Dependencies: Encoder_Nucleo_16_bits mbed
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 }
Generated on Sun Jul 31 2022 20:40:08 by
![doxygen](doxygen.png)