Julien VILLEMEJANE / Mbed 2 deprecated PIMS2020_Pointage_v2

Pointage Laser - Manip PIMS avec PID numerique

Dependencies:   mbed mbed-dsp

main2.cpp@1:d2ed8dcf965a, 2020-12-17 (annotated)

Committer:
villemejane
Date:
Thu Dec 17 10:30:23 2020 +0000
Revision:
1:d2ed8dcf965a
Parent:
0:44e787b4e1d0
Child:
2:e9cff52e8ee2
PID Controller - PIMS

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 1:d2ed8dcf965a 5 #define PER_STEP 0.001
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 1:d2ed8dcf965a 16 arm_pid_instance_f32 pidX;
villemejane 1:d2ed8dcf965a 17 arm_pid_instance_f32 pidY;
villemejane 1:d2ed8dcf965a 18 /* Modules pour communication avec Matlab */
villemejane 1:d2ed8dcf965a 19 Serial pc(USBTX, USBRX); //TX - transmission, RX - reception
villemejane 1:d2ed8dcf965a 20 InterruptIn button (USER_BUTTON);
villemejane 0:44e787b4e1d0 21
villemejane 1:d2ed8dcf965a 22 /* Variables globales pour Asservissement */
villemejane 0:44e787b4e1d0 23 double Kp = 1.0;
villemejane 1:d2ed8dcf965a 24 /* Variables gloables pour Rampe / Step */
villemejane 1:d2ed8dcf965a 25 double valX;
villemejane 1:d2ed8dcf965a 26 int signeX;
villemejane 1:d2ed8dcf965a 27 double pasX = 0.01;
villemejane 1:d2ed8dcf965a 28 /* Variables globales pour communication avec Matlab */
villemejane 1:d2ed8dcf965a 29 char g_value[MAX_CHAR];
villemejane 1:d2ed8dcf965a 30 int g_index=0;
villemejane 1:d2ed8dcf965a 31 char g_ch;
villemejane 1:d2ed8dcf965a 32 int g_t_temp=0;
villemejane 1:d2ed8dcf965a 33 char data_ok = 0;
villemejane 1:d2ed8dcf965a 34 int periode_led = PER_LED;
villemejane 1:d2ed8dcf965a 35
villemejane 1:d2ed8dcf965a 36 /* Fonction d'interruption PID - X et Y */
villemejane 0:44e787b4e1d0 37 void controlLoop(void)
villemejane 0:44e787b4e1d0 38 {
villemejane 0:44e787b4e1d0 39 out_led = 1;
villemejane 0:44e787b4e1d0 40 //Process the PID controller
villemejane 1:d2ed8dcf965a 41 float outxx = arm_pid_f32(&pidX, (float) inX.read()-0.5);
villemejane 1:d2ed8dcf965a 42 float outyy = arm_pid_f32(&pidY, (float) inY.read()-0.5);
villemejane 0:44e787b4e1d0 43 //Range limit the output
villemejane 0:44e787b4e1d0 44 if (outxx < -0.5)
villemejane 0:44e787b4e1d0 45 outxx = -0.5;
villemejane 0:44e787b4e1d0 46 else if (outxx > 0.5)
villemejane 0:44e787b4e1d0 47 outxx = 0.5;
villemejane 0:44e787b4e1d0 48 if (outyy < -0.5)
villemejane 0:44e787b4e1d0 49 outyy = -0.5;
villemejane 0:44e787b4e1d0 50 else if (outyy > 0.5)
villemejane 0:44e787b4e1d0 51 outyy = 0.5;
villemejane 0:44e787b4e1d0 52 //Set the new output duty cycle
villemejane 0:44e787b4e1d0 53 outX.write(outxx+0.5);
villemejane 1:d2ed8dcf965a 54 //outY.write(outyy+0.5);
villemejane 1:d2ed8dcf965a 55 out_led = 0;
villemejane 1:d2ed8dcf965a 56 g_t_temp++;
villemejane 1:d2ed8dcf965a 57 }
villemejane 1:d2ed8dcf965a 58 /* Fonction d'interruption de test - Rampe */
villemejane 1:d2ed8dcf965a 59 void stepLoop(void){
villemejane 1:d2ed8dcf965a 60 out_led = 1;
villemejane 1:d2ed8dcf965a 61 valX = valX + signeX * pasX;
villemejane 1:d2ed8dcf965a 62 if(valX > maxX){ signeX = -1; }
villemejane 1:d2ed8dcf965a 63 if(valX < 0.0){ signeX = 1; }
villemejane 1:d2ed8dcf965a 64 outX.write(valX);
villemejane 0:44e787b4e1d0 65 out_led = 0;
villemejane 1:d2ed8dcf965a 66 g_t_temp++;
villemejane 1:d2ed8dcf965a 67 }
villemejane 1:d2ed8dcf965a 68 /* Fonction d'interruption de test - NOTHING */
villemejane 1:d2ed8dcf965a 69 void noLoop(void){
villemejane 1:d2ed8dcf965a 70 g_t_temp++;
villemejane 0:44e787b4e1d0 71 }
villemejane 1:d2ed8dcf965a 72 /* Fonction d'interruption pour la communication RS232 avec Matlab */
villemejane 1:d2ed8dcf965a 73 void IT_Rx_Matlab(){
villemejane 1:d2ed8dcf965a 74 tik_asst.detach();
villemejane 1:d2ed8dcf965a 75 g_ch = pc.getc(); // read it
villemejane 1:d2ed8dcf965a 76 if ((g_index<MAX_CHAR-1) and (data_ok == 0)){
villemejane 1:d2ed8dcf965a 77 g_value[g_index]=g_ch; // put it into the value array and increment the index
villemejane 1:d2ed8dcf965a 78 g_index++;
villemejane 1:d2ed8dcf965a 79 }
villemejane 1:d2ed8dcf965a 80 if(g_ch == '\n'){
villemejane 1:d2ed8dcf965a 81 g_value[g_index] = '\0';
villemejane 1:d2ed8dcf965a 82 g_index=0;
villemejane 1:d2ed8dcf965a 83 data_ok = 1;
villemejane 1:d2ed8dcf965a 84 }
villemejane 1:d2ed8dcf965a 85 tik_asst.attach(&controlLoop, PER_ACQ);
villemejane 1:d2ed8dcf965a 86 }
villemejane 1:d2ed8dcf965a 87
villemejane 1:d2ed8dcf965a 88 /* Fonction d'interruption pour bouton */
villemejane 1:d2ed8dcf965a 89 void IT_button_pressed(void){
villemejane 1:d2ed8dcf965a 90 int k=0;
villemejane 1:d2ed8dcf965a 91 do{
villemejane 1:d2ed8dcf965a 92 pc.putc(g_value[k]);
villemejane 1:d2ed8dcf965a 93 k++;
villemejane 1:d2ed8dcf965a 94 } while (g_value[k]!='\0'); // loop until the '\n' character
villemejane 1:d2ed8dcf965a 95 }
villemejane 1:d2ed8dcf965a 96 /* Fonction principale */
villemejane 0:44e787b4e1d0 97 int main()
villemejane 0:44e787b4e1d0 98 {
villemejane 1:d2ed8dcf965a 99 valX = 0;
villemejane 1:d2ed8dcf965a 100 signeX = 1;
villemejane 1:d2ed8dcf965a 101 pc.baud(9600);
villemejane 1:d2ed8dcf965a 102 pc.attach(&IT_Rx_Matlab);
villemejane 1:d2ed8dcf965a 103 strcpy(g_value, "Coucou");
villemejane 1:d2ed8dcf965a 104 g_value[6] = '\r';
villemejane 1:d2ed8dcf965a 105 g_value[7] = '\n';
villemejane 0:44e787b4e1d0 106 //Initialize the PID instance structure
villemejane 0:44e787b4e1d0 107 pidX.Kp = 1.0;
villemejane 0:44e787b4e1d0 108 pidX.Ki = 0.001;
villemejane 0:44e787b4e1d0 109 pidX.Kd = 0.0;
villemejane 0:44e787b4e1d0 110 arm_pid_init_f32(&pidX, 1);
villemejane 0:44e787b4e1d0 111 pidY.Kp = 1.0;
villemejane 0:44e787b4e1d0 112 pidY.Ki = 0.001;
villemejane 0:44e787b4e1d0 113 pidY.Kd = 0.0;
villemejane 0:44e787b4e1d0 114 arm_pid_init_f32(&pidY, 1);
villemejane 0:44e787b4e1d0 115 //Run the PID control loop every 1ms
villemejane 1:d2ed8dcf965a 116 // tik_asst.attach(&stepLoop, PER_STEP);
villemejane 1:d2ed8dcf965a 117 button.fall(&IT_button_pressed);
villemejane 1:d2ed8dcf965a 118 tik_asst.attach(&controlLoop, PER_ACQ);
villemejane 1:d2ed8dcf965a 119 //tik_asst.attach(&noLoop, PER_ACQ);
villemejane 1:d2ed8dcf965a 120
villemejane 1:d2ed8dcf965a 121 pc.printf("im here !!\r\n");
villemejane 1:d2ed8dcf965a 122
villemejane 1:d2ed8dcf965a 123 while (true) {
villemejane 1:d2ed8dcf965a 124
villemejane 1:d2ed8dcf965a 125 if(g_t_temp>=periode_led) // LED blinking
villemejane 1:d2ed8dcf965a 126 {
villemejane 1:d2ed8dcf965a 127 g_t_temp=0;
villemejane 1:d2ed8dcf965a 128 out_led = !out_led;
villemejane 1:d2ed8dcf965a 129 }
villemejane 1:d2ed8dcf965a 130
villemejane 1:d2ed8dcf965a 131 if(data_ok){
villemejane 1:d2ed8dcf965a 132 if(g_value[0] == 'P'){ // Mode PID
villemejane 1:d2ed8dcf965a 133 periode_led = 10000;
villemejane 1:d2ed8dcf965a 134 pc.printf("P_OK!");
villemejane 1:d2ed8dcf965a 135 }
villemejane 1:d2ed8dcf965a 136 if(g_value[0] == 'S'){ // Mode STEP
villemejane 1:d2ed8dcf965a 137 periode_led = 2000;
villemejane 1:d2ed8dcf965a 138 pc.printf("S_OK!");
villemejane 1:d2ed8dcf965a 139 }
villemejane 1:d2ed8dcf965a 140 data_ok = 0;
villemejane 1:d2ed8dcf965a 141 g_index = 0;
villemejane 1:d2ed8dcf965a 142 }
villemejane 1:d2ed8dcf965a 143 }
villemejane 0:44e787b4e1d0 144 }