first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
main.cpp@15:65f295a49a4b, 2017-11-02 (annotated)
- Committer:
- Arnoud113
- Date:
- Thu Nov 02 17:43:05 2017 +0000
- Revision:
- 15:65f295a49a4b
- Parent:
- 14:54cbd8f0efe4
- Child:
- 16:000f2ebbd16c
fINAL VERSION
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 | 13:b5868fd8ffe9 | 7 | #include "BiQuad.h" |
Arnoud113 | 0:77ad62c61c78 | 8 | |
Arnoud113 | 0:77ad62c61c78 | 9 | |
Arnoud113 | 0:77ad62c61c78 | 10 | |
Arnoud113 | 0:77ad62c61c78 | 11 | DigitalOut gpo(D0); |
Arnoud113 | 0:77ad62c61c78 | 12 | DigitalOut ledb(LED_BLUE); |
Arnoud113 | 0:77ad62c61c78 | 13 | DigitalOut ledr(LED_RED); |
Arnoud113 | 0:77ad62c61c78 | 14 | DigitalOut ledg(LED_GREEN); |
Arnoud113 | 12:02eba9a294d2 | 15 | |
Arnoud113 | 0:77ad62c61c78 | 16 | DigitalOut motor1DC(D7); |
Arnoud113 | 3:b353ee86230a | 17 | DigitalOut motor2DC(D4); |
Arnoud113 | 8:b932f8b71d3a | 18 | FastPWM motor1PWM(D6); |
Arnoud113 | 8:b932f8b71d3a | 19 | FastPWM motor2PWM(D5); |
Arnoud113 | 0:77ad62c61c78 | 20 | |
Arnoud113 | 12:02eba9a294d2 | 21 | AnalogIn potMeter1(A0); |
Arnoud113 | 12:02eba9a294d2 | 22 | AnalogIn potMeter2(A1); |
Arnoud113 | 12:02eba9a294d2 | 23 | AnalogIn emg1(A2); |
Arnoud113 | 12:02eba9a294d2 | 24 | AnalogIn emg2(A3); |
Arnoud113 | 12:02eba9a294d2 | 25 | AnalogIn emg3(A4); |
Arnoud113 | 12:02eba9a294d2 | 26 | AnalogIn emg4(A5); |
Arnoud113 | 12:02eba9a294d2 | 27 | |
Arnoud113 | 12:02eba9a294d2 | 28 | DigitalIn button1(D11); |
Arnoud113 | 12:02eba9a294d2 | 29 | DigitalIn button2(D12); |
Arnoud113 | 12:02eba9a294d2 | 30 | DigitalIn button3(SW2); |
Arnoud113 | 12:02eba9a294d2 | 31 | DigitalIn button4(SW3); |
Arnoud113 | 12:02eba9a294d2 | 32 | Encoder Encoder1(D12,D13); |
Arnoud113 | 12:02eba9a294d2 | 33 | Encoder Encoder2(D8,D9); |
Arnoud113 | 0:77ad62c61c78 | 34 | |
Arnoud113 | 0:77ad62c61c78 | 35 | MODSERIAL pc(USBTX,USBRX); |
Arnoud113 | 0:77ad62c61c78 | 36 | |
Arnoud113 | 0:77ad62c61c78 | 37 | Ticker controller; |
Arnoud113 | 0:77ad62c61c78 | 38 | |
Arnoud113 | 12:02eba9a294d2 | 39 | // Constants EMG ----------------------- Start |
Arnoud113 | 12:02eba9a294d2 | 40 | double X; |
Arnoud113 | 15:65f295a49a4b | 41 | double X_maxTreshold = 450; |
Arnoud113 | 12:02eba9a294d2 | 42 | double X_minTreshold = 20; |
Arnoud113 | 12:02eba9a294d2 | 43 | const double X_Callibrate = 300; |
Arnoud113 | 12:02eba9a294d2 | 44 | |
Arnoud113 | 12:02eba9a294d2 | 45 | double Y; |
Arnoud113 | 15:65f295a49a4b | 46 | double Y_maxTreshold = 450; |
Arnoud113 | 15:65f295a49a4b | 47 | double Y_minTreshold = 0; |
Arnoud113 | 12:02eba9a294d2 | 48 | const double Y_Callibrate = 300; |
Arnoud113 | 12:02eba9a294d2 | 49 | |
Arnoud113 | 12:02eba9a294d2 | 50 | |
Arnoud113 | 3:b353ee86230a | 51 | // ---- Motor Constants------- |
Arnoud113 | 12:02eba9a294d2 | 52 | const double pi = 3.1415926535897; |
Arnoud113 | 12:02eba9a294d2 | 53 | float Pwmperiod = 0.001f; |
Arnoud113 | 12:02eba9a294d2 | 54 | int potmultiplier = 600; // Multiplier for the pot meter reference which is normally between 0 and 1 |
Arnoud113 | 15:65f295a49a4b | 55 | const double gainM1 = 1/29.17; // encoder pulses per degree theta |
Arnoud113 | 15:65f295a49a4b | 56 | const double gainM2 = 1/105.0; // pulses per mm |
Arnoud113 | 0:77ad62c61c78 | 57 | |
Arnoud113 | 12:02eba9a294d2 | 58 | double motor1; |
Arnoud113 | 12:02eba9a294d2 | 59 | double motor2; |
Arnoud113 | 14:54cbd8f0efe4 | 60 | |
Arnoud113 | 14:54cbd8f0efe4 | 61 | double reference_motor1 = 0; // reference for Theta |
Arnoud113 | 14:54cbd8f0efe4 | 62 | double reference_motor2 = 0; |
Arnoud113 | 14:54cbd8f0efe4 | 63 | double pos_M1 = 0; // start angle theta |
Arnoud113 | 14:54cbd8f0efe4 | 64 | double pos_M2 = 0; // start radius |
Arnoud113 | 14:54cbd8f0efe4 | 65 | |
Arnoud113 | 3:b353ee86230a | 66 | |
Arnoud113 | 3:b353ee86230a | 67 | //Start constants PID ------------------------------- |
Arnoud113 | 12:02eba9a294d2 | 68 | 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 | 69 | |
Arnoud113 | 0:77ad62c61c78 | 70 | //verplaatst |
Arnoud113 | 3:b353ee86230a | 71 | const float RAD_PER_PULSE = (2*pi)/4200; |
Arnoud113 | 8:b932f8b71d3a | 72 | const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ |
Arnoud113 | 8:b932f8b71d3a | 73 | |
Arnoud113 | 11:66d0be7efd3f | 74 | const float M1_KP = 10; |
Arnoud113 | 7:88d1ccba9200 | 75 | const float M1_KI = 0.5; |
Arnoud113 | 8:b932f8b71d3a | 76 | const float M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5 |
Arnoud113 | 7:88d1ccba9200 | 77 | double m1_err_int = 0; |
Arnoud113 | 7:88d1ccba9200 | 78 | double m1_prev_err = 0; |
Arnoud113 | 7:88d1ccba9200 | 79 | |
Arnoud113 | 13:b5868fd8ffe9 | 80 | const float M2_KP = 30; |
Arnoud113 | 11:66d0be7efd3f | 81 | const float M2_KI = 0.5; |
Arnoud113 | 7:88d1ccba9200 | 82 | const float M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5 |
Arnoud113 | 7:88d1ccba9200 | 83 | double m2_err_int = 0; |
Arnoud113 | 7:88d1ccba9200 | 84 | double m2_prev_err = 0; |
Arnoud113 | 7:88d1ccba9200 | 85 | |
Arnoud113 | 8:b932f8b71d3a | 86 | // Constants Biquad |
Arnoud113 | 13:b5868fd8ffe9 | 87 | BiQuad bq1(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01); |
Arnoud113 | 13:b5868fd8ffe9 | 88 | BiQuad bq2(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01); |
Arnoud113 | 13:b5868fd8ffe9 | 89 | BiQuadChain bqc; |
Arnoud113 | 13:b5868fd8ffe9 | 90 | |
Arnoud113 | 7:88d1ccba9200 | 91 | const double M1_F_A1 = 1.0; |
Arnoud113 | 8:b932f8b71d3a | 92 | const double M1_F_A2 = 2.0; |
Arnoud113 | 8:b932f8b71d3a | 93 | const double M1_F_B0 = 1.0; |
Arnoud113 | 7:88d1ccba9200 | 94 | const double M1_F_B1 = 3.0; |
Arnoud113 | 7:88d1ccba9200 | 95 | const double M1_F_B2 = 4.0; |
Arnoud113 | 7:88d1ccba9200 | 96 | double m1_f_v1 = 0; |
Arnoud113 | 7:88d1ccba9200 | 97 | double m1_f_v2 = 0; |
Arnoud113 | 12:02eba9a294d2 | 98 | |
Arnoud113 | 12:02eba9a294d2 | 99 | |
Arnoud113 | 12:02eba9a294d2 | 100 | |
Arnoud113 | 5:a1a5b5bebd5c | 101 | //---------------------------------End of constants PID |
Arnoud113 | 0:77ad62c61c78 | 102 | |
Arnoud113 | 3:b353ee86230a | 103 | //-----------------Start PID part----------------------------START |
Arnoud113 | 7:88d1ccba9200 | 104 | 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 | 105 | |
Arnoud113 | 8:b932f8b71d3a | 106 | double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep // Derivative |
Arnoud113 | 13:b5868fd8ffe9 | 107 | |
Arnoud113 | 13:b5868fd8ffe9 | 108 | e_der1 = bqc.step(e_der1); |
Arnoud113 | 7:88d1ccba9200 | 109 | // biquad part, see slide |
Arnoud113 | 7:88d1ccba9200 | 110 | //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); |
Arnoud113 | 8:b932f8b71d3a | 111 | |
Arnoud113 | 7:88d1ccba9200 | 112 | e_prev1 = e1; |
Arnoud113 | 12:02eba9a294d2 | 113 | e_int1 += Ts*e1; |
Arnoud113 | 12:02eba9a294d2 | 114 | |
Arnoud113 | 12:02eba9a294d2 | 115 | if(button3 == 0 || button4 ==0){ |
Arnoud113 | 12:02eba9a294d2 | 116 | e1 = 0; |
Arnoud113 | 12:02eba9a294d2 | 117 | e_prev1 = 0; |
Arnoud113 | 12:02eba9a294d2 | 118 | e_int1 = 0; |
Arnoud113 | 12:02eba9a294d2 | 119 | e_der1 = 0; |
Arnoud113 | 12:02eba9a294d2 | 120 | } |
Arnoud113 | 12:02eba9a294d2 | 121 | // Integral |
Arnoud113 | 8:b932f8b71d3a | 122 | return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1; //PID |
Arnoud113 | 7:88d1ccba9200 | 123 | } |
Arnoud113 | 3:b353ee86230a | 124 | |
Arnoud113 | 7:88d1ccba9200 | 125 | 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 | 126 | |
Arnoud113 | 8:b932f8b71d3a | 127 | double e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep // Derivative |
Arnoud113 | 7:88d1ccba9200 | 128 | // biquad part, see slide |
Arnoud113 | 7:88d1ccba9200 | 129 | //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); |
Arnoud113 | 8:b932f8b71d3a | 130 | |
Arnoud113 | 13:b5868fd8ffe9 | 131 | e_der2 = bqc.step(e_der2); |
Arnoud113 | 13:b5868fd8ffe9 | 132 | |
Arnoud113 | 7:88d1ccba9200 | 133 | e_prev2 = e2; |
Arnoud113 | 12:02eba9a294d2 | 134 | e_int2 += Ts*e2; |
Arnoud113 | 12:02eba9a294d2 | 135 | |
Arnoud113 | 12:02eba9a294d2 | 136 | if(button3 == 0 || button4 ==0){ |
Arnoud113 | 12:02eba9a294d2 | 137 | e2 = 0; |
Arnoud113 | 12:02eba9a294d2 | 138 | e_prev2 = 0; |
Arnoud113 | 12:02eba9a294d2 | 139 | e_int2 = 0; |
Arnoud113 | 12:02eba9a294d2 | 140 | e_der2 = 0; |
Arnoud113 | 12:02eba9a294d2 | 141 | } |
Arnoud113 | 12:02eba9a294d2 | 142 | // Integral |
Arnoud113 | 8:b932f8b71d3a | 143 | return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID |
Arnoud113 | 3:b353ee86230a | 144 | } |
Arnoud113 | 3:b353ee86230a | 145 | |
Arnoud113 | 3:b353ee86230a | 146 | //------------Get reference position-----------------START |
Arnoud113 | 0:77ad62c61c78 | 147 | float Get_X_Position(){ |
Arnoud113 | 12:02eba9a294d2 | 148 | // X = potMeter1 * potmultiplier; //--------- for Potmerter use |
Arnoud113 | 12:02eba9a294d2 | 149 | |
Arnoud113 | 12:02eba9a294d2 | 150 | // -- Potmeter use ----------------------------------- |
Arnoud113 | 15:65f295a49a4b | 151 | |
Arnoud113 | 12:02eba9a294d2 | 152 | if (potMeter1 < 0.3) |
Arnoud113 | 12:02eba9a294d2 | 153 | { |
Arnoud113 | 12:02eba9a294d2 | 154 | X = X-0.5; |
Arnoud113 | 12:02eba9a294d2 | 155 | } |
Arnoud113 | 12:02eba9a294d2 | 156 | else if (potMeter1> 0.7) |
Arnoud113 | 12:02eba9a294d2 | 157 | { |
Arnoud113 | 12:02eba9a294d2 | 158 | X = X+0.5; |
Arnoud113 | 12:02eba9a294d2 | 159 | } |
Arnoud113 | 12:02eba9a294d2 | 160 | else |
Arnoud113 | 12:02eba9a294d2 | 161 | { |
Arnoud113 | 12:02eba9a294d2 | 162 | X = X; |
Arnoud113 | 12:02eba9a294d2 | 163 | } |
Arnoud113 | 15:65f295a49a4b | 164 | |
Arnoud113 | 12:02eba9a294d2 | 165 | |
Arnoud113 | 15:65f295a49a4b | 166 | ///* |
Arnoud113 | 12:02eba9a294d2 | 167 | double in1 = emg1.read(); |
Arnoud113 | 12:02eba9a294d2 | 168 | double in2 = emg2.read(); |
Arnoud113 | 12:02eba9a294d2 | 169 | |
Arnoud113 | 12:02eba9a294d2 | 170 | double RA = in1+in2; |
Arnoud113 | 12:02eba9a294d2 | 171 | |
Arnoud113 | 12:02eba9a294d2 | 172 | |
Arnoud113 | 12:02eba9a294d2 | 173 | if (RA < 0.5) |
Arnoud113 | 12:02eba9a294d2 | 174 | { |
Arnoud113 | 12:02eba9a294d2 | 175 | X = X; |
Arnoud113 | 12:02eba9a294d2 | 176 | } |
Arnoud113 | 12:02eba9a294d2 | 177 | else if (RA > 1.5) |
Arnoud113 | 12:02eba9a294d2 | 178 | { |
Arnoud113 | 15:65f295a49a4b | 179 | X = X-0.4; |
Arnoud113 | 12:02eba9a294d2 | 180 | } |
Arnoud113 | 12:02eba9a294d2 | 181 | else |
Arnoud113 | 12:02eba9a294d2 | 182 | { |
Arnoud113 | 15:65f295a49a4b | 183 | X = X+0.4; |
Arnoud113 | 12:02eba9a294d2 | 184 | } |
Arnoud113 | 15:65f295a49a4b | 185 | // */ |
Arnoud113 | 12:02eba9a294d2 | 186 | |
Arnoud113 | 13:b5868fd8ffe9 | 187 | if (X >= X_maxTreshold){ |
Arnoud113 | 12:02eba9a294d2 | 188 | X = X_maxTreshold; |
Arnoud113 | 12:02eba9a294d2 | 189 | } |
Arnoud113 | 13:b5868fd8ffe9 | 190 | else if (X <= X_minTreshold){ |
Arnoud113 | 12:02eba9a294d2 | 191 | X = X_minTreshold; |
Arnoud113 | 12:02eba9a294d2 | 192 | } |
Arnoud113 | 12:02eba9a294d2 | 193 | else{ |
Arnoud113 | 12:02eba9a294d2 | 194 | X = X; |
Arnoud113 | 12:02eba9a294d2 | 195 | } |
Arnoud113 | 12:02eba9a294d2 | 196 | |
Arnoud113 | 12:02eba9a294d2 | 197 | if(button3 == 0){ |
Arnoud113 | 12:02eba9a294d2 | 198 | X = X_minTreshold; |
Arnoud113 | 12:02eba9a294d2 | 199 | } |
Arnoud113 | 12:02eba9a294d2 | 200 | else if (button4 == 0){ |
Arnoud113 | 12:02eba9a294d2 | 201 | X = X_Callibrate; |
Arnoud113 | 12:02eba9a294d2 | 202 | } |
Arnoud113 | 12:02eba9a294d2 | 203 | else{ |
Arnoud113 | 12:02eba9a294d2 | 204 | X = X; |
Arnoud113 | 12:02eba9a294d2 | 205 | } |
Arnoud113 | 12:02eba9a294d2 | 206 | //pc.baud(115200); |
Arnoud113 | 12:02eba9a294d2 | 207 | //pc.printf("\r (in1,in2):(%f,%f), RA = %f, X = %f \n",in1, in2, RA, X); |
Arnoud113 | 12:02eba9a294d2 | 208 | |
Arnoud113 | 1:13d8940f0fd4 | 209 | return X; |
Arnoud113 | 0:77ad62c61c78 | 210 | } |
Arnoud113 | 0:77ad62c61c78 | 211 | |
Arnoud113 | 0:77ad62c61c78 | 212 | float Get_Y_Position(){ |
Arnoud113 | 12:02eba9a294d2 | 213 | //Y = potMeter2 * potmultiplier; //--------- for Potmerter use |
Arnoud113 | 12:02eba9a294d2 | 214 | |
Arnoud113 | 12:02eba9a294d2 | 215 | // ---- Potmeter Use-------------------------- |
Arnoud113 | 15:65f295a49a4b | 216 | |
Arnoud113 | 12:02eba9a294d2 | 217 | if (potMeter2 < 0.3) |
Arnoud113 | 12:02eba9a294d2 | 218 | { |
Arnoud113 | 12:02eba9a294d2 | 219 | Y = Y-0.5; |
Arnoud113 | 12:02eba9a294d2 | 220 | } |
Arnoud113 | 12:02eba9a294d2 | 221 | else if (potMeter2 > 0.7) |
Arnoud113 | 12:02eba9a294d2 | 222 | { |
Arnoud113 | 12:02eba9a294d2 | 223 | Y = Y+0.5; |
Arnoud113 | 12:02eba9a294d2 | 224 | } |
Arnoud113 | 12:02eba9a294d2 | 225 | else |
Arnoud113 | 12:02eba9a294d2 | 226 | { |
Arnoud113 | 12:02eba9a294d2 | 227 | Y = Y; |
Arnoud113 | 12:02eba9a294d2 | 228 | } |
Arnoud113 | 12:02eba9a294d2 | 229 | |
Arnoud113 | 15:65f295a49a4b | 230 | |
Arnoud113 | 15:65f295a49a4b | 231 | // /* |
Arnoud113 | 12:02eba9a294d2 | 232 | double in3 = emg3.read(); |
Arnoud113 | 12:02eba9a294d2 | 233 | double in4 = emg4.read(); |
Arnoud113 | 12:02eba9a294d2 | 234 | |
Arnoud113 | 12:02eba9a294d2 | 235 | |
Arnoud113 | 12:02eba9a294d2 | 236 | double LA = in3+in4; |
Arnoud113 | 12:02eba9a294d2 | 237 | |
Arnoud113 | 12:02eba9a294d2 | 238 | if (LA < 0.5) |
Arnoud113 | 12:02eba9a294d2 | 239 | { |
Arnoud113 | 12:02eba9a294d2 | 240 | Y = Y; |
Arnoud113 | 12:02eba9a294d2 | 241 | } |
Arnoud113 | 12:02eba9a294d2 | 242 | else if (LA > 1.5) |
Arnoud113 | 12:02eba9a294d2 | 243 | { |
Arnoud113 | 15:65f295a49a4b | 244 | Y = Y-0.4; |
Arnoud113 | 12:02eba9a294d2 | 245 | } |
Arnoud113 | 12:02eba9a294d2 | 246 | else |
Arnoud113 | 12:02eba9a294d2 | 247 | { |
Arnoud113 | 15:65f295a49a4b | 248 | Y = Y+0.4; |
Arnoud113 | 12:02eba9a294d2 | 249 | } |
Arnoud113 | 15:65f295a49a4b | 250 | // */ |
Arnoud113 | 13:b5868fd8ffe9 | 251 | if (Y >= Y_maxTreshold){ |
Arnoud113 | 12:02eba9a294d2 | 252 | Y = Y_maxTreshold; |
Arnoud113 | 12:02eba9a294d2 | 253 | } |
Arnoud113 | 13:b5868fd8ffe9 | 254 | else if (Y <= Y_minTreshold){ |
Arnoud113 | 12:02eba9a294d2 | 255 | Y = Y_minTreshold; |
Arnoud113 | 12:02eba9a294d2 | 256 | } |
Arnoud113 | 12:02eba9a294d2 | 257 | else{ |
Arnoud113 | 12:02eba9a294d2 | 258 | Y = Y; |
Arnoud113 | 12:02eba9a294d2 | 259 | } |
Arnoud113 | 12:02eba9a294d2 | 260 | |
Arnoud113 | 12:02eba9a294d2 | 261 | if(button3 == 0){ |
Arnoud113 | 12:02eba9a294d2 | 262 | Y = Y_minTreshold; |
Arnoud113 | 12:02eba9a294d2 | 263 | } |
Arnoud113 | 12:02eba9a294d2 | 264 | else if (button4 == 0){ |
Arnoud113 | 12:02eba9a294d2 | 265 | Y = Y_Callibrate; |
Arnoud113 | 12:02eba9a294d2 | 266 | } |
Arnoud113 | 12:02eba9a294d2 | 267 | else{ |
Arnoud113 | 12:02eba9a294d2 | 268 | Y = Y; |
Arnoud113 | 12:02eba9a294d2 | 269 | } |
Arnoud113 | 12:02eba9a294d2 | 270 | |
Arnoud113 | 12:02eba9a294d2 | 271 | //pc.printf("\r (in3,in4):(%f,%f), LA = %f, Y = %f \n",in3, in4, LA, Y); |
Arnoud113 | 12:02eba9a294d2 | 272 | |
Arnoud113 | 1:13d8940f0fd4 | 273 | return Y; |
Arnoud113 | 0:77ad62c61c78 | 274 | } |
Arnoud113 | 3:b353ee86230a | 275 | //----------------------------------------------------END |
Arnoud113 | 0:77ad62c61c78 | 276 | |
Arnoud113 | 3:b353ee86230a | 277 | //-------------Get current Position-------------------START |
Arnoud113 | 3:b353ee86230a | 278 | double motor1_Position(){ // has as output Theta |
Arnoud113 | 12:02eba9a294d2 | 279 | |
Arnoud113 | 12:02eba9a294d2 | 280 | if (button3 == 0){ |
Arnoud113 | 12:02eba9a294d2 | 281 | int T1 = ((atan(Y_minTreshold/X_minTreshold)*180)/pi)/gainM1; |
Arnoud113 | 12:02eba9a294d2 | 282 | Encoder1.setPosition(T1); |
Arnoud113 | 12:02eba9a294d2 | 283 | } |
Arnoud113 | 12:02eba9a294d2 | 284 | else if (button4 ==0){ |
Arnoud113 | 12:02eba9a294d2 | 285 | int T1 = ((atan(Y_Callibrate/X_Callibrate)*180)/pi)/gainM1; |
Arnoud113 | 12:02eba9a294d2 | 286 | Encoder1.setPosition(T1); |
Arnoud113 | 12:02eba9a294d2 | 287 | } |
Arnoud113 | 12:02eba9a294d2 | 288 | else{ |
Arnoud113 | 12:02eba9a294d2 | 289 | } |
Arnoud113 | 12:02eba9a294d2 | 290 | double pos_M1 = gainM1*Encoder1.getPosition(); // current position for theta |
Arnoud113 | 14:54cbd8f0efe4 | 291 | double countM1 = Encoder1.getPosition(); |
Arnoud113 | 14:54cbd8f0efe4 | 292 | |
Arnoud113 | 14:54cbd8f0efe4 | 293 | pc.baud(115200); |
Arnoud113 | 15:65f295a49a4b | 294 | //pc.printf("\r counts encoder: %f\n",countM1); |
Arnoud113 | 14:54cbd8f0efe4 | 295 | |
Arnoud113 | 12:02eba9a294d2 | 296 | return pos_M1; |
Arnoud113 | 0:77ad62c61c78 | 297 | } |
Arnoud113 | 3:b353ee86230a | 298 | double motor2_Position(){ //output R |
Arnoud113 | 12:02eba9a294d2 | 299 | int R1; |
Arnoud113 | 12:02eba9a294d2 | 300 | |
Arnoud113 | 12:02eba9a294d2 | 301 | if (button3 == 0){ |
Arnoud113 | 12:02eba9a294d2 | 302 | R1 = (sqrt(X_minTreshold*X_minTreshold+Y_minTreshold*Y_minTreshold))/gainM2; |
Arnoud113 | 12:02eba9a294d2 | 303 | Encoder2.setPosition(R1); |
Arnoud113 | 12:02eba9a294d2 | 304 | } |
Arnoud113 | 12:02eba9a294d2 | 305 | else if (button4 ==0){ |
Arnoud113 | 12:02eba9a294d2 | 306 | R1 = (sqrt(X_Callibrate*X_Callibrate+Y_Callibrate*Y_Callibrate))/gainM2; |
Arnoud113 | 12:02eba9a294d2 | 307 | Encoder2.setPosition(R1); |
Arnoud113 | 12:02eba9a294d2 | 308 | } |
Arnoud113 | 12:02eba9a294d2 | 309 | else{ |
Arnoud113 | 12:02eba9a294d2 | 310 | } |
Arnoud113 | 12:02eba9a294d2 | 311 | |
Arnoud113 | 12:02eba9a294d2 | 312 | double pos_M2 = gainM2 *Encoder2.getPosition(); // current position for the radius; |
Arnoud113 | 15:65f295a49a4b | 313 | double countM2 = Encoder2.getPosition(); |
Arnoud113 | 12:02eba9a294d2 | 314 | pc.baud(115200); |
Arnoud113 | 15:65f295a49a4b | 315 | //pc.printf("\r counts encoder: %f\n",countM2); |
Arnoud113 | 12:02eba9a294d2 | 316 | return pos_M2; |
Arnoud113 | 3:b353ee86230a | 317 | } |
Arnoud113 | 3:b353ee86230a | 318 | //-----------------------------------------------------END |
Arnoud113 | 0:77ad62c61c78 | 319 | |
Arnoud113 | 0:77ad62c61c78 | 320 | |
Arnoud113 | 3:b353ee86230a | 321 | //------------Controller-------------------------------START |
Arnoud113 | 0:77ad62c61c78 | 322 | void Controller(){ |
Arnoud113 | 3:b353ee86230a | 323 | |
Arnoud113 | 1:13d8940f0fd4 | 324 | double x = Get_X_Position(); |
Arnoud113 | 1:13d8940f0fd4 | 325 | double y = Get_Y_Position(); |
Arnoud113 | 3:b353ee86230a | 326 | |
Arnoud113 | 14:54cbd8f0efe4 | 327 | reference_motor1 = (atan(y/x)*180)/pi; // reference for Theta |
Arnoud113 | 14:54cbd8f0efe4 | 328 | reference_motor2 = sqrt((x*x+y*y)); // reference for radius |
Arnoud113 | 0:77ad62c61c78 | 329 | |
Arnoud113 | 14:54cbd8f0efe4 | 330 | pos_M1 = motor1_Position(); // current position for theta |
Arnoud113 | 14:54cbd8f0efe4 | 331 | pos_M2 = motor2_Position(); // current position for the radius |
Arnoud113 | 12:02eba9a294d2 | 332 | |
Arnoud113 | 7:88d1ccba9200 | 333 | 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 | 334 | 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 | 335 | |
Arnoud113 | 3:b353ee86230a | 336 | double dTheta = reference_motor1 - pos_M1; |
Arnoud113 | 3:b353ee86230a | 337 | double dRadius = reference_motor2 - pos_M2; |
Arnoud113 | 0:77ad62c61c78 | 338 | |
Arnoud113 | 0:77ad62c61c78 | 339 | pc.baud(115200); |
Arnoud113 | 13:b5868fd8ffe9 | 340 | //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (delta1,delta2):(%f,%f)\n",x,y, dTheta ,dRadius,delta1, delta2); |
Arnoud113 | 15:65f295a49a4b | 341 | pc.printf("\r DesPosition(X,Y):(%f,%f)\n", x,y); |
Arnoud113 | 14:54cbd8f0efe4 | 342 | //pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2); |
Arnoud113 | 0:77ad62c61c78 | 343 | |
Arnoud113 | 2:2563d1d8461f | 344 | //motor1PWM = motor1; |
Arnoud113 | 2:2563d1d8461f | 345 | //motor2PWM = motor2; |
Arnoud113 | 0:77ad62c61c78 | 346 | |
Arnoud113 | 11:66d0be7efd3f | 347 | if(delta1 > 0.5){ |
Arnoud113 | 3:b353ee86230a | 348 | motor1DC = 0; |
Arnoud113 | 0:77ad62c61c78 | 349 | |
Arnoud113 | 0:77ad62c61c78 | 350 | ledr = 1; |
Arnoud113 | 0:77ad62c61c78 | 351 | ledg = 1; //Blau |
Arnoud113 | 0:77ad62c61c78 | 352 | ledb = 0; |
Arnoud113 | 0:77ad62c61c78 | 353 | } |
Arnoud113 | 11:66d0be7efd3f | 354 | else if (delta1< -0.5) { |
Arnoud113 | 3:b353ee86230a | 355 | motor1DC = 1; |
Arnoud113 | 0:77ad62c61c78 | 356 | |
Arnoud113 | 0:77ad62c61c78 | 357 | ledb = 1; |
Arnoud113 | 0:77ad62c61c78 | 358 | ledr = 1; |
Arnoud113 | 0:77ad62c61c78 | 359 | ledg = 0; //Groen |
Arnoud113 | 0:77ad62c61c78 | 360 | |
Arnoud113 | 0:77ad62c61c78 | 361 | } |
Arnoud113 | 0:77ad62c61c78 | 362 | else{ |
Arnoud113 | 0:77ad62c61c78 | 363 | motor1PWM = 0; |
Arnoud113 | 0:77ad62c61c78 | 364 | |
Arnoud113 | 0:77ad62c61c78 | 365 | ledb = 1; //Rood |
Arnoud113 | 0:77ad62c61c78 | 366 | ledr = 0; |
Arnoud113 | 0:77ad62c61c78 | 367 | ledg = 1; |
Arnoud113 | 0:77ad62c61c78 | 368 | } |
Arnoud113 | 3:b353ee86230a | 369 | |
Arnoud113 | 9:edf01d06935e | 370 | motor1 = abs(delta1)/1000.0; |
Arnoud113 | 15:65f295a49a4b | 371 | if(motor1 >= 0.10) { |
Arnoud113 | 15:65f295a49a4b | 372 | motor1 = 0.10; |
Arnoud113 | 9:edf01d06935e | 373 | //pc.baud(115200); |
Arnoud113 | 9:edf01d06935e | 374 | //pc.printf("\r val motor1: %f\n", motor1); |
Arnoud113 | 11:66d0be7efd3f | 375 | } |
Arnoud113 | 0:77ad62c61c78 | 376 | |
Arnoud113 | 11:66d0be7efd3f | 377 | if(delta2 > 2.0){ |
Arnoud113 | 3:b353ee86230a | 378 | motor2DC = 0; |
Arnoud113 | 0:77ad62c61c78 | 379 | |
Arnoud113 | 8:b932f8b71d3a | 380 | ledr = 1; |
Arnoud113 | 8:b932f8b71d3a | 381 | ledg = 1; //Blau |
Arnoud113 | 8:b932f8b71d3a | 382 | ledb = 0; |
Arnoud113 | 0:77ad62c61c78 | 383 | } |
Arnoud113 | 11:66d0be7efd3f | 384 | else if (delta2<-2.0) { |
Arnoud113 | 3:b353ee86230a | 385 | motor2DC = 1; |
Arnoud113 | 0:77ad62c61c78 | 386 | |
Arnoud113 | 8:b932f8b71d3a | 387 | ledb = 1; |
Arnoud113 | 8:b932f8b71d3a | 388 | ledr = 1; |
Arnoud113 | 8:b932f8b71d3a | 389 | ledg = 0; //Groen |
Arnoud113 | 0:77ad62c61c78 | 390 | |
Arnoud113 | 0:77ad62c61c78 | 391 | } |
Arnoud113 | 0:77ad62c61c78 | 392 | else{ |
Arnoud113 | 0:77ad62c61c78 | 393 | motor2PWM = 0; |
Arnoud113 | 0:77ad62c61c78 | 394 | |
Arnoud113 | 8:b932f8b71d3a | 395 | ledb = 1; //Rood |
Arnoud113 | 8:b932f8b71d3a | 396 | ledr = 0; |
Arnoud113 | 8:b932f8b71d3a | 397 | ledg = 1; |
Arnoud113 | 0:77ad62c61c78 | 398 | } |
Arnoud113 | 3:b353ee86230a | 399 | |
Arnoud113 | 9:edf01d06935e | 400 | motor2 = abs(delta2)/1000.0; |
Arnoud113 | 11:66d0be7efd3f | 401 | if(motor2 >= 0.50) { |
Arnoud113 | 11:66d0be7efd3f | 402 | motor2 = 0.50; |
Arnoud113 | 11:66d0be7efd3f | 403 | } |
Arnoud113 | 3:b353ee86230a | 404 | |
Arnoud113 | 15:65f295a49a4b | 405 | motor1PWM = motor1 + 0.90; |
Arnoud113 | 12:02eba9a294d2 | 406 | motor2PWM = motor2 + 0.50; |
Arnoud113 | 3:b353ee86230a | 407 | |
Arnoud113 | 11:66d0be7efd3f | 408 | //pc.printf("\r delta(1,2):(%f,%f)\n", delta1,delta2); |
Arnoud113 | 12:02eba9a294d2 | 409 | //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n", motor1 + 0.65, motor2 + 0.20); |
Arnoud113 | 3:b353ee86230a | 410 | //pc.printf("\r |
Arnoud113 | 0:77ad62c61c78 | 411 | } |
Arnoud113 | 0:77ad62c61c78 | 412 | |
Arnoud113 | 0:77ad62c61c78 | 413 | int main() |
Arnoud113 | 0:77ad62c61c78 | 414 | { |
Arnoud113 | 0:77ad62c61c78 | 415 | controller.attach(&Controller, M1_TS); |
Arnoud113 | 10:4b0b4f2abacf | 416 | motor1PWM.period(Pwmperiod); |
Arnoud113 | 10:4b0b4f2abacf | 417 | motor2PWM.period(Pwmperiod); |
Arnoud113 | 0:77ad62c61c78 | 418 | |
Arnoud113 | 13:b5868fd8ffe9 | 419 | bqc.add(&bq1).add(&bq2); |
Arnoud113 | 13:b5868fd8ffe9 | 420 | |
Arnoud113 | 3:b353ee86230a | 421 | while(1){ |
Arnoud113 | 3:b353ee86230a | 422 | /* |
Arnoud113 | 3:b353ee86230a | 423 | double x = Get_X_Position(); |
Arnoud113 | 3:b353ee86230a | 424 | double y = Get_Y_Position(); |
Arnoud113 | 3:b353ee86230a | 425 | double reference_motor1 = atan(y/x); |
Arnoud113 | 3:b353ee86230a | 426 | int position_Motor1 = motor1_Position(); |
Arnoud113 | 3:b353ee86230a | 427 | 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 | 428 | |
Arnoud113 | 3:b353ee86230a | 429 | pc.baud(115200); |
Arnoud113 | 3:b353ee86230a | 430 | 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 | 431 | */ |
Arnoud113 | 3:b353ee86230a | 432 | } |
Arnoud113 | 0:77ad62c61c78 | 433 | |
Arnoud113 | 0:77ad62c61c78 | 434 | } |