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