Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@1:13d8940f0fd4, 2017-10-27 (annotated)

Committer:
Arnoud113
Date:
Fri Oct 27 08:59:41 2017 +0000
Revision:
1:13d8940f0fd4
Parent:
0:77ad62c61c78
Child:
2:2563d1d8461f
first publish not working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arnoud113 0:77ad62c61c78 1 #include "mbed.h"
Arnoud113 0:77ad62c61c78 2 #include "QEI.h"
Arnoud113 0:77ad62c61c78 3 #include "MODSERIAL.h"
Arnoud113 0:77ad62c61c78 4 #include "math.h"
Arnoud113 0:77ad62c61c78 5
Arnoud113 0:77ad62c61c78 6
Arnoud113 0:77ad62c61c78 7
Arnoud113 0:77ad62c61c78 8 DigitalOut gpo(D0);
Arnoud113 0:77ad62c61c78 9 DigitalOut ledb(LED_BLUE);
Arnoud113 0:77ad62c61c78 10 DigitalOut ledr(LED_RED);
Arnoud113 0:77ad62c61c78 11 DigitalOut ledg(LED_GREEN);
Arnoud113 0:77ad62c61c78 12 DigitalOut motor1DC(D7);
Arnoud113 0:77ad62c61c78 13 PwmOut motor1PWM(D6);
Arnoud113 0:77ad62c61c78 14 DigitalOut motor2DC(D4);
Arnoud113 0:77ad62c61c78 15 PwmOut motor2PWM(D5);
Arnoud113 0:77ad62c61c78 16
Arnoud113 0:77ad62c61c78 17 AnalogIn potMeter1(A0);
Arnoud113 0:77ad62c61c78 18 AnalogIn potMeter2(A1);
Arnoud113 0:77ad62c61c78 19 DigitalIn button1(D11);
Arnoud113 0:77ad62c61c78 20 DigitalIn button2(D12);
Arnoud113 0:77ad62c61c78 21 QEI Encoder1(D12,D13,NC,4200);
Arnoud113 0:77ad62c61c78 22 QEI Encoder2(D9,D8,NC,4200);
Arnoud113 0:77ad62c61c78 23
Arnoud113 0:77ad62c61c78 24 MODSERIAL pc(USBTX,USBRX);
Arnoud113 0:77ad62c61c78 25
Arnoud113 0:77ad62c61c78 26 Ticker controller;
Arnoud113 0:77ad62c61c78 27
Arnoud113 0:77ad62c61c78 28 int potmultiplier = 8000; // Multiplier for the pot meter reference which is normally between 0 and 1
Arnoud113 0:77ad62c61c78 29
Arnoud113 0:77ad62c61c78 30 //Start constants for the PID -------------------------------
Arnoud113 0:77ad62c61c78 31 const double pi = 3.1415926535897;
Arnoud113 0:77ad62c61c78 32 const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
Arnoud113 0:77ad62c61c78 33
Arnoud113 0:77ad62c61c78 34 //verplaatst
Arnoud113 0:77ad62c61c78 35 const float RAD_PER_PULSE = (2*pi)/4200;
Arnoud113 0:77ad62c61c78 36
Arnoud113 0:77ad62c61c78 37 const float M1_KP = 10, M1_KI = 0.5, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
Arnoud113 0:77ad62c61c78 38 double m1_err_int = 0, m1_prev_err = 0 ;
Arnoud113 0:77ad62c61c78 39 const double M1_F_A1 = 1.0 , M1_F_A2 = 2.0 , M1_F_B0 = 1.0 , M1_F_B1 = 3.0 , M1_F_B2 = 4.0 ;
Arnoud113 0:77ad62c61c78 40 double m1_f_v1 = 0 , m1_f_v2 = 0 ;
Arnoud113 0:77ad62c61c78 41 //End of constant for the PID
Arnoud113 0:77ad62c61c78 42
Arnoud113 1:13d8940f0fd4 43 //Get reference position in different way-----------------START
Arnoud113 0:77ad62c61c78 44 float Get_X_Position(){
Arnoud113 1:13d8940f0fd4 45 double X = potMeter1 * potmultiplier;
Arnoud113 1:13d8940f0fd4 46 return X;
Arnoud113 0:77ad62c61c78 47 }
Arnoud113 0:77ad62c61c78 48
Arnoud113 0:77ad62c61c78 49 float Get_Y_Position(){
Arnoud113 1:13d8940f0fd4 50 double Y = potMeter2 * potmultiplier;
Arnoud113 1:13d8940f0fd4 51 return Y;
Arnoud113 0:77ad62c61c78 52 }
Arnoud113 1:13d8940f0fd4 53 //-----------------------------------------------------------END
Arnoud113 0:77ad62c61c78 54
Arnoud113 0:77ad62c61c78 55 //Start PID part ------------------------------------------START
Arnoud113 0:77ad62c61c78 56 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
Arnoud113 0:77ad62c61c78 57
Arnoud113 0:77ad62c61c78 58 // Derivative
Arnoud113 0:77ad62c61c78 59 double e_der = (e - e_prev)/Ts; // Ts = motor1-timestep
Arnoud113 0:77ad62c61c78 60
Arnoud113 0:77ad62c61c78 61 // biquad part, see slide
Arnoud113 0:77ad62c61c78 62 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
Arnoud113 0:77ad62c61c78 63
Arnoud113 0:77ad62c61c78 64 e_prev = e;
Arnoud113 0:77ad62c61c78 65
Arnoud113 0:77ad62c61c78 66 // Integral
Arnoud113 0:77ad62c61c78 67 e_int += Ts*e;
Arnoud113 0:77ad62c61c78 68
Arnoud113 0:77ad62c61c78 69
Arnoud113 0:77ad62c61c78 70 //PID
Arnoud113 0:77ad62c61c78 71 return Kp*e + Ki*e_int + Kd * e_der;
Arnoud113 0:77ad62c61c78 72
Arnoud113 0:77ad62c61c78 73 }
Arnoud113 0:77ad62c61c78 74
Arnoud113 0:77ad62c61c78 75
Arnoud113 0:77ad62c61c78 76
Arnoud113 0:77ad62c61c78 77
Arnoud113 0:77ad62c61c78 78
Arnoud113 0:77ad62c61c78 79
Arnoud113 0:77ad62c61c78 80 void Controller(){
Arnoud113 1:13d8940f0fd4 81 //double x = potMeter1 * potmultiplier;
Arnoud113 1:13d8940f0fd4 82 double x = Get_X_Position();
Arnoud113 1:13d8940f0fd4 83
Arnoud113 1:13d8940f0fd4 84 double y = Get_Y_Position();
Arnoud113 1:13d8940f0fd4 85 //double y = potMeter2 * potmultiplier;
Arnoud113 0:77ad62c61c78 86
Arnoud113 0:77ad62c61c78 87 double reference_motor1 = atan(y/x); // reference for Theta
Arnoud113 0:77ad62c61c78 88 double reference_motor2 = sqrt((x*x+y*y)); // reference for radius
Arnoud113 0:77ad62c61c78 89
Arnoud113 1:13d8940f0fd4 90 int position_motor1 = RAD_PER_PULSE*Encoder1.getPulses(); // current position for theta
Arnoud113 1:13d8940f0fd4 91 int position_motor2 = RAD_PER_PULSE*Encoder2.getPulses(); // current position for the radius
Arnoud113 0:77ad62c61c78 92
Arnoud113 0:77ad62c61c78 93 double motor1 = PID(reference_motor1 - position_motor1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
Arnoud113 0:77ad62c61c78 94 double motor2 = PID(reference_motor2 - position_motor2, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
Arnoud113 0:77ad62c61c78 95
Arnoud113 0:77ad62c61c78 96 pc.baud(115200);
Arnoud113 1:13d8940f0fd4 97 pc.printf("\r Position(X,Y)=(%f,%f), Ref(Theta,R): (%f,%f), Pos(Theta,R):(%i,%i), Motor Value(M1,M2):(%f,%f).\n",x, y, reference_motor1, reference_motor2, position_motor1, position_motor2, motor1, motor2);
Arnoud113 0:77ad62c61c78 98
Arnoud113 0:77ad62c61c78 99 motor1PWM = motor1;
Arnoud113 0:77ad62c61c78 100 motor2PWM = motor2;
Arnoud113 0:77ad62c61c78 101
Arnoud113 1:13d8940f0fd4 102 if(motor1 > 0.3){
Arnoud113 0:77ad62c61c78 103 motor1DC = 1;
Arnoud113 0:77ad62c61c78 104
Arnoud113 0:77ad62c61c78 105 ledr = 1;
Arnoud113 0:77ad62c61c78 106 ledg = 1; //Blau
Arnoud113 0:77ad62c61c78 107 ledb = 0;
Arnoud113 0:77ad62c61c78 108 }
Arnoud113 1:13d8940f0fd4 109 else if (motor1<-0.3) {
Arnoud113 0:77ad62c61c78 110 motor1DC = 0;
Arnoud113 0:77ad62c61c78 111
Arnoud113 0:77ad62c61c78 112 ledb = 1;
Arnoud113 0:77ad62c61c78 113 ledr = 1;
Arnoud113 0:77ad62c61c78 114 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 115
Arnoud113 0:77ad62c61c78 116 }
Arnoud113 0:77ad62c61c78 117 else{
Arnoud113 0:77ad62c61c78 118 motor1PWM = 0;
Arnoud113 0:77ad62c61c78 119
Arnoud113 0:77ad62c61c78 120 ledb = 1; //Rood
Arnoud113 0:77ad62c61c78 121 ledr = 0;
Arnoud113 0:77ad62c61c78 122 ledg = 1;
Arnoud113 0:77ad62c61c78 123 }
Arnoud113 0:77ad62c61c78 124
Arnoud113 0:77ad62c61c78 125 if(motor2 > 0.3){
Arnoud113 0:77ad62c61c78 126 motor1DC = 1;
Arnoud113 0:77ad62c61c78 127
Arnoud113 0:77ad62c61c78 128 ledr = 1;
Arnoud113 0:77ad62c61c78 129 ledg = 1; //Blau
Arnoud113 0:77ad62c61c78 130 ledb = 0;
Arnoud113 0:77ad62c61c78 131 }
Arnoud113 0:77ad62c61c78 132 else if (motor2<-0.3) {
Arnoud113 0:77ad62c61c78 133 motor1DC = 0;
Arnoud113 0:77ad62c61c78 134
Arnoud113 0:77ad62c61c78 135 ledb = 1;
Arnoud113 0:77ad62c61c78 136 ledr = 1;
Arnoud113 0:77ad62c61c78 137 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 138
Arnoud113 0:77ad62c61c78 139 }
Arnoud113 0:77ad62c61c78 140 else{
Arnoud113 0:77ad62c61c78 141 motor2PWM = 0;
Arnoud113 0:77ad62c61c78 142
Arnoud113 0:77ad62c61c78 143 ledb = 1; //Rood
Arnoud113 0:77ad62c61c78 144 ledr = 0;
Arnoud113 0:77ad62c61c78 145 ledg = 1;
Arnoud113 0:77ad62c61c78 146 }
Arnoud113 0:77ad62c61c78 147 }
Arnoud113 0:77ad62c61c78 148
Arnoud113 0:77ad62c61c78 149 int main()
Arnoud113 0:77ad62c61c78 150 {
Arnoud113 0:77ad62c61c78 151 controller.attach(&Controller, M1_TS);
Arnoud113 0:77ad62c61c78 152
Arnoud113 0:77ad62c61c78 153 while(1){}
Arnoud113 0:77ad62c61c78 154
Arnoud113 0:77ad62c61c78 155 }