Code om de PID controller af te stellen aan de hand van een sinus golf
Dependencies: mbed QEI MODSERIAL FastPWM biquadFilter
main.cpp@17:190c86f4828f, 2019-04-17 (annotated)
- Committer:
- aschut
- Date:
- Wed Apr 17 12:17:51 2019 +0000
- Revision:
- 17:190c86f4828f
- Parent:
- 14:30d32f51dd9f
- Child:
- 18:161b7ca7f98d
sinus
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aschut | 0:a9a42914138c | 1 | #include "mbed.h" |
aschut | 2:926d56babb1a | 2 | #include "MODSERIAL.h" |
aschut | 4:99f7fdce608e | 3 | #include "QEI.h" |
aschut | 6:14a9c4f30c86 | 4 | #include "BiQuad.h" |
aschut | 13:b7a1a4245f37 | 5 | #include "FastPWM.h" |
aschut | 4:99f7fdce608e | 6 | // Algemeen |
aschut | 17:190c86f4828f | 7 | Timer t; |
aschut | 1:58f34947c674 | 8 | DigitalIn button2(SW2); |
aschut | 1:58f34947c674 | 9 | DigitalIn button3(SW3); |
aschut | 17:190c86f4828f | 10 | DigitalIn But2(D12); |
aschut | 17:190c86f4828f | 11 | DigitalIn But1(D13); |
aschut | 2:926d56babb1a | 12 | DigitalOut led(LED_GREEN); |
aschut | 2:926d56babb1a | 13 | DigitalOut led2(LED_RED); |
aschut | 4:99f7fdce608e | 14 | DigitalOut led3(LED_BLUE); |
aschut | 2:926d56babb1a | 15 | MODSERIAL pc(USBTX, USBRX); |
aschut | 17:190c86f4828f | 16 | float A; |
aschut | 17:190c86f4828f | 17 | int i = 0; |
aschut | 17:190c86f4828f | 18 | float pi = 3.14159265359; |
aschut | 17:190c86f4828f | 19 | |
aschut | 2:926d56babb1a | 20 | |
aschut | 4:99f7fdce608e | 21 | //Motoren |
aschut | 0:a9a42914138c | 22 | DigitalOut direction1(D4); |
aschut | 13:b7a1a4245f37 | 23 | FastPWM pwmpin1(D5); |
aschut | 13:b7a1a4245f37 | 24 | FastPWM pwmpin2(D6); |
aschut | 0:a9a42914138c | 25 | DigitalOut direction2(D7); |
aschut | 9:aa5d6636197b | 26 | volatile float pwm1; |
aschut | 4:99f7fdce608e | 27 | volatile float pwm2; |
aschut | 4:99f7fdce608e | 28 | |
aschut | 4:99f7fdce608e | 29 | //Encoder |
aschut | 17:190c86f4828f | 30 | QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING); |
aschut | 17:190c86f4828f | 31 | QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING); |
aschut | 9:aa5d6636197b | 32 | double Pulses1; |
aschut | 9:aa5d6636197b | 33 | double motor_position1; |
aschut | 6:14a9c4f30c86 | 34 | double Pulses2; |
aschut | 6:14a9c4f30c86 | 35 | double motor_position2; |
aschut | 9:aa5d6636197b | 36 | double error1; |
aschut | 2:926d56babb1a | 37 | |
aschut | 2:926d56babb1a | 38 | //Pot meter |
aschut | 17:190c86f4828f | 39 | AnalogIn pot(A5); |
aschut | 6:14a9c4f30c86 | 40 | AnalogIn pot0(A0); |
aschut | 4:99f7fdce608e | 41 | float Pot2; |
aschut | 6:14a9c4f30c86 | 42 | float Pot1; |
aschut | 4:99f7fdce608e | 43 | |
aschut | 4:99f7fdce608e | 44 | //Ticker |
aschut | 4:99f7fdce608e | 45 | Ticker Pwm; |
aschut | 2:926d56babb1a | 46 | Ticker PotRead; |
aschut | 7:9a1007e35bac | 47 | Ticker Kdc; |
aschut | 6:14a9c4f30c86 | 48 | |
aschut | 0:a9a42914138c | 49 | |
aschut | 5:4b25551aeb6e | 50 | //Kinematica |
aschut | 9:aa5d6636197b | 51 | double stap1; |
aschut | 9:aa5d6636197b | 52 | double stap2; |
aschut | 6:14a9c4f30c86 | 53 | double KPot; |
aschut | 5:4b25551aeb6e | 54 | float KPotabs; |
aschut | 9:aa5d6636197b | 55 | |
aschut | 5:4b25551aeb6e | 56 | float ElbowReference; |
aschut | 5:4b25551aeb6e | 57 | float Ellebooghoek1; |
aschut | 5:4b25551aeb6e | 58 | float Ellebooghoek2; |
aschut | 5:4b25551aeb6e | 59 | float Ellebooghoek3; |
aschut | 5:4b25551aeb6e | 60 | float Ellebooghoek4; |
aschut | 9:aa5d6636197b | 61 | |
aschut | 9:aa5d6636197b | 62 | float PolsReference; |
aschut | 9:aa5d6636197b | 63 | float Polshoek1; |
aschut | 9:aa5d6636197b | 64 | float Polshoek2; |
aschut | 9:aa5d6636197b | 65 | float Polshoek3; |
aschut | 9:aa5d6636197b | 66 | float Polshoek4; |
aschut | 9:aa5d6636197b | 67 | |
aschut | 9:aa5d6636197b | 68 | float Hoeknieuw1; |
aschut | 9:aa5d6636197b | 69 | float Hoeknieuw2; |
aschut | 5:4b25551aeb6e | 70 | |
aschut | 5:4b25551aeb6e | 71 | //Limiet in graden |
aschut | 13:b7a1a4245f37 | 72 | float lowerlim1 = -900; |
aschut | 13:b7a1a4245f37 | 73 | float upperlim1 = 900; |
aschut | 9:aa5d6636197b | 74 | float lowerlim2 = 0; |
aschut | 9:aa5d6636197b | 75 | float upperlim2 = 748.8; //40% van 1 ronde van het grote tandwiel is 2,08 rondes van de motor |
aschut | 2:926d56babb1a | 76 | |
aschut | 6:14a9c4f30c86 | 77 | // VARIABLES PID CONTROLLER |
aschut | 14:30d32f51dd9f | 78 | double Kp1 = 0; |
aschut | 9:aa5d6636197b | 79 | double Ki1 = 0; |
aschut | 9:aa5d6636197b | 80 | double Kd1 = 1; |
aschut | 9:aa5d6636197b | 81 | double Kp2 = 6; // Zonder arm: 6,0,1 |
aschut | 9:aa5d6636197b | 82 | double Ki2 = 0; |
aschut | 9:aa5d6636197b | 83 | double Kd2 = 1; |
aschut | 11:2375b538b631 | 84 | double Ts = 0.0005; // Sample time in seconds |
aschut | 6:14a9c4f30c86 | 85 | |
aschut | 9:aa5d6636197b | 86 | float Kinematics1(float KPot) |
aschut | 6:14a9c4f30c86 | 87 | { |
aschut | 6:14a9c4f30c86 | 88 | |
aschut | 6:14a9c4f30c86 | 89 | if (KPot > 0.45f) { |
aschut | 13:b7a1a4245f37 | 90 | stap1 = KPot*450*Ts; |
aschut | 9:aa5d6636197b | 91 | Hoeknieuw1 = PolsReference + stap1; |
aschut | 9:aa5d6636197b | 92 | return Hoeknieuw1; |
aschut | 5:4b25551aeb6e | 93 | } |
aschut | 4:99f7fdce608e | 94 | |
aschut | 6:14a9c4f30c86 | 95 | else if (KPot < -0.45f) { |
aschut | 13:b7a1a4245f37 | 96 | stap1 = KPot*450*Ts; |
aschut | 9:aa5d6636197b | 97 | Hoeknieuw1 = PolsReference + stap1; |
aschut | 9:aa5d6636197b | 98 | return Hoeknieuw1; |
aschut | 9:aa5d6636197b | 99 | } |
aschut | 9:aa5d6636197b | 100 | |
aschut | 9:aa5d6636197b | 101 | else { |
aschut | 9:aa5d6636197b | 102 | return PolsReference; |
aschut | 9:aa5d6636197b | 103 | } |
aschut | 9:aa5d6636197b | 104 | } |
aschut | 9:aa5d6636197b | 105 | float Kinematics2(float KPot) |
aschut | 9:aa5d6636197b | 106 | { |
aschut | 9:aa5d6636197b | 107 | |
aschut | 9:aa5d6636197b | 108 | if (KPot > 0.45f) { |
aschut | 9:aa5d6636197b | 109 | stap2 = KPot*150*Ts; // 144 graden van de arm in 5 seconden |
aschut | 9:aa5d6636197b | 110 | Hoeknieuw2 = ElbowReference + stap2; |
aschut | 9:aa5d6636197b | 111 | return Hoeknieuw2; |
aschut | 9:aa5d6636197b | 112 | } |
aschut | 9:aa5d6636197b | 113 | |
aschut | 9:aa5d6636197b | 114 | else if (KPot < -0.45f) { |
aschut | 9:aa5d6636197b | 115 | stap2 = KPot*150*Ts; |
aschut | 9:aa5d6636197b | 116 | Hoeknieuw2 = ElbowReference + stap2; |
aschut | 9:aa5d6636197b | 117 | return Hoeknieuw2; |
aschut | 6:14a9c4f30c86 | 118 | } |
aschut | 6:14a9c4f30c86 | 119 | |
aschut | 6:14a9c4f30c86 | 120 | else { |
aschut | 6:14a9c4f30c86 | 121 | return ElbowReference; |
aschut | 6:14a9c4f30c86 | 122 | } |
aschut | 6:14a9c4f30c86 | 123 | } |
aschut | 6:14a9c4f30c86 | 124 | |
aschut | 9:aa5d6636197b | 125 | float Limits1(float Polshoek2) |
aschut | 6:14a9c4f30c86 | 126 | { |
aschut | 6:14a9c4f30c86 | 127 | |
aschut | 9:aa5d6636197b | 128 | if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten |
aschut | 9:aa5d6636197b | 129 | Polshoek3 = Polshoek2; |
aschut | 9:aa5d6636197b | 130 | } |
aschut | 9:aa5d6636197b | 131 | |
aschut | 9:aa5d6636197b | 132 | else { |
aschut | 9:aa5d6636197b | 133 | if (Polshoek2 >= upperlim1) { //Boven de limiet |
aschut | 9:aa5d6636197b | 134 | Polshoek3 = upperlim1; |
aschut | 9:aa5d6636197b | 135 | } else { //Onder de limiet |
aschut | 9:aa5d6636197b | 136 | Polshoek3 = lowerlim1; |
aschut | 9:aa5d6636197b | 137 | } |
aschut | 9:aa5d6636197b | 138 | } |
aschut | 9:aa5d6636197b | 139 | |
aschut | 9:aa5d6636197b | 140 | return Polshoek3; |
aschut | 9:aa5d6636197b | 141 | } |
aschut | 9:aa5d6636197b | 142 | |
aschut | 9:aa5d6636197b | 143 | |
aschut | 9:aa5d6636197b | 144 | float Limits2(float Ellebooghoek2) |
aschut | 9:aa5d6636197b | 145 | { |
aschut | 9:aa5d6636197b | 146 | |
aschut | 9:aa5d6636197b | 147 | if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten |
aschut | 5:4b25551aeb6e | 148 | Ellebooghoek3 = Ellebooghoek2; |
aschut | 6:14a9c4f30c86 | 149 | } |
aschut | 6:14a9c4f30c86 | 150 | |
aschut | 5:4b25551aeb6e | 151 | else { |
aschut | 9:aa5d6636197b | 152 | if (Ellebooghoek2 >= upperlim2) { //Boven de limiet |
aschut | 9:aa5d6636197b | 153 | Ellebooghoek3 = upperlim2; |
aschut | 6:14a9c4f30c86 | 154 | } else { //Onder de limiet |
aschut | 9:aa5d6636197b | 155 | Ellebooghoek3 = lowerlim2; |
aschut | 5:4b25551aeb6e | 156 | } |
aschut | 5:4b25551aeb6e | 157 | } |
aschut | 6:14a9c4f30c86 | 158 | |
aschut | 5:4b25551aeb6e | 159 | return Ellebooghoek3; |
aschut | 6:14a9c4f30c86 | 160 | } |
aschut | 6:14a9c4f30c86 | 161 | |
aschut | 6:14a9c4f30c86 | 162 | |
aschut | 4:99f7fdce608e | 163 | |
aschut | 0:a9a42914138c | 164 | |
aschut | 6:14a9c4f30c86 | 165 | // ~~~~~~~~~~~~~~PID CONTROLLER~~~~~~~~~~~~~~~~~~ |
aschut | 9:aa5d6636197b | 166 | double PID_controller1(double error1) |
aschut | 6:14a9c4f30c86 | 167 | { |
aschut | 9:aa5d6636197b | 168 | static double error1_integral = 0; |
aschut | 9:aa5d6636197b | 169 | static double error1_prev = error1; // initialization with this value only done once! |
aschut | 6:14a9c4f30c86 | 170 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); |
aschut | 7:9a1007e35bac | 171 | |
aschut | 14:30d32f51dd9f | 172 | //PID testing |
aschut | 14:30d32f51dd9f | 173 | Kp1 = 30 * Pot2; |
aschut | 9:aa5d6636197b | 174 | Ki1 = 10 * Pot1; |
aschut | 7:9a1007e35bac | 175 | |
aschut | 7:9a1007e35bac | 176 | if (!But2) { |
aschut | 14:30d32f51dd9f | 177 | Kd1 = Kd1 + 0.001; |
aschut | 7:9a1007e35bac | 178 | } |
aschut | 7:9a1007e35bac | 179 | if (!But1){ |
aschut | 14:30d32f51dd9f | 180 | Kd1 = Kd1 - 0.001; |
aschut | 7:9a1007e35bac | 181 | } |
aschut | 14:30d32f51dd9f | 182 | |
aschut | 7:9a1007e35bac | 183 | |
aschut | 6:14a9c4f30c86 | 184 | // Proportional part: |
aschut | 9:aa5d6636197b | 185 | double u_k1 = Kp1 * error1; |
aschut | 6:14a9c4f30c86 | 186 | |
aschut | 6:14a9c4f30c86 | 187 | // Integral part |
aschut | 9:aa5d6636197b | 188 | error1_integral = error1_integral + error1 * Ts; |
aschut | 9:aa5d6636197b | 189 | double u_i1 = Ki1* error1_integral; |
aschut | 6:14a9c4f30c86 | 190 | |
aschut | 6:14a9c4f30c86 | 191 | // Derivative part |
aschut | 9:aa5d6636197b | 192 | double error1_derivative = (error1 - error1_prev)/Ts; |
aschut | 9:aa5d6636197b | 193 | double filtered_error1_derivative = LowPassFilter.step(error1_derivative); |
aschut | 9:aa5d6636197b | 194 | double u_d1 = Kd1 * filtered_error1_derivative; |
aschut | 9:aa5d6636197b | 195 | error1_prev = error1; |
aschut | 6:14a9c4f30c86 | 196 | |
aschut | 6:14a9c4f30c86 | 197 | // Sum all parts and return it |
aschut | 9:aa5d6636197b | 198 | return u_k1 + u_i1 + u_d1; |
aschut | 9:aa5d6636197b | 199 | } |
aschut | 9:aa5d6636197b | 200 | double PID_controller2(double error2) |
aschut | 9:aa5d6636197b | 201 | { |
aschut | 9:aa5d6636197b | 202 | static double error2_integral = 0; |
aschut | 9:aa5d6636197b | 203 | static double error2_prev = error2; // initialization with this value only done once! |
aschut | 9:aa5d6636197b | 204 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth); |
aschut | 9:aa5d6636197b | 205 | |
aschut | 9:aa5d6636197b | 206 | /* PID testing |
aschut | 9:aa5d6636197b | 207 | Kp2 = 10 * Pot2; |
aschut | 9:aa5d6636197b | 208 | Ki2 = 10 * Pot1; |
aschut | 9:aa5d6636197b | 209 | |
aschut | 9:aa5d6636197b | 210 | if (!But2) { |
aschut | 9:aa5d6636197b | 211 | Kd2 = Kd2 + 0.01; |
aschut | 9:aa5d6636197b | 212 | } |
aschut | 9:aa5d6636197b | 213 | if (!But1){ |
aschut | 9:aa5d6636197b | 214 | Kd2 = Kd2 - 0.01; |
aschut | 9:aa5d6636197b | 215 | } |
aschut | 9:aa5d6636197b | 216 | */ |
aschut | 9:aa5d6636197b | 217 | |
aschut | 9:aa5d6636197b | 218 | // Proportional part: |
aschut | 9:aa5d6636197b | 219 | double u_k2 = Kp2 * error2; |
aschut | 9:aa5d6636197b | 220 | |
aschut | 9:aa5d6636197b | 221 | // Integral part |
aschut | 9:aa5d6636197b | 222 | error2_integral = error2_integral + error2 * Ts; |
aschut | 9:aa5d6636197b | 223 | double u_i2 = Ki2 * error2_integral; |
aschut | 9:aa5d6636197b | 224 | |
aschut | 9:aa5d6636197b | 225 | // Derivative part |
aschut | 9:aa5d6636197b | 226 | double error2_derivative = (error2 - error2_prev)/Ts; |
aschut | 9:aa5d6636197b | 227 | double filtered_error2_derivative = LowPassFilter.step(error2_derivative); |
aschut | 9:aa5d6636197b | 228 | double u_d2 = Kd2 * filtered_error2_derivative; |
aschut | 9:aa5d6636197b | 229 | error2_prev = error2; |
aschut | 9:aa5d6636197b | 230 | |
aschut | 9:aa5d6636197b | 231 | // Sum all parts and return it |
aschut | 9:aa5d6636197b | 232 | return u_k2 + u_i2 + u_d2; |
aschut | 6:14a9c4f30c86 | 233 | } |
aschut | 6:14a9c4f30c86 | 234 | |
aschut | 9:aa5d6636197b | 235 | void moter1_control(double u1) |
aschut | 6:14a9c4f30c86 | 236 | { |
aschut | 9:aa5d6636197b | 237 | direction1= u1 > 0.0f; //positief = CW |
aschut | 11:2375b538b631 | 238 | if (fabs(u1)> 0.5f) { |
aschut | 11:2375b538b631 | 239 | u1 = 0.5f; |
aschut | 6:14a9c4f30c86 | 240 | } else { |
aschut | 9:aa5d6636197b | 241 | u1= u1; |
aschut | 0:a9a42914138c | 242 | } |
aschut | 13:b7a1a4245f37 | 243 | pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value |
aschut | 9:aa5d6636197b | 244 | } |
aschut | 9:aa5d6636197b | 245 | |
aschut | 9:aa5d6636197b | 246 | void moter2_control(double u2) |
aschut | 9:aa5d6636197b | 247 | { |
aschut | 9:aa5d6636197b | 248 | direction2= u2 < 0.0f; //positief = CW |
aschut | 9:aa5d6636197b | 249 | if (fabs(u2)> 0.7f) { |
aschut | 9:aa5d6636197b | 250 | u2 = 0.7f; |
aschut | 9:aa5d6636197b | 251 | } else { |
aschut | 9:aa5d6636197b | 252 | u2= u2; |
aschut | 9:aa5d6636197b | 253 | } |
aschut | 13:b7a1a4245f37 | 254 | pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value |
aschut | 6:14a9c4f30c86 | 255 | } |
aschut | 6:14a9c4f30c86 | 256 | |
aschut | 4:99f7fdce608e | 257 | void PwmMotor(void) |
aschut | 6:14a9c4f30c86 | 258 | { |
aschut | 14:30d32f51dd9f | 259 | //Reference hoek berekenen, in graden |
aschut | 9:aa5d6636197b | 260 | float Ellebooghoek1 = Kinematics2(pwm2); |
aschut | 9:aa5d6636197b | 261 | float Ellebooghoek4 = Limits2(Ellebooghoek1); |
aschut | 14:30d32f51dd9f | 262 | //ElbowReference = Ellebooghoek4; |
aschut | 9:aa5d6636197b | 263 | |
aschut | 14:30d32f51dd9f | 264 | //float Polshoek1 = Kinematics1(pwm2); |
aschut | 14:30d32f51dd9f | 265 | //float Polshoek4 = Limits1(Polshoek1); |
aschut | 14:30d32f51dd9f | 266 | //PolsReference = Polshoek4; |
aschut | 6:14a9c4f30c86 | 267 | |
aschut | 7:9a1007e35bac | 268 | // Positie motor berekenen, in graden |
aschut | 9:aa5d6636197b | 269 | Pulses1 = encoder1.getPulses(); |
aschut | 9:aa5d6636197b | 270 | motor_position1 = -(Pulses1/1200)*360; |
aschut | 6:14a9c4f30c86 | 271 | Pulses2 = encoder2.getPulses(); |
aschut | 9:aa5d6636197b | 272 | motor_position2 = -(Pulses2/4800)*360; |
aschut | 2:926d56babb1a | 273 | |
aschut | 9:aa5d6636197b | 274 | double error1 = PolsReference - motor_position1; |
aschut | 9:aa5d6636197b | 275 | double u1 = PID_controller1(error1); |
aschut | 9:aa5d6636197b | 276 | moter1_control(u1); |
aschut | 9:aa5d6636197b | 277 | |
aschut | 9:aa5d6636197b | 278 | double error2 = ElbowReference - motor_position2; |
aschut | 9:aa5d6636197b | 279 | double u2 = PID_controller2(error2); |
aschut | 9:aa5d6636197b | 280 | moter2_control(u2); |
aschut | 6:14a9c4f30c86 | 281 | |
aschut | 6:14a9c4f30c86 | 282 | } |
aschut | 6:14a9c4f30c86 | 283 | |
aschut | 1:58f34947c674 | 284 | void MotorOn(void) |
aschut | 6:14a9c4f30c86 | 285 | { |
aschut | 9:aa5d6636197b | 286 | pwmpin1 = 0; |
aschut | 4:99f7fdce608e | 287 | pwmpin2 = 0; |
aschut | 6:14a9c4f30c86 | 288 | Pwm.attach (PwmMotor, Ts); |
aschut | 6:14a9c4f30c86 | 289 | |
aschut | 6:14a9c4f30c86 | 290 | } |
aschut | 1:58f34947c674 | 291 | void MotorOff(void) |
aschut | 6:14a9c4f30c86 | 292 | { |
aschut | 6:14a9c4f30c86 | 293 | Pwm.detach (); |
aschut | 1:58f34947c674 | 294 | pwmpin2 = 0; |
aschut | 9:aa5d6636197b | 295 | pwmpin1 = 0; |
aschut | 6:14a9c4f30c86 | 296 | } |
aschut | 4:99f7fdce608e | 297 | |
aschut | 6:14a9c4f30c86 | 298 | void ContinuousReader(void) |
aschut | 6:14a9c4f30c86 | 299 | { |
aschut | 3:ac13255164cd | 300 | Pot2 = pot.read(); |
aschut | 6:14a9c4f30c86 | 301 | Pot1 = pot0.read(); |
aschut | 7:9a1007e35bac | 302 | pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1 |
aschut | 9:aa5d6636197b | 303 | pwm1 =(Pot1*2)-1; |
aschut | 6:14a9c4f30c86 | 304 | } |
aschut | 6:14a9c4f30c86 | 305 | |
aschut | 14:30d32f51dd9f | 306 | |
aschut | 7:9a1007e35bac | 307 | void Kdcount (void) // Voor het testen van de PID waardes |
aschut | 7:9a1007e35bac | 308 | { |
aschut | 17:190c86f4828f | 309 | float ts = t.read(); |
aschut | 17:190c86f4828f | 310 | A = 50; |
aschut | 17:190c86f4828f | 311 | PolsReference = A*sin(2*pi*ts*0.2); |
aschut | 17:190c86f4828f | 312 | //DA = sin(2/180*3.14)*0.5+0.5; |
aschut | 17:190c86f4828f | 313 | /* |
aschut | 17:190c86f4828f | 314 | if( i<360) { |
aschut | 17:190c86f4828f | 315 | DA = sin(i/180*3.14)*0.5+0.5; |
aschut | 17:190c86f4828f | 316 | i++; |
aschut | 17:190c86f4828f | 317 | } |
aschut | 17:190c86f4828f | 318 | */ |
aschut | 17:190c86f4828f | 319 | |
aschut | 7:9a1007e35bac | 320 | } |
aschut | 14:30d32f51dd9f | 321 | |
aschut | 1:58f34947c674 | 322 | |
aschut | 6:14a9c4f30c86 | 323 | int main() |
aschut | 6:14a9c4f30c86 | 324 | { |
aschut | 17:190c86f4828f | 325 | |
aschut | 6:14a9c4f30c86 | 326 | t.start(); |
aschut | 6:14a9c4f30c86 | 327 | int counter = 0; |
aschut | 6:14a9c4f30c86 | 328 | pwmpin2.period_us(60); |
aschut | 6:14a9c4f30c86 | 329 | PotRead.attach(ContinuousReader,Ts); |
aschut | 17:190c86f4828f | 330 | Kdc.attach(Kdcount,0.01); //Voor PID waarde testen |
aschut | 17:190c86f4828f | 331 | //Kdcount(); |
aschut | 2:926d56babb1a | 332 | pc.baud(115200); |
aschut | 6:14a9c4f30c86 | 333 | //pc.printf("start\r\n"); |
aschut | 4:99f7fdce608e | 334 | led = 1; |
aschut | 4:99f7fdce608e | 335 | led2 =1; |
aschut | 4:99f7fdce608e | 336 | led3 =1; |
aschut | 6:14a9c4f30c86 | 337 | |
aschut | 6:14a9c4f30c86 | 338 | while (true) { |
aschut | 6:14a9c4f30c86 | 339 | led3 = 0; |
aschut | 6:14a9c4f30c86 | 340 | if (!button2) { |
aschut | 6:14a9c4f30c86 | 341 | led3 = 1; |
aschut | 6:14a9c4f30c86 | 342 | led = 0; |
aschut | 6:14a9c4f30c86 | 343 | //pc.printf("MotorOn\r\n"); |
aschut | 6:14a9c4f30c86 | 344 | MotorOn(); |
aschut | 6:14a9c4f30c86 | 345 | } |
aschut | 6:14a9c4f30c86 | 346 | if (!button3) { |
aschut | 6:14a9c4f30c86 | 347 | //pc.printf("MotorOff\r\n"); |
aschut | 6:14a9c4f30c86 | 348 | PotRead.detach(); |
aschut | 6:14a9c4f30c86 | 349 | MotorOff(); |
aschut | 6:14a9c4f30c86 | 350 | } |
aschut | 4:99f7fdce608e | 351 | led = 0; |
aschut | 17:190c86f4828f | 352 | |
aschut | 6:14a9c4f30c86 | 353 | if(counter==10) { |
aschut | 6:14a9c4f30c86 | 354 | float tmp = t.read(); |
aschut | 9:aa5d6636197b | 355 | printf("%f,%f,%f,%f,%f,%f,%f\n\r",tmp,motor_position1,PolsReference,error1,Kp1,Ki1,Kd1); |
aschut | 6:14a9c4f30c86 | 356 | counter = 0; |
aschut | 6:14a9c4f30c86 | 357 | } |
aschut | 6:14a9c4f30c86 | 358 | counter++; |
aschut | 6:14a9c4f30c86 | 359 | wait(0.001); |
aschut | 1:58f34947c674 | 360 | } |
aschut | 6:14a9c4f30c86 | 361 | } |
aschut | 0:a9a42914138c | 362 | |
aschut | 0:a9a42914138c | 363 | |
aschut | 6:14a9c4f30c86 | 364 |