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