Julien VILLEMEJANE / Mbed 2 deprecated PIMS2020_Pointage_v2

Pointage Laser - Manip PIMS avec PID numerique

Dependencies:   mbed mbed-dsp

main2.cpp@2:e9cff52e8ee2, 2020-12-17 (annotated)

Committer:
villemejane
Date:
Thu Dec 17 13:01:44 2020 +0000
Revision:
2:e9cff52e8ee2
Parent:
1:d2ed8dcf965a 
Last version of PID congtroller via RS232

Who changed what in which revision?

UserRevisionLine numberNew contents of line
villemejane 0:44e787b4e1d0 1 #include "mbed.h"
villemejane 0:44e787b4e1d0 2 #include "dsp.h"
villemejane 1:d2ed8dcf965a 3 #define maxX 0.5
villemejane 1:d2ed8dcf965a 4 #define PER_ACQ 0.0001
villemejane 2:e9cff52e8ee2 5 #define PER_STEP 0.00005
villemejane 1:d2ed8dcf965a 6 #define PER_LED 2000
villemejane 1:d2ed8dcf965a 7 #define MAX_CHAR 128
villemejane 1:d2ed8dcf965a 8
villemejane 1:d2ed8dcf965a 9 /* Modules pour Asservissement */
villemejane 1:d2ed8dcf965a 10 Ticker tik_asst;
villemejane 0:44e787b4e1d0 11 AnalogIn inX(PC_2);
villemejane 0:44e787b4e1d0 12 AnalogIn inY(PC_0);
villemejane 0:44e787b4e1d0 13 AnalogOut outX(PA_4);
villemejane 1:d2ed8dcf965a 14 //AnalogOut outY(PA_5);
villemejane 1:d2ed8dcf965a 15 DigitalOut out_led(D13);
villemejane 2:e9cff52e8ee2 16 DigitalOut debug_out(D4);
villemejane 1:d2ed8dcf965a 17 arm_pid_instance_f32 pidX;
villemejane 1:d2ed8dcf965a 18 arm_pid_instance_f32 pidY;
villemejane 1:d2ed8dcf965a 19 /* Modules pour communication avec Matlab */
villemejane 2:e9cff52e8ee2 20 Serial pc(PC_10, PC_11); //TX - transmission, RX - reception
villemejane 2:e9cff52e8ee2 21 Serial realPC(USBTX, USBRX); //TX - transmission, RX - reception
villemejane 1:d2ed8dcf965a 22 InterruptIn button (USER_BUTTON);
villemejane 0:44e787b4e1d0 23
villemejane 1:d2ed8dcf965a 24 /* Variables globales pour Asservissement */
villemejane 2:e9cff52e8ee2 25 char mode = '0';
villemejane 2:e9cff52e8ee2 26 double g_Kpx = 1;
villemejane 2:e9cff52e8ee2 27 double g_Kix = 0;
villemejane 2:e9cff52e8ee2 28 double g_Kdx = 0;
villemejane 2:e9cff52e8ee2 29 double g_Kpy = 1;
villemejane 2:e9cff52e8ee2 30 double g_Kiy = 0;
villemejane 2:e9cff52e8ee2 31 double g_Kdy = 0;
villemejane 2:e9cff52e8ee2 32 double g_sampling_frequency = 1000;
villemejane 1:d2ed8dcf965a 33 /* Variables gloables pour Rampe / Step */
villemejane 1:d2ed8dcf965a 34 double valX;
villemejane 1:d2ed8dcf965a 35 int signeX;
villemejane 1:d2ed8dcf965a 36 double pasX = 0.01;
villemejane 1:d2ed8dcf965a 37 /* Variables globales pour communication avec Matlab */
villemejane 1:d2ed8dcf965a 38 char g_value[MAX_CHAR];
villemejane 1:d2ed8dcf965a 39 int g_index=0;
villemejane 1:d2ed8dcf965a 40 char g_ch;
villemejane 1:d2ed8dcf965a 41 int g_t_temp=0;
villemejane 1:d2ed8dcf965a 42 char data_ok = 0;
villemejane 2:e9cff52e8ee2 43 int g_i = 0;
villemejane 2:e9cff52e8ee2 44 int g_NotFullCommand = 0;
villemejane 1:d2ed8dcf965a 45 int periode_led = PER_LED;
villemejane 1:d2ed8dcf965a 46
villemejane 2:e9cff52e8ee2 47 bool IsAN(char A);
villemejane 2:e9cff52e8ee2 48 int read_command();
villemejane 2:e9cff52e8ee2 49 double k_coef(int i);
villemejane 2:e9cff52e8ee2 50
villemejane 2:e9cff52e8ee2 51 void updatePID(void);
villemejane 2:e9cff52e8ee2 52
villemejane 1:d2ed8dcf965a 53 /* Fonction d'interruption PID - X et Y */
villemejane 0:44e787b4e1d0 54 void controlLoop(void)
villemejane 0:44e787b4e1d0 55 {
villemejane 2:e9cff52e8ee2 56 debug_out = 1;
villemejane 2:e9cff52e8ee2 57 float inXdata = inX.read() - 0.5;
villemejane 0:44e787b4e1d0 58 //Process the PID controller
villemejane 2:e9cff52e8ee2 59 float outxx = arm_pid_f32(&pidX, inXdata);
villemejane 1:d2ed8dcf965a 60 float outyy = arm_pid_f32(&pidY, (float) inY.read()-0.5);
villemejane 0:44e787b4e1d0 61 //Range limit the output
villemejane 0:44e787b4e1d0 62 if (outxx < -0.5)
villemejane 0:44e787b4e1d0 63 outxx = -0.5;
villemejane 0:44e787b4e1d0 64 else if (outxx > 0.5)
villemejane 0:44e787b4e1d0 65 outxx = 0.5;
villemejane 0:44e787b4e1d0 66 if (outyy < -0.5)
villemejane 0:44e787b4e1d0 67 outyy = -0.5;
villemejane 0:44e787b4e1d0 68 else if (outyy > 0.5)
villemejane 0:44e787b4e1d0 69 outyy = 0.5;
villemejane 0:44e787b4e1d0 70 //Set the new output duty cycle
villemejane 2:e9cff52e8ee2 71 //outxx = inXdata;
villemejane 0:44e787b4e1d0 72 outX.write(outxx+0.5);
villemejane 1:d2ed8dcf965a 73 //outY.write(outyy+0.5);
villemejane 2:e9cff52e8ee2 74 debug_out = 0;
villemejane 1:d2ed8dcf965a 75 g_t_temp++;
villemejane 1:d2ed8dcf965a 76 }
villemejane 1:d2ed8dcf965a 77 /* Fonction d'interruption de test - Rampe */
villemejane 1:d2ed8dcf965a 78 void stepLoop(void){
villemejane 2:e9cff52e8ee2 79 debug_out = 1;
villemejane 1:d2ed8dcf965a 80 valX = valX + signeX * pasX;
villemejane 1:d2ed8dcf965a 81 if(valX > maxX){ signeX = -1; }
villemejane 1:d2ed8dcf965a 82 if(valX < 0.0){ signeX = 1; }
villemejane 1:d2ed8dcf965a 83 outX.write(valX);
villemejane 2:e9cff52e8ee2 84 debug_out = 0;
villemejane 1:d2ed8dcf965a 85 g_t_temp++;
villemejane 1:d2ed8dcf965a 86 }
villemejane 2:e9cff52e8ee2 87
villemejane 1:d2ed8dcf965a 88 /* Fonction d'interruption pour la communication RS232 avec Matlab */
villemejane 1:d2ed8dcf965a 89 void IT_Rx_Matlab(){
villemejane 1:d2ed8dcf965a 90 tik_asst.detach();
villemejane 1:d2ed8dcf965a 91 g_ch = pc.getc(); // read it
villemejane 1:d2ed8dcf965a 92 if ((g_index<MAX_CHAR-1) and (data_ok == 0)){
villemejane 1:d2ed8dcf965a 93 g_value[g_index]=g_ch; // put it into the value array and increment the index
villemejane 1:d2ed8dcf965a 94 g_index++;
villemejane 1:d2ed8dcf965a 95 }
villemejane 1:d2ed8dcf965a 96 if(g_ch == '\n'){
villemejane 1:d2ed8dcf965a 97 g_value[g_index] = '\0';
villemejane 1:d2ed8dcf965a 98 g_index=0;
villemejane 1:d2ed8dcf965a 99 data_ok = 1;
villemejane 1:d2ed8dcf965a 100 }
villemejane 1:d2ed8dcf965a 101 tik_asst.attach(&controlLoop, PER_ACQ);
villemejane 1:d2ed8dcf965a 102 }
villemejane 1:d2ed8dcf965a 103
villemejane 1:d2ed8dcf965a 104 /* Fonction d'interruption pour bouton */
villemejane 1:d2ed8dcf965a 105 void IT_button_pressed(void){
villemejane 1:d2ed8dcf965a 106 int k=0;
villemejane 1:d2ed8dcf965a 107 do{
villemejane 1:d2ed8dcf965a 108 pc.putc(g_value[k]);
villemejane 1:d2ed8dcf965a 109 k++;
villemejane 1:d2ed8dcf965a 110 } while (g_value[k]!='\0'); // loop until the '\n' character
villemejane 1:d2ed8dcf965a 111 }
villemejane 2:e9cff52e8ee2 112
villemejane 1:d2ed8dcf965a 113 /* Fonction principale */
villemejane 0:44e787b4e1d0 114 int main()
villemejane 0:44e787b4e1d0 115 {
villemejane 1:d2ed8dcf965a 116 valX = 0;
villemejane 1:d2ed8dcf965a 117 signeX = 1;
villemejane 1:d2ed8dcf965a 118 pc.baud(9600);
villemejane 1:d2ed8dcf965a 119 pc.attach(&IT_Rx_Matlab);
villemejane 1:d2ed8dcf965a 120 strcpy(g_value, "Coucou");
villemejane 1:d2ed8dcf965a 121 g_value[6] = '\r';
villemejane 1:d2ed8dcf965a 122 g_value[7] = '\n';
villemejane 0:44e787b4e1d0 123 //Initialize the PID instance structure
villemejane 2:e9cff52e8ee2 124 updatePID();
villemejane 0:44e787b4e1d0 125 //Run the PID control loop every 1ms
villemejane 1:d2ed8dcf965a 126 // tik_asst.attach(&stepLoop, PER_STEP);
villemejane 1:d2ed8dcf965a 127 button.fall(&IT_button_pressed);
villemejane 1:d2ed8dcf965a 128 tik_asst.attach(&controlLoop, PER_ACQ);
villemejane 1:d2ed8dcf965a 129 //tik_asst.attach(&noLoop, PER_ACQ);
villemejane 1:d2ed8dcf965a 130
villemejane 1:d2ed8dcf965a 131 pc.printf("im here !!\r\n");
villemejane 1:d2ed8dcf965a 132
villemejane 1:d2ed8dcf965a 133 while (true) {
villemejane 1:d2ed8dcf965a 134
villemejane 1:d2ed8dcf965a 135 if(g_t_temp>=periode_led) // LED blinking
villemejane 1:d2ed8dcf965a 136 {
villemejane 1:d2ed8dcf965a 137 g_t_temp=0;
villemejane 1:d2ed8dcf965a 138 out_led = !out_led;
villemejane 1:d2ed8dcf965a 139 }
villemejane 1:d2ed8dcf965a 140
villemejane 1:d2ed8dcf965a 141 if(data_ok){
villemejane 2:e9cff52e8ee2 142 tik_asst.detach();
villemejane 2:e9cff52e8ee2 143 int comm_ok = read_command();//recognizing coefficients
villemejane 2:e9cff52e8ee2 144 if(comm_ok){
villemejane 2:e9cff52e8ee2 145 pc.printf("%c_OK! gX = %lf gY = %lf \r\n", mode, g_Kpx, g_Kpy);
villemejane 2:e9cff52e8ee2 146 if((mode == 'P') or (mode == 'I') or (mode == 'D')){
villemejane 2:e9cff52e8ee2 147 updatePID();
villemejane 2:e9cff52e8ee2 148 }
villemejane 1:d2ed8dcf965a 149 }
villemejane 1:d2ed8dcf965a 150 data_ok = 0;
villemejane 1:d2ed8dcf965a 151 g_index = 0;
villemejane 2:e9cff52e8ee2 152 tik_asst.attach(&controlLoop, PER_ACQ);
villemejane 1:d2ed8dcf965a 153 }
villemejane 1:d2ed8dcf965a 154 }
villemejane 2:e9cff52e8ee2 155 }
villemejane 2:e9cff52e8ee2 156
villemejane 2:e9cff52e8ee2 157 /* Mise à jour des coefficients */
villemejane 2:e9cff52e8ee2 158 void updatePID(void){
villemejane 2:e9cff52e8ee2 159 switch(mode){
villemejane 2:e9cff52e8ee2 160 case 'P' : g_Kix = 0; g_Kiy = 0; g_Kdx = 0; g_Kdy = 0; break;
villemejane 2:e9cff52e8ee2 161 case 'I' : g_Kdx = 0; g_Kdy = 0; break;
villemejane 2:e9cff52e8ee2 162 case 'D' : break;
villemejane 2:e9cff52e8ee2 163 default : g_Kpx = 1; g_Kpy = 1;
villemejane 2:e9cff52e8ee2 164 }
villemejane 2:e9cff52e8ee2 165 pidX.Kp = g_Kpx;
villemejane 2:e9cff52e8ee2 166 pidX.Ki = g_Kix;
villemejane 2:e9cff52e8ee2 167 pidX.Kd = g_Kdx;
villemejane 2:e9cff52e8ee2 168 arm_pid_init_f32(&pidX, 1);
villemejane 2:e9cff52e8ee2 169 pidY.Kp = g_Kpy;
villemejane 2:e9cff52e8ee2 170 pidY.Ki = g_Kiy;
villemejane 2:e9cff52e8ee2 171 pidY.Kd = g_Kdy;
villemejane 2:e9cff52e8ee2 172 arm_pid_init_f32(&pidY, 1);
villemejane 2:e9cff52e8ee2 173 }
villemejane 2:e9cff52e8ee2 174 /* Reconnaissance caractères */
villemejane 2:e9cff52e8ee2 175 bool IsAN(char A)//check if the character if a number or not
villemejane 2:e9cff52e8ee2 176 {
villemejane 2:e9cff52e8ee2 177 char numbers[] = "0123456789\0";
villemejane 2:e9cff52e8ee2 178 for (int i = 0; numbers[i] != '\0'; i++)
villemejane 2:e9cff52e8ee2 179 if (A == numbers[i])
villemejane 2:e9cff52e8ee2 180 return true;
villemejane 2:e9cff52e8ee2 181 return false;
villemejane 2:e9cff52e8ee2 182 }
villemejane 2:e9cff52e8ee2 183
villemejane 2:e9cff52e8ee2 184 double k_coef(int i)
villemejane 2:e9cff52e8ee2 185 {
villemejane 2:e9cff52e8ee2 186 double K = 0;
villemejane 2:e9cff52e8ee2 187 int tempK[100];
villemejane 2:e9cff52e8ee2 188
villemejane 2:e9cff52e8ee2 189 int initial_I = i;
villemejane 2:e9cff52e8ee2 190 int point_j = 0;
villemejane 2:e9cff52e8ee2 191 int negative = 0;
villemejane 2:e9cff52e8ee2 192
villemejane 2:e9cff52e8ee2 193 int exp_trigger = 0;
villemejane 2:e9cff52e8ee2 194 int exp_negative = 0;
villemejane 2:e9cff52e8ee2 195 int temp_exp[100];
villemejane 2:e9cff52e8ee2 196 int j = 0;
villemejane 2:e9cff52e8ee2 197 int j_exp = 0;
villemejane 2:e9cff52e8ee2 198 double power_of_ten = 0;
villemejane 2:e9cff52e8ee2 199
villemejane 2:e9cff52e8ee2 200 for (; g_value[i] != '\0' && g_value[i] != '_'; i++)
villemejane 2:e9cff52e8ee2 201 {
villemejane 2:e9cff52e8ee2 202 if (exp_trigger == 0 && IsAN(g_value[i]))
villemejane 2:e9cff52e8ee2 203 {
villemejane 2:e9cff52e8ee2 204 tempK[j] = g_value[i] - 48;
villemejane 2:e9cff52e8ee2 205 j++;
villemejane 2:e9cff52e8ee2 206 }
villemejane 2:e9cff52e8ee2 207 else if (exp_trigger == 1 && IsAN(g_value[i]))
villemejane 2:e9cff52e8ee2 208 {
villemejane 2:e9cff52e8ee2 209 temp_exp[j_exp] = g_value[i] - 48;
villemejane 2:e9cff52e8ee2 210 j_exp++;
villemejane 2:e9cff52e8ee2 211 }
villemejane 2:e9cff52e8ee2 212
villemejane 2:e9cff52e8ee2 213 else if (exp_trigger == 0 && g_value[i] == '-')
villemejane 2:e9cff52e8ee2 214 {
villemejane 2:e9cff52e8ee2 215 negative = 1;
villemejane 2:e9cff52e8ee2 216 initial_I++;
villemejane 2:e9cff52e8ee2 217 }
villemejane 2:e9cff52e8ee2 218 else if (g_value[i] == 'e' || g_value[i] == 'E')
villemejane 2:e9cff52e8ee2 219 {
villemejane 2:e9cff52e8ee2 220 exp_trigger = 1;
villemejane 2:e9cff52e8ee2 221 }
villemejane 2:e9cff52e8ee2 222 else if (exp_trigger == 1 && (g_value[i] == '+' || g_value[i] == '-'))
villemejane 2:e9cff52e8ee2 223 {
villemejane 2:e9cff52e8ee2 224 if (g_value[i] == '-')
villemejane 2:e9cff52e8ee2 225 {
villemejane 2:e9cff52e8ee2 226 exp_negative = 1;
villemejane 2:e9cff52e8ee2 227 }
villemejane 2:e9cff52e8ee2 228 }
villemejane 2:e9cff52e8ee2 229 else if (exp_trigger == 0 && g_value[i] == '.')
villemejane 2:e9cff52e8ee2 230 {
villemejane 2:e9cff52e8ee2 231 point_j = j;
villemejane 2:e9cff52e8ee2 232 }
villemejane 2:e9cff52e8ee2 233 }
villemejane 2:e9cff52e8ee2 234 g_i = i;
villemejane 2:e9cff52e8ee2 235 for (int k = 0; k < j; k++)
villemejane 2:e9cff52e8ee2 236 {
villemejane 2:e9cff52e8ee2 237 K+= pow(10.0, point_j - k - 1) * tempK[k];
villemejane 2:e9cff52e8ee2 238 }
villemejane 2:e9cff52e8ee2 239
villemejane 2:e9cff52e8ee2 240 if (exp_trigger==1)
villemejane 2:e9cff52e8ee2 241 {
villemejane 2:e9cff52e8ee2 242 for (int k = 0; k < j_exp; k++)
villemejane 2:e9cff52e8ee2 243 {
villemejane 2:e9cff52e8ee2 244 power_of_ten += pow(10.0, j_exp-1 - k) * temp_exp[k];
villemejane 2:e9cff52e8ee2 245 }
villemejane 2:e9cff52e8ee2 246 power_of_ten = (exp_negative == 0) ? (power_of_ten) : (-power_of_ten);
villemejane 2:e9cff52e8ee2 247 K = K * pow(10.0, power_of_ten);
villemejane 2:e9cff52e8ee2 248 }
villemejane 2:e9cff52e8ee2 249
villemejane 2:e9cff52e8ee2 250 return negative==0?K:(-K);
villemejane 2:e9cff52e8ee2 251 }
villemejane 2:e9cff52e8ee2 252
villemejane 2:e9cff52e8ee2 253 int read_command()
villemejane 2:e9cff52e8ee2 254 {
villemejane 2:e9cff52e8ee2 255 int i;
villemejane 2:e9cff52e8ee2 256 switch(g_value[0]){
villemejane 2:e9cff52e8ee2 257 case 'P':
villemejane 2:e9cff52e8ee2 258 mode = 'P';
villemejane 2:e9cff52e8ee2 259 i = 2;
villemejane 2:e9cff52e8ee2 260 g_Kpx=k_coef(i);
villemejane 2:e9cff52e8ee2 261 g_Kpy=k_coef(g_i+1);
villemejane 2:e9cff52e8ee2 262 g_sampling_frequency = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 263 if (g_value[g_i + 1] == '!') g_NotFullCommand = 0;
villemejane 2:e9cff52e8ee2 264 else g_NotFullCommand = 1;
villemejane 2:e9cff52e8ee2 265 return 1;
villemejane 2:e9cff52e8ee2 266 case 'I':
villemejane 2:e9cff52e8ee2 267 mode = 'I';
villemejane 2:e9cff52e8ee2 268 i = 2;
villemejane 2:e9cff52e8ee2 269 g_Kpx =k_coef(i);
villemejane 2:e9cff52e8ee2 270 g_Kpy = k_coef(g_i+1);
villemejane 2:e9cff52e8ee2 271 g_Kix = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 272 g_Kiy = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 273 g_sampling_frequency = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 274 if (g_value[g_i + 1] == '!') g_NotFullCommand = 0;
villemejane 2:e9cff52e8ee2 275 else g_NotFullCommand = 1;
villemejane 2:e9cff52e8ee2 276 return 1;
villemejane 2:e9cff52e8ee2 277 case 'D' :
villemejane 2:e9cff52e8ee2 278 mode = 'D';
villemejane 2:e9cff52e8ee2 279 i = 2;
villemejane 2:e9cff52e8ee2 280 g_Kpx = k_coef(i);
villemejane 2:e9cff52e8ee2 281 g_Kpy = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 282 g_Kix = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 283 g_Kiy = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 284 g_Kdx = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 285 g_Kdy = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 286 g_sampling_frequency = k_coef(g_i + 1);
villemejane 2:e9cff52e8ee2 287 if (g_value[g_i + 1] == '!') g_NotFullCommand = 0;
villemejane 2:e9cff52e8ee2 288 else g_NotFullCommand = 1;
villemejane 2:e9cff52e8ee2 289 return 1;
villemejane 2:e9cff52e8ee2 290 case 'S':
villemejane 2:e9cff52e8ee2 291 mode = 'S';
villemejane 2:e9cff52e8ee2 292 g_Kpx = 1.0;
villemejane 2:e9cff52e8ee2 293 g_Kpy = 1.0;
villemejane 2:e9cff52e8ee2 294 g_Kix = 0;
villemejane 2:e9cff52e8ee2 295 g_Kiy = 0;
villemejane 2:e9cff52e8ee2 296 g_Kdx = 0;
villemejane 2:e9cff52e8ee2 297 g_Kdy = 0;
villemejane 2:e9cff52e8ee2 298 return 1;
villemejane 2:e9cff52e8ee2 299 default :
villemejane 2:e9cff52e8ee2 300 mode = '0';
villemejane 2:e9cff52e8ee2 301 return 0;
villemejane 2:e9cff52e8ee2 302 }
villemejane 2:e9cff52e8ee2 303 }