first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
main.cpp@9:edf01d06935e, 2017-11-01 (annotated)
- Committer:
- Arnoud113
- Date:
- Wed Nov 01 10:00:10 2017 +0000
- Revision:
- 9:edf01d06935e
- Parent:
- 8:b932f8b71d3a
- Child:
- 10:4b0b4f2abacf
voor jerome
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:b353ee86230a | 31 | float Pwmperiod = 0.0001f; |
Arnoud113 | 9:edf01d06935e | 32 | int potmultiplier = 36000; // 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 | 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 | 9:edf01d06935e | 47 | const float M1_KP = 20; |
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 | 8:b932f8b71d3a | 53 | const float M2_KP = 10; |
Arnoud113 | 7:88d1ccba9200 | 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 | 9:edf01d06935e | 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 | 8:b932f8b71d3a | 140 | if(delta1 > 10.0){ |
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 | 8:b932f8b71d3a | 147 | else if (delta1< -10.0) { |
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 | 9:edf01d06935e | 164 | //if(motor1 >= 0.50f) { |
Arnoud113 | 9:edf01d06935e | 165 | // motor1 = 0.50f; |
Arnoud113 | 9:edf01d06935e | 166 | //pc.baud(115200); |
Arnoud113 | 9:edf01d06935e | 167 | //pc.printf("\r val motor1: %f\n", motor1); |
Arnoud113 | 9:edf01d06935e | 168 | // } |
Arnoud113 | 0:77ad62c61c78 | 169 | |
Arnoud113 | 3:b353ee86230a | 170 | if(delta2 > 10.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 | 3:b353ee86230a | 177 | else if (delta2<-10.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 | 9:edf01d06935e | 194 | //if(motor2 >= 0.50f) { |
Arnoud113 | 9:edf01d06935e | 195 | // motor2 = 0.50f; |
Arnoud113 | 9:edf01d06935e | 196 | // } |
Arnoud113 | 3:b353ee86230a | 197 | |
Arnoud113 | 9:edf01d06935e | 198 | motor1PWM = motor1*2; // + 0.50f; |
Arnoud113 | 9:edf01d06935e | 199 | motor2PWM = motor2; //+ 0.50f; |
Arnoud113 | 3:b353ee86230a | 200 | |
Arnoud113 | 9:edf01d06935e | 201 | //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", motor1PWM, motor2PWM); |
Arnoud113 | 3:b353ee86230a | 202 | //pc.printf("\r |
Arnoud113 | 0:77ad62c61c78 | 203 | } |
Arnoud113 | 0:77ad62c61c78 | 204 | |
Arnoud113 | 0:77ad62c61c78 | 205 | int main() |
Arnoud113 | 0:77ad62c61c78 | 206 | { |
Arnoud113 | 0:77ad62c61c78 | 207 | controller.attach(&Controller, M1_TS); |
Arnoud113 | 3:b353ee86230a | 208 | //motor1PWM.period(Pwmperiod); |
Arnoud113 | 3:b353ee86230a | 209 | //motor2PWM.period(Pwmperiod); |
Arnoud113 | 0:77ad62c61c78 | 210 | |
Arnoud113 | 3:b353ee86230a | 211 | while(1){ |
Arnoud113 | 3:b353ee86230a | 212 | /* |
Arnoud113 | 3:b353ee86230a | 213 | double x = Get_X_Position(); |
Arnoud113 | 3:b353ee86230a | 214 | double y = Get_Y_Position(); |
Arnoud113 | 3:b353ee86230a | 215 | double reference_motor1 = atan(y/x); |
Arnoud113 | 3:b353ee86230a | 216 | int position_Motor1 = motor1_Position(); |
Arnoud113 | 3:b353ee86230a | 217 | 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 | 218 | |
Arnoud113 | 3:b353ee86230a | 219 | pc.baud(115200); |
Arnoud113 | 3:b353ee86230a | 220 | 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 | 221 | */ |
Arnoud113 | 3:b353ee86230a | 222 | } |
Arnoud113 | 0:77ad62c61c78 | 223 | |
Arnoud113 | 0:77ad62c61c78 | 224 | } |