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