Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@5:a1a5b5bebd5c, 2017-10-31 (annotated)

Committer:
Arnoud113
Date:
Tue Oct 31 23:04:25 2017 +0000
Revision:
5:a1a5b5bebd5c
Parent:
4:5f7d1654108d
Child:
6:d4f6d9400f53
Partially reverted to exclude the second PID that I implemented. Because of this PID the whole system got stuck and I was not sure why.

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 2:2563d1d8461f 5 #include "FastPWM.h"
Arnoud113 3:b353ee86230a 6 #include "encoder.h"
Arnoud113 0:77ad62c61c78 7
Arnoud113 0:77ad62c61c78 8
Arnoud113 0:77ad62c61c78 9
Arnoud113 0:77ad62c61c78 10 DigitalOut gpo(D0);
Arnoud113 0:77ad62c61c78 11 DigitalOut ledb(LED_BLUE);
Arnoud113 0:77ad62c61c78 12 DigitalOut ledr(LED_RED);
Arnoud113 0:77ad62c61c78 13 DigitalOut ledg(LED_GREEN);
Arnoud113 0:77ad62c61c78 14 DigitalOut motor1DC(D7);
Arnoud113 3:b353ee86230a 15 DigitalOut motor2DC(D4);
Arnoud113 2:2563d1d8461f 16 FastPWM motor1PWM(D6);
Arnoud113 2:2563d1d8461f 17 FastPWM motor2PWM(D5);
Arnoud113 0:77ad62c61c78 18
Arnoud113 0:77ad62c61c78 19 AnalogIn potMeter1(A0);
Arnoud113 0:77ad62c61c78 20 AnalogIn potMeter2(A1);
Arnoud113 0:77ad62c61c78 21 DigitalIn button1(D11);
Arnoud113 0:77ad62c61c78 22 DigitalIn button2(D12);
Arnoud113 3:b353ee86230a 23 Encoder Encoder1(D12,D13);
Arnoud113 3:b353ee86230a 24 Encoder Encoder2(D8,D9);
Arnoud113 0:77ad62c61c78 25
Arnoud113 0:77ad62c61c78 26 MODSERIAL pc(USBTX,USBRX);
Arnoud113 0:77ad62c61c78 27
Arnoud113 0:77ad62c61c78 28 Ticker controller;
Arnoud113 0:77ad62c61c78 29
Arnoud113 3:b353ee86230a 30 // ---- Motor Constants-------
Arnoud113 3:b353ee86230a 31 float Pwmperiod = 0.0001f;
Arnoud113 3:b353ee86230a 32 int potmultiplier = 800; // Multiplier for the pot meter reference which is normally between 0 and 1
Arnoud113 3:b353ee86230a 33 float gainM1 = 1/35.17; // encoder pulses per degree theta
Arnoud113 3:b353ee86230a 34 float gainM2 = 0.01; // gain for radius r
Arnoud113 0:77ad62c61c78 35
Arnoud113 3:b353ee86230a 36 // new PID constants, will have to be determined trough trial and error.
Arnoud113 2:2563d1d8461f 37
Arnoud113 3:b353ee86230a 38 double kp = 250;
Arnoud113 3:b353ee86230a 39 double ki = 100;
Arnoud113 3:b353ee86230a 40 double kd = 0;
Arnoud113 3:b353ee86230a 41
Arnoud113 3:b353ee86230a 42
Arnoud113 3:b353ee86230a 43 volatile float motor1;
Arnoud113 3:b353ee86230a 44 volatile float motor2;
Arnoud113 3:b353ee86230a 45
Arnoud113 3:b353ee86230a 46 //Start constants PID -------------------------------
Arnoud113 0:77ad62c61c78 47 const double pi = 3.1415926535897;
Arnoud113 5:a1a5b5bebd5c 48 const double M1_TS = 0.001f; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
Arnoud113 0:77ad62c61c78 49
Arnoud113 0:77ad62c61c78 50 //verplaatst
Arnoud113 3:b353ee86230a 51 const float RAD_PER_PULSE = (2*pi)/4200;
Arnoud113 3:b353ee86230a 52 const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ
Arnoud113 5:a1a5b5bebd5c 53 const float M1_KP = 10
Arnoud113 5:a1a5b5bebd5c 54 const float M1_KI = 0.5
Arnoud113 3:b353ee86230a 55 const float M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
Arnoud113 3:b353ee86230a 56 double m1_err_int = 0;
Arnoud113 3:b353ee86230a 57 double m1_prev_err = 0;
Arnoud113 3:b353ee86230a 58
Arnoud113 3:b353ee86230a 59
Arnoud113 3:b353ee86230a 60 //---- Biquad constants---------
Arnoud113 3:b353ee86230a 61 const double M1_F_A1 = 1.0 ;
Arnoud113 3:b353ee86230a 62 const double M1_F_A2 = 2.0;
Arnoud113 3:b353ee86230a 63 const double M1_F_B0 = 1.0;
Arnoud113 3:b353ee86230a 64 const double M1_F_B1 = 3.0;
Arnoud113 3:b353ee86230a 65 const double M1_F_B2 = 4.0;
Arnoud113 3:b353ee86230a 66 double m1_f_v1 = 0;
Arnoud113 3:b353ee86230a 67 double m1_f_v2 = 0;
Arnoud113 5:a1a5b5bebd5c 68 //---------------------------------End of constants PID
Arnoud113 0:77ad62c61c78 69
Arnoud113 3:b353ee86230a 70 //-----------------Start PID part----------------------------START
Arnoud113 5:a1a5b5bebd5c 71 double PID1(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 3:b353ee86230a 72
Arnoud113 5:a1a5b5bebd5c 73 double e_der = (e - e_prev)/Ts; // Derivative, Ts = motor1-timestep
Arnoud113 3:b353ee86230a 74 // biquad part, see slide
Arnoud113 5:a1a5b5bebd5c 75 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
Arnoud113 5:a1a5b5bebd5c 76 e_prev = e;
Arnoud113 5:a1a5b5bebd5c 77 e_int += Ts*e; // Integral
Arnoud113 3:b353ee86230a 78
Arnoud113 5:a1a5b5bebd5c 79 return Kp*e + Ki*e_int + Kd * e_der;
Arnoud113 3:b353ee86230a 80
Arnoud113 3:b353ee86230a 81 }
Arnoud113 3:b353ee86230a 82
Arnoud113 3:b353ee86230a 83 //------------Get reference position-----------------START
Arnoud113 0:77ad62c61c78 84 float Get_X_Position(){
Arnoud113 1:13d8940f0fd4 85 double X = potMeter1 * potmultiplier;
Arnoud113 1:13d8940f0fd4 86 return X;
Arnoud113 0:77ad62c61c78 87 }
Arnoud113 0:77ad62c61c78 88
Arnoud113 0:77ad62c61c78 89 float Get_Y_Position(){
Arnoud113 1:13d8940f0fd4 90 double Y = potMeter2 * potmultiplier;
Arnoud113 1:13d8940f0fd4 91 return Y;
Arnoud113 0:77ad62c61c78 92 }
Arnoud113 3:b353ee86230a 93 //----------------------------------------------------END
Arnoud113 0:77ad62c61c78 94
Arnoud113 3:b353ee86230a 95 //-------------Get current Position-------------------START
Arnoud113 3:b353ee86230a 96 double motor1_Position(){ // has as output Theta
Arnoud113 5:a1a5b5bebd5c 97 double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta
Arnoud113 3:b353ee86230a 98 return pos_m1;
Arnoud113 0:77ad62c61c78 99 }
Arnoud113 3:b353ee86230a 100 double motor2_Position(){ //output R
Arnoud113 3:b353ee86230a 101 double pos_m2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
Arnoud113 3:b353ee86230a 102 return pos_m2;
Arnoud113 3:b353ee86230a 103 }
Arnoud113 3:b353ee86230a 104 //-----------------------------------------------------END
Arnoud113 0:77ad62c61c78 105
Arnoud113 0:77ad62c61c78 106
Arnoud113 3:b353ee86230a 107 //------------Controller-------------------------------START
Arnoud113 0:77ad62c61c78 108 void Controller(){
Arnoud113 3:b353ee86230a 109
Arnoud113 1:13d8940f0fd4 110 double x = Get_X_Position();
Arnoud113 1:13d8940f0fd4 111 double y = Get_Y_Position();
Arnoud113 3:b353ee86230a 112
Arnoud113 3:b353ee86230a 113 double reference_motor1 = (atan(y/x)*180)/pi; // reference for Theta
Arnoud113 3:b353ee86230a 114 double reference_motor2 = sqrt((x*x+y*y)); // reference for radius
Arnoud113 0:77ad62c61c78 115
Arnoud113 3:b353ee86230a 116 float pos_M1 = motor1_Position(); // current position for theta
Arnoud113 3:b353ee86230a 117 float pos_M2 = motor2_Position(); // current position for the radius
Arnoud113 0:77ad62c61c78 118
Arnoud113 3:b353ee86230a 119 double delta1 = PID1(reference_motor1 - pos_M1, 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 5:a1a5b5bebd5c 120 double delta2 = PID2(reference_motor2 - pos_M2, 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 3:b353ee86230a 121
Arnoud113 3:b353ee86230a 122 double dTheta = reference_motor1 - pos_M1;
Arnoud113 3:b353ee86230a 123 double dRadius = reference_motor2 - pos_M2;
Arnoud113 0:77ad62c61c78 124
Arnoud113 3:b353ee86230a 125
Arnoud113 0:77ad62c61c78 126
Arnoud113 0:77ad62c61c78 127 pc.baud(115200);
Arnoud113 5:a1a5b5bebd5c 128 pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, delta1 ,delta2);
Arnoud113 0:77ad62c61c78 129
Arnoud113 2:2563d1d8461f 130 //motor1PWM = motor1;
Arnoud113 2:2563d1d8461f 131 //motor2PWM = motor2;
Arnoud113 0:77ad62c61c78 132
Arnoud113 3:b353ee86230a 133 if(delta1 > 10.0){
Arnoud113 3:b353ee86230a 134 motor1DC = 0;
Arnoud113 0:77ad62c61c78 135
Arnoud113 0:77ad62c61c78 136 ledr = 1;
Arnoud113 0:77ad62c61c78 137 ledg = 1; //Blau
Arnoud113 0:77ad62c61c78 138 ledb = 0;
Arnoud113 0:77ad62c61c78 139 }
Arnoud113 3:b353ee86230a 140 else if (delta1< -10.0) {
Arnoud113 3:b353ee86230a 141 motor1DC = 1;
Arnoud113 0:77ad62c61c78 142
Arnoud113 0:77ad62c61c78 143 ledb = 1;
Arnoud113 0:77ad62c61c78 144 ledr = 1;
Arnoud113 0:77ad62c61c78 145 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 146
Arnoud113 0:77ad62c61c78 147 }
Arnoud113 0:77ad62c61c78 148 else{
Arnoud113 0:77ad62c61c78 149 motor1PWM = 0;
Arnoud113 0:77ad62c61c78 150
Arnoud113 0:77ad62c61c78 151 ledb = 1; //Rood
Arnoud113 0:77ad62c61c78 152 ledr = 0;
Arnoud113 0:77ad62c61c78 153 ledg = 1;
Arnoud113 0:77ad62c61c78 154 }
Arnoud113 3:b353ee86230a 155
Arnoud113 3:b353ee86230a 156 motor1 = abs(delta1)/1000.0f;
Arnoud113 3:b353ee86230a 157 if(motor1 >= 0.50f) {
Arnoud113 3:b353ee86230a 158 motor1 = 0.50f;
Arnoud113 3:b353ee86230a 159 }
Arnoud113 0:77ad62c61c78 160
Arnoud113 3:b353ee86230a 161 if(delta2 > 10.0){
Arnoud113 3:b353ee86230a 162 motor2DC = 0;
Arnoud113 0:77ad62c61c78 163
Arnoud113 0:77ad62c61c78 164 ledr = 1;
Arnoud113 0:77ad62c61c78 165 ledg = 1; //Blau
Arnoud113 0:77ad62c61c78 166 ledb = 0;
Arnoud113 0:77ad62c61c78 167 }
Arnoud113 3:b353ee86230a 168 else if (delta2<-10.0) {
Arnoud113 3:b353ee86230a 169 motor2DC = 1;
Arnoud113 0:77ad62c61c78 170
Arnoud113 0:77ad62c61c78 171 ledb = 1;
Arnoud113 0:77ad62c61c78 172 ledr = 1;
Arnoud113 0:77ad62c61c78 173 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 174
Arnoud113 0:77ad62c61c78 175 }
Arnoud113 0:77ad62c61c78 176 else{
Arnoud113 0:77ad62c61c78 177 motor2PWM = 0;
Arnoud113 0:77ad62c61c78 178
Arnoud113 0:77ad62c61c78 179 ledb = 1; //Rood
Arnoud113 0:77ad62c61c78 180 ledr = 0;
Arnoud113 0:77ad62c61c78 181 ledg = 1;
Arnoud113 0:77ad62c61c78 182 }
Arnoud113 3:b353ee86230a 183
Arnoud113 3:b353ee86230a 184 motor2 = abs(delta2)/1000.0f;
Arnoud113 3:b353ee86230a 185 if(motor1 >= 0.50f) {
Arnoud113 3:b353ee86230a 186 motor1 = 0.50f;
Arnoud113 3:b353ee86230a 187 }
Arnoud113 3:b353ee86230a 188
Arnoud113 3:b353ee86230a 189 motor1PWM = motor1 + 0.50f;
Arnoud113 3:b353ee86230a 190 motor2PWM = motor1 + 0.50f;
Arnoud113 3:b353ee86230a 191
Arnoud113 3:b353ee86230a 192 //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", , motor1PWM, motor2PWM);
Arnoud113 3:b353ee86230a 193 //pc.printf("\r
Arnoud113 0:77ad62c61c78 194 }
Arnoud113 0:77ad62c61c78 195
Arnoud113 0:77ad62c61c78 196 int main()
Arnoud113 0:77ad62c61c78 197 {
Arnoud113 0:77ad62c61c78 198 controller.attach(&Controller, M1_TS);
Arnoud113 3:b353ee86230a 199 //motor1PWM.period(Pwmperiod);
Arnoud113 3:b353ee86230a 200 //motor2PWM.period(Pwmperiod);
Arnoud113 0:77ad62c61c78 201
Arnoud113 3:b353ee86230a 202 while(1){
Arnoud113 3:b353ee86230a 203 /*
Arnoud113 3:b353ee86230a 204 double x = Get_X_Position();
Arnoud113 3:b353ee86230a 205 double y = Get_Y_Position();
Arnoud113 3:b353ee86230a 206 double reference_motor1 = atan(y/x);
Arnoud113 3:b353ee86230a 207 int position_Motor1 = motor1_Position();
Arnoud113 3:b353ee86230a 208 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 3:b353ee86230a 209
Arnoud113 3:b353ee86230a 210 pc.baud(115200);
Arnoud113 3:b353ee86230a 211 pc.printf("\r Position(X)=(%f), Ref(Theta,R): (%f,), Pos(Theta,R):(%i,), Motor Value(M1,M2):(%f,).\n",x, reference_motor1, position_Motor1, motor1);
Arnoud113 3:b353ee86230a 212 */
Arnoud113 3:b353ee86230a 213 }
Arnoud113 0:77ad62c61c78 214
Arnoud113 0:77ad62c61c78 215 }