Julien VILLEMEJANE / Mbed 2 deprecated PIMS2020_Pointage_v2

Pointage Laser - Manip PIMS avec PID numerique

Dependencies:   mbed mbed-dsp

main2.cpp@0:44e787b4e1d0, 2020-12-12 (annotated)

Committer:
villemejane
Date:
Sat Dec 12 12:19:03 2020 +0000
Revision:
0:44e787b4e1d0
Child:
1:d2ed8dcf965a
Pointage Laser - Manip PIMS avec PID numerique

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 0:44e787b4e1d0 3
villemejane 0:44e787b4e1d0 4 Ticker ticker;
villemejane 0:44e787b4e1d0 5 AnalogIn inX(PC_2);
villemejane 0:44e787b4e1d0 6 AnalogIn inY(PC_0);
villemejane 0:44e787b4e1d0 7 AnalogOut outX(PA_4);
villemejane 0:44e787b4e1d0 8 AnalogOut outY(PA_5);
villemejane 0:44e787b4e1d0 9 DigitalOut out_led(D4);
villemejane 0:44e787b4e1d0 10 arm_pid_instance_f32 pidX;
villemejane 0:44e787b4e1d0 11 arm_pid_instance_f32 pidY;
villemejane 0:44e787b4e1d0 12
villemejane 0:44e787b4e1d0 13 double Kp = 1.0;
villemejane 0:44e787b4e1d0 14 //
villemejane 0:44e787b4e1d0 15 void controlLop(void){
villemejane 0:44e787b4e1d0 16 out_led = 1;
villemejane 0:44e787b4e1d0 17 double outxx, outyy;
villemejane 0:44e787b4e1d0 18
villemejane 0:44e787b4e1d0 19 outxx = inX.read()-0.5;
villemejane 0:44e787b4e1d0 20 outyy = inY.read()-0.5;
villemejane 0:44e787b4e1d0 21
villemejane 0:44e787b4e1d0 22 outX.write(Kp*outxx);
villemejane 0:44e787b4e1d0 23 outY.write(Kp*outyy);
villemejane 0:44e787b4e1d0 24 out_led = 0;
villemejane 0:44e787b4e1d0 25 }
villemejane 0:44e787b4e1d0 26 //
villemejane 0:44e787b4e1d0 27 void controlLoop(void)
villemejane 0:44e787b4e1d0 28 {
villemejane 0:44e787b4e1d0 29 out_led = 1;
villemejane 0:44e787b4e1d0 30
villemejane 0:44e787b4e1d0 31 //Process the PID controller
villemejane 0:44e787b4e1d0 32 double outxx = arm_pid_f32(&pidX, inX.read()-0.5);
villemejane 0:44e787b4e1d0 33 double outyy = arm_pid_f32(&pidY, inY.read()-0.5);
villemejane 0:44e787b4e1d0 34
villemejane 0:44e787b4e1d0 35 //Range limit the output
villemejane 0:44e787b4e1d0 36 if (outxx < -0.5)
villemejane 0:44e787b4e1d0 37 outxx = -0.5;
villemejane 0:44e787b4e1d0 38 else if (outxx > 0.5)
villemejane 0:44e787b4e1d0 39 outxx = 0.5;
villemejane 0:44e787b4e1d0 40 if (outyy < -0.5)
villemejane 0:44e787b4e1d0 41 outyy = -0.5;
villemejane 0:44e787b4e1d0 42 else if (outyy > 0.5)
villemejane 0:44e787b4e1d0 43 outyy = 0.5;
villemejane 0:44e787b4e1d0 44 //Set the new output duty cycle
villemejane 0:44e787b4e1d0 45 outX.write(outxx+0.5);
villemejane 0:44e787b4e1d0 46 outY.write(outyy+0.5);
villemejane 0:44e787b4e1d0 47 /*
villemejane 0:44e787b4e1d0 48 outX.write(inX.read());
villemejane 0:44e787b4e1d0 49 outY.write(inY.read());
villemejane 0:44e787b4e1d0 50 */
villemejane 0:44e787b4e1d0 51 out_led = 0;
villemejane 0:44e787b4e1d0 52 }
villemejane 0:44e787b4e1d0 53
villemejane 0:44e787b4e1d0 54 int main()
villemejane 0:44e787b4e1d0 55 {
villemejane 0:44e787b4e1d0 56 //Initialize the PID instance structure
villemejane 0:44e787b4e1d0 57 pidX.Kp = 1.0;
villemejane 0:44e787b4e1d0 58 pidX.Ki = 0.001;
villemejane 0:44e787b4e1d0 59 pidX.Kd = 0.0;
villemejane 0:44e787b4e1d0 60 arm_pid_init_f32(&pidX, 1);
villemejane 0:44e787b4e1d0 61 pidY.Kp = 1.0;
villemejane 0:44e787b4e1d0 62 pidY.Ki = 0.001;
villemejane 0:44e787b4e1d0 63 pidY.Kd = 0.0;
villemejane 0:44e787b4e1d0 64 arm_pid_init_f32(&pidY, 1);
villemejane 0:44e787b4e1d0 65
villemejane 0:44e787b4e1d0 66 //Run the PID control loop every 1ms
villemejane 0:44e787b4e1d0 67 ticker.attach(&controlLoop, 0.0001);
villemejane 0:44e787b4e1d0 68 //ticker.attach(&controlLop, 0.0001);
villemejane 0:44e787b4e1d0 69
villemejane 0:44e787b4e1d0 70 while(1) {
villemejane 0:44e787b4e1d0 71 //Nothing to do here (except sleep maybe)
villemejane 0:44e787b4e1d0 72 }
villemejane 0:44e787b4e1d0 73 }