Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@11:66d0be7efd3f, 2017-11-01 (annotated)

Committer:
Arnoud113
Date:
Wed Nov 01 15:30:59 2017 +0000
Revision:
11:66d0be7efd3f
Parent:
10:4b0b4f2abacf
Child:
12:02eba9a294d2
quite well working version with pot meters

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 8:b932f8b71d3a 16 FastPWM motor1PWM(D6);
Arnoud113 8:b932f8b71d3a 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 10:4b0b4f2abacf 31 float Pwmperiod = 0.001f;
Arnoud113 10:4b0b4f2abacf 32 int potmultiplier = 600; // 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 10:4b0b4f2abacf 34 float gainM2 = 1/109.4; // gain for radius r
Arnoud113 0:77ad62c61c78 35
Arnoud113 3:b353ee86230a 36 volatile float motor1;
Arnoud113 3:b353ee86230a 37 volatile float motor2;
Arnoud113 3:b353ee86230a 38
Arnoud113 3:b353ee86230a 39 //Start constants PID -------------------------------
Arnoud113 0:77ad62c61c78 40 const double pi = 3.1415926535897;
Arnoud113 8:b932f8b71d3a 41 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 42
Arnoud113 0:77ad62c61c78 43 //verplaatst
Arnoud113 3:b353ee86230a 44 const float RAD_PER_PULSE = (2*pi)/4200;
Arnoud113 8:b932f8b71d3a 45 const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ
Arnoud113 8:b932f8b71d3a 46
Arnoud113 11:66d0be7efd3f 47 const float M1_KP = 10;
Arnoud113 7:88d1ccba9200 48 const float M1_KI = 0.5;
Arnoud113 8:b932f8b71d3a 49 const float M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
Arnoud113 7:88d1ccba9200 50 double m1_err_int = 0;
Arnoud113 7:88d1ccba9200 51 double m1_prev_err = 0;
Arnoud113 7:88d1ccba9200 52
Arnoud113 11:66d0be7efd3f 53 const float M2_KP = 10;
Arnoud113 11:66d0be7efd3f 54 const float M2_KI = 0.5;
Arnoud113 7:88d1ccba9200 55 const float M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
Arnoud113 7:88d1ccba9200 56 double m2_err_int = 0;
Arnoud113 7:88d1ccba9200 57 double m2_prev_err = 0;
Arnoud113 7:88d1ccba9200 58
Arnoud113 8:b932f8b71d3a 59 // Constants Biquad
Arnoud113 7:88d1ccba9200 60 const double M1_F_A1 = 1.0;
Arnoud113 8:b932f8b71d3a 61 const double M1_F_A2 = 2.0;
Arnoud113 8:b932f8b71d3a 62 const double M1_F_B0 = 1.0;
Arnoud113 7:88d1ccba9200 63 const double M1_F_B1 = 3.0;
Arnoud113 7:88d1ccba9200 64 const double M1_F_B2 = 4.0;
Arnoud113 7:88d1ccba9200 65 double m1_f_v1 = 0;
Arnoud113 7:88d1ccba9200 66 double m1_f_v2 = 0;
Arnoud113 5:a1a5b5bebd5c 67 //---------------------------------End of constants PID
Arnoud113 0:77ad62c61c78 68
Arnoud113 3:b353ee86230a 69 //-----------------Start PID part----------------------------START
Arnoud113 7:88d1ccba9200 70 double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1, 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 71
Arnoud113 8:b932f8b71d3a 72 double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep // Derivative
Arnoud113 7:88d1ccba9200 73 // biquad part, see slide
Arnoud113 7:88d1ccba9200 74 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
Arnoud113 8:b932f8b71d3a 75
Arnoud113 7:88d1ccba9200 76 e_prev1 = e1;
Arnoud113 8:b932f8b71d3a 77 e_int1 += Ts*e1; // Integral
Arnoud113 8:b932f8b71d3a 78 return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1; //PID
Arnoud113 7:88d1ccba9200 79 }
Arnoud113 3:b353ee86230a 80
Arnoud113 7:88d1ccba9200 81 double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2, 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 7:88d1ccba9200 82
Arnoud113 8:b932f8b71d3a 83 double e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep // Derivative
Arnoud113 7:88d1ccba9200 84 // biquad part, see slide
Arnoud113 7:88d1ccba9200 85 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
Arnoud113 8:b932f8b71d3a 86
Arnoud113 7:88d1ccba9200 87 e_prev2 = e2;
Arnoud113 8:b932f8b71d3a 88 e_int2 += Ts*e2; // Integral
Arnoud113 8:b932f8b71d3a 89 return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID
Arnoud113 3:b353ee86230a 90 }
Arnoud113 3:b353ee86230a 91
Arnoud113 3:b353ee86230a 92 //------------Get reference position-----------------START
Arnoud113 0:77ad62c61c78 93 float Get_X_Position(){
Arnoud113 1:13d8940f0fd4 94 double X = potMeter1 * potmultiplier;
Arnoud113 1:13d8940f0fd4 95 return X;
Arnoud113 0:77ad62c61c78 96 }
Arnoud113 0:77ad62c61c78 97
Arnoud113 0:77ad62c61c78 98 float Get_Y_Position(){
Arnoud113 1:13d8940f0fd4 99 double Y = potMeter2 * potmultiplier;
Arnoud113 1:13d8940f0fd4 100 return Y;
Arnoud113 0:77ad62c61c78 101 }
Arnoud113 3:b353ee86230a 102 //----------------------------------------------------END
Arnoud113 0:77ad62c61c78 103
Arnoud113 3:b353ee86230a 104 //-------------Get current Position-------------------START
Arnoud113 3:b353ee86230a 105 double motor1_Position(){ // has as output Theta
Arnoud113 5:a1a5b5bebd5c 106 double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta
Arnoud113 3:b353ee86230a 107 return pos_m1;
Arnoud113 0:77ad62c61c78 108 }
Arnoud113 3:b353ee86230a 109 double motor2_Position(){ //output R
Arnoud113 3:b353ee86230a 110 double pos_m2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
Arnoud113 3:b353ee86230a 111 return pos_m2;
Arnoud113 3:b353ee86230a 112 }
Arnoud113 3:b353ee86230a 113 //-----------------------------------------------------END
Arnoud113 0:77ad62c61c78 114
Arnoud113 0:77ad62c61c78 115
Arnoud113 3:b353ee86230a 116 //------------Controller-------------------------------START
Arnoud113 0:77ad62c61c78 117 void Controller(){
Arnoud113 3:b353ee86230a 118
Arnoud113 1:13d8940f0fd4 119 double x = Get_X_Position();
Arnoud113 1:13d8940f0fd4 120 double y = Get_Y_Position();
Arnoud113 3:b353ee86230a 121
Arnoud113 3:b353ee86230a 122 double reference_motor1 = (atan(y/x)*180)/pi; // reference for Theta
Arnoud113 3:b353ee86230a 123 double reference_motor2 = sqrt((x*x+y*y)); // reference for radius
Arnoud113 0:77ad62c61c78 124
Arnoud113 3:b353ee86230a 125 float pos_M1 = motor1_Position(); // current position for theta
Arnoud113 3:b353ee86230a 126 float pos_M2 = motor2_Position(); // current position for the radius
Arnoud113 0:77ad62c61c78 127
Arnoud113 7:88d1ccba9200 128 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 7:88d1ccba9200 129 double delta2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_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 130
Arnoud113 3:b353ee86230a 131 double dTheta = reference_motor1 - pos_M1;
Arnoud113 3:b353ee86230a 132 double dRadius = reference_motor2 - pos_M2;
Arnoud113 0:77ad62c61c78 133
Arnoud113 0:77ad62c61c78 134 pc.baud(115200);
Arnoud113 11:66d0be7efd3f 135 pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius);
Arnoud113 0:77ad62c61c78 136
Arnoud113 2:2563d1d8461f 137 //motor1PWM = motor1;
Arnoud113 2:2563d1d8461f 138 //motor2PWM = motor2;
Arnoud113 0:77ad62c61c78 139
Arnoud113 11:66d0be7efd3f 140 if(delta1 > 0.5){
Arnoud113 3:b353ee86230a 141 motor1DC = 0;
Arnoud113 0:77ad62c61c78 142
Arnoud113 0:77ad62c61c78 143 ledr = 1;
Arnoud113 0:77ad62c61c78 144 ledg = 1; //Blau
Arnoud113 0:77ad62c61c78 145 ledb = 0;
Arnoud113 0:77ad62c61c78 146 }
Arnoud113 11:66d0be7efd3f 147 else if (delta1< -0.5) {
Arnoud113 3:b353ee86230a 148 motor1DC = 1;
Arnoud113 0:77ad62c61c78 149
Arnoud113 0:77ad62c61c78 150 ledb = 1;
Arnoud113 0:77ad62c61c78 151 ledr = 1;
Arnoud113 0:77ad62c61c78 152 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 153
Arnoud113 0:77ad62c61c78 154 }
Arnoud113 0:77ad62c61c78 155 else{
Arnoud113 0:77ad62c61c78 156 motor1PWM = 0;
Arnoud113 0:77ad62c61c78 157
Arnoud113 0:77ad62c61c78 158 ledb = 1; //Rood
Arnoud113 0:77ad62c61c78 159 ledr = 0;
Arnoud113 0:77ad62c61c78 160 ledg = 1;
Arnoud113 0:77ad62c61c78 161 }
Arnoud113 3:b353ee86230a 162
Arnoud113 9:edf01d06935e 163 motor1 = abs(delta1)/1000.0;
Arnoud113 11:66d0be7efd3f 164 if(motor1 >= 0.50) {
Arnoud113 11:66d0be7efd3f 165 motor1 = 0.50;
Arnoud113 9:edf01d06935e 166 //pc.baud(115200);
Arnoud113 9:edf01d06935e 167 //pc.printf("\r val motor1: %f\n", motor1);
Arnoud113 11:66d0be7efd3f 168 }
Arnoud113 0:77ad62c61c78 169
Arnoud113 11:66d0be7efd3f 170 if(delta2 > 2.0){
Arnoud113 3:b353ee86230a 171 motor2DC = 0;
Arnoud113 0:77ad62c61c78 172
Arnoud113 8:b932f8b71d3a 173 ledr = 1;
Arnoud113 8:b932f8b71d3a 174 ledg = 1; //Blau
Arnoud113 8:b932f8b71d3a 175 ledb = 0;
Arnoud113 0:77ad62c61c78 176 }
Arnoud113 11:66d0be7efd3f 177 else if (delta2<-2.0) {
Arnoud113 3:b353ee86230a 178 motor2DC = 1;
Arnoud113 0:77ad62c61c78 179
Arnoud113 8:b932f8b71d3a 180 ledb = 1;
Arnoud113 8:b932f8b71d3a 181 ledr = 1;
Arnoud113 8:b932f8b71d3a 182 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 183
Arnoud113 0:77ad62c61c78 184 }
Arnoud113 0:77ad62c61c78 185 else{
Arnoud113 0:77ad62c61c78 186 motor2PWM = 0;
Arnoud113 0:77ad62c61c78 187
Arnoud113 8:b932f8b71d3a 188 ledb = 1; //Rood
Arnoud113 8:b932f8b71d3a 189 ledr = 0;
Arnoud113 8:b932f8b71d3a 190 ledg = 1;
Arnoud113 0:77ad62c61c78 191 }
Arnoud113 3:b353ee86230a 192
Arnoud113 9:edf01d06935e 193 motor2 = abs(delta2)/1000.0;
Arnoud113 11:66d0be7efd3f 194 if(motor2 >= 0.50) {
Arnoud113 11:66d0be7efd3f 195 motor2 = 0.50;
Arnoud113 11:66d0be7efd3f 196 }
Arnoud113 3:b353ee86230a 197
Arnoud113 11:66d0be7efd3f 198 motor1PWM = motor1 + 0.20;
Arnoud113 11:66d0be7efd3f 199 motor2PWM = motor2 + 0.20;
Arnoud113 3:b353ee86230a 200
Arnoud113 11:66d0be7efd3f 201 //pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2);
Arnoud113 11:66d0be7efd3f 202 //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n", motor1, motor2);
Arnoud113 3:b353ee86230a 203 //pc.printf("\r
Arnoud113 0:77ad62c61c78 204 }
Arnoud113 0:77ad62c61c78 205
Arnoud113 0:77ad62c61c78 206 int main()
Arnoud113 0:77ad62c61c78 207 {
Arnoud113 0:77ad62c61c78 208 controller.attach(&Controller, M1_TS);
Arnoud113 10:4b0b4f2abacf 209 motor1PWM.period(Pwmperiod);
Arnoud113 10:4b0b4f2abacf 210 motor2PWM.period(Pwmperiod);
Arnoud113 0:77ad62c61c78 211
Arnoud113 3:b353ee86230a 212 while(1){
Arnoud113 3:b353ee86230a 213 /*
Arnoud113 3:b353ee86230a 214 double x = Get_X_Position();
Arnoud113 3:b353ee86230a 215 double y = Get_Y_Position();
Arnoud113 3:b353ee86230a 216 double reference_motor1 = atan(y/x);
Arnoud113 3:b353ee86230a 217 int position_Motor1 = motor1_Position();
Arnoud113 3:b353ee86230a 218 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 219
Arnoud113 3:b353ee86230a 220 pc.baud(115200);
Arnoud113 3:b353ee86230a 221 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 222 */
Arnoud113 3:b353ee86230a 223 }
Arnoud113 0:77ad62c61c78 224
Arnoud113 0:77ad62c61c78 225 }