Operation Mode State Machine plus homing for StateMachinePTR
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of Controller by
OperationMode.cpp@16:1c8a8643260f, 2018-11-01 (annotated)
- Committer:
- Roooos
- Date:
- Thu Nov 01 10:10:54 2018 +0000
- Revision:
- 16:1c8a8643260f
- Parent:
- 15:97311058e012
aangepast op StateMachinePTR, werkende en met Homing
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Roooos | 10:93957c339972 | 1 | #include "mbed.h" |
Roooos | 10:93957c339972 | 2 | #include "math.h" |
Roooos | 10:93957c339972 | 3 | #include "MODSERIAL.h" |
Roooos | 10:93957c339972 | 4 | #include "QEI.h" |
Roooos | 10:93957c339972 | 5 | #include "BiQuad.h" |
Roooos | 10:93957c339972 | 6 | |
Roooos | 10:93957c339972 | 7 | //Tickers |
Roooos | 10:93957c339972 | 8 | Ticker TickerMeasureAndControl; |
Roooos | 10:93957c339972 | 9 | Timer TimerLoop; |
Roooos | 10:93957c339972 | 10 | //Communication |
Roooos | 10:93957c339972 | 11 | MODSERIAL pc(USBTX,USBRX); |
Roooos | 10:93957c339972 | 12 | QEI Encoder(D10,D11,NC,32); |
Roooos | 10:93957c339972 | 13 | QEI Encoder2(D12,D13,NC,32); |
Roooos | 10:93957c339972 | 14 | //Global pin variables |
Roooos | 10:93957c339972 | 15 | PwmOut PwmPin(D5); |
Roooos | 10:93957c339972 | 16 | PwmOut PwmPin2(D6); |
Roooos | 10:93957c339972 | 17 | DigitalOut DirectionPin(D4); |
Roooos | 10:93957c339972 | 18 | DigitalOut DirectionPin2(D7); |
Roooos | 10:93957c339972 | 19 | DigitalIn BUT1(D8); |
Roooos | 10:93957c339972 | 20 | DigitalIn BUT2(D9); |
Roooos | 10:93957c339972 | 21 | DigitalIn button_sw2(SW2); |
Roooos | 10:93957c339972 | 22 | DigitalIn button_sw3(SW3); |
Roooos | 10:93957c339972 | 23 | |
Roooos | 10:93957c339972 | 24 | DigitalOut LedGreen(LED_GREEN); |
Roooos | 10:93957c339972 | 25 | DigitalOut LedRed(LED_RED); |
Roooos | 10:93957c339972 | 26 | DigitalOut LedBlue(LED_BLUE); |
Roooos | 10:93957c339972 | 27 | |
Roooos | 16:1c8a8643260f | 28 | double q1Ref=0; |
Roooos | 15:97311058e012 | 29 | double q2Ref =0; |
Roooos | 10:93957c339972 | 30 | |
Roooos | 10:93957c339972 | 31 | |
Roooos | 16:1c8a8643260f | 32 | |
Roooos | 16:1c8a8643260f | 33 | |
Roooos | 10:93957c339972 | 34 | //Define substates Operation Mode |
Roooos | 10:93957c339972 | 35 | |
Roooos | 11:dcc416dbe3ea | 36 | enum States {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming}; |
Roooos | 10:93957c339972 | 37 | States CurrentOperationState; |
Roooos | 10:93957c339972 | 38 | |
Roooos | 16:1c8a8643260f | 39 | void SetMotorsOff() |
Roooos | 16:1c8a8643260f | 40 | { |
Roooos | 16:1c8a8643260f | 41 | PwmPin = 0; |
Roooos | 16:1c8a8643260f | 42 | PwmPin2 = 0; |
Roooos | 16:1c8a8643260f | 43 | } |
Roooos | 15:97311058e012 | 44 | //State Machine for OperationMode |
Roooos | 10:93957c339972 | 45 | void OperationStateMachine() |
Roooos | 10:93957c339972 | 46 | { |
Roooos | 10:93957c339972 | 47 | switch (CurrentOperationState) { |
Roooos | 10:93957c339972 | 48 | case OPWait: |
Roooos | 16:1c8a8643260f | 49 | if (BUT1 == false) { //When Left Biceps are contracted, initiate chain to flip the page |
Roooos | 12:cae29e41ac2e | 50 | CurrentOperationState = OPState1; |
Roooos | 12:cae29e41ac2e | 51 | TimerLoop.reset(); |
Roooos | 12:cae29e41ac2e | 52 | } |
Roooos | 16:1c8a8643260f | 53 | if (BUT2 == false) { //When sw3 is pressed, go to homing for Operation Mode |
Roooos | 11:dcc416dbe3ea | 54 | CurrentOperationState = OPSet; |
Roooos | 13:9125d359619c | 55 | TimerLoop.reset(); |
Roooos | 11:dcc416dbe3ea | 56 | } |
Roooos | 11:dcc416dbe3ea | 57 | break; |
Roooos | 16:1c8a8643260f | 58 | |
Roooos | 15:97311058e012 | 59 | case OPSet: // Set new homing for Operation Mode and go back to waiting mode |
Roooos | 15:97311058e012 | 60 | if (q2Ref > -0.733*(6.380)) { |
Roooos | 15:97311058e012 | 61 | q2Ref -= 0.0005*(6.380); |
Roooos | 16:1c8a8643260f | 62 | } |
Roooos | 15:97311058e012 | 63 | if (q2Ref < -0.733*(6.380) && TimerLoop > 2) { |
Roooos | 13:9125d359619c | 64 | CurrentOperationState = OPWait; |
Roooos | 10:93957c339972 | 65 | TimerLoop.reset(); |
Roooos | 16:1c8a8643260f | 66 | } |
Roooos | 10:93957c339972 | 67 | break; |
Roooos | 16:1c8a8643260f | 68 | |
Roooos | 15:97311058e012 | 69 | case OPState1: // First step of chain to flip the page |
Roooos | 15:97311058e012 | 70 | if (q1Ref < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4 |
Roooos | 15:97311058e012 | 71 | q1Ref += 0.0005*(6.380); |
Roooos | 10:93957c339972 | 72 | } |
Roooos | 15:97311058e012 | 73 | if ((q1Ref > 0.48*(6.380)) && (TimerLoop >= 2.0)) { |
Roooos | 10:93957c339972 | 74 | CurrentOperationState = OPState2; |
Roooos | 10:93957c339972 | 75 | TimerLoop.reset(); |
Roooos | 10:93957c339972 | 76 | } |
Roooos | 10:93957c339972 | 77 | break; |
Roooos | 16:1c8a8643260f | 78 | |
Roooos | 15:97311058e012 | 79 | case OPState2: //Second step of chain to flip the page |
Roooos | 15:97311058e012 | 80 | if (q2Ref > -1.133*(6.380)) { |
Roooos | 15:97311058e012 | 81 | q2Ref -= 0.0005*(6.380); |
Roooos | 10:93957c339972 | 82 | } |
Roooos | 15:97311058e012 | 83 | if ((q2Ref < -1.133*(6.380)) && (TimerLoop >= 2.0)) { |
Roooos | 10:93957c339972 | 84 | CurrentOperationState = OPState3; |
Roooos | 10:93957c339972 | 85 | TimerLoop.reset(); |
Roooos | 10:93957c339972 | 86 | } |
Roooos | 10:93957c339972 | 87 | break; |
Roooos | 16:1c8a8643260f | 88 | |
Roooos | 15:97311058e012 | 89 | case OPState3: //Third step of chain to flip the page |
Roooos | 15:97311058e012 | 90 | if (q1Ref > -0.15*(6.380)) { |
Roooos | 15:97311058e012 | 91 | q1Ref -= 0.0005*(6.380); |
Roooos | 10:93957c339972 | 92 | } |
Roooos | 15:97311058e012 | 93 | if (q2Ref > -1.483*(6.380)) { |
Roooos | 15:97311058e012 | 94 | q2Ref -= 0.0003*(6.380); |
Roooos | 10:93957c339972 | 95 | } |
Roooos | 15:97311058e012 | 96 | if ((q1Ref < -0.15*(6.380)) && (q2Ref < -1.483*(6.380)) && (TimerLoop >= 3.0)) { |
Roooos | 10:93957c339972 | 97 | CurrentOperationState = OPState4; |
Roooos | 10:93957c339972 | 98 | TimerLoop.reset(); |
Roooos | 10:93957c339972 | 99 | } |
Roooos | 10:93957c339972 | 100 | break; |
Roooos | 16:1c8a8643260f | 101 | |
Roooos | 15:97311058e012 | 102 | case OPState4: //Fourth step of chain to flip the page |
Roooos | 15:97311058e012 | 103 | if (q2Ref > -2.133*(6.380)) { |
Roooos | 15:97311058e012 | 104 | q2Ref -= 0.005*(6.380); |
Roooos | 10:93957c339972 | 105 | } |
Roooos | 15:97311058e012 | 106 | if ((q2Ref < -2.133*(6.380)) && (TimerLoop > 0.5)) { |
Roooos | 11:dcc416dbe3ea | 107 | TimerLoop.reset(); |
Roooos | 10:93957c339972 | 108 | CurrentOperationState = OPHoming; |
Roooos | 10:93957c339972 | 109 | } |
Roooos | 10:93957c339972 | 110 | break; |
Roooos | 16:1c8a8643260f | 111 | |
Roooos | 15:97311058e012 | 112 | case OPHoming: //Go back to Home for Operation Mode and back to Waiting |
Roooos | 15:97311058e012 | 113 | if (q1Ref < 0) { |
Roooos | 15:97311058e012 | 114 | q1Ref += 0.003*(6.380); |
Roooos | 11:dcc416dbe3ea | 115 | } |
Roooos | 15:97311058e012 | 116 | if (q2Ref < -0.733*(6.380)) { |
Roooos | 15:97311058e012 | 117 | q2Ref += 0.001*(6.380); |
Roooos | 11:dcc416dbe3ea | 118 | } |
Roooos | 15:97311058e012 | 119 | if ((q1Ref > 0) && (q2Ref > -0.733*(6.380)) && (TimerLoop > 3)) { |
Roooos | 11:dcc416dbe3ea | 120 | CurrentOperationState = OPWait; |
Roooos | 11:dcc416dbe3ea | 121 | } |
Roooos | 10:93957c339972 | 122 | break; |
Roooos | 10:93957c339972 | 123 | } |
Roooos | 10:93957c339972 | 124 | } |
Roooos | 16:1c8a8643260f | 125 | //Homing |
Roooos | 16:1c8a8643260f | 126 | /* |
Roooos | 16:1c8a8643260f | 127 | if (q1Ref >= 0) { |
Roooos | 16:1c8a8643260f | 128 | q1Ref -= 0.001*(6.380); |
Roooos | 16:1c8a8643260f | 129 | } |
Roooos | 16:1c8a8643260f | 130 | if (q1Ref <= 0) { |
Roooos | 16:1c8a8643260f | 131 | q1Ref += 0.001*(6.380); |
Roooos | 16:1c8a8643260f | 132 | } |
Roooos | 16:1c8a8643260f | 133 | if (q2Ref >= 0) { |
Roooos | 16:1c8a8643260f | 134 | q2Ref -= 0.001*(6.380); |
Roooos | 16:1c8a8643260f | 135 | } |
Roooos | 16:1c8a8643260f | 136 | if (q2Ref <= 0) { |
Roooos | 16:1c8a8643260f | 137 | q2Ref += 0.001*(6.380); |
Roooos | 16:1c8a8643260f | 138 | } |
Roooos | 16:1c8a8643260f | 139 | if ((-0.01*(6.380) < q1Ref < 0.01*(6.380)) && (-0.01*(6.380) < q1Ref < 0.01*(6.380))) { |
Roooos | 16:1c8a8643260f | 140 | SetMotorsOff(); |
Roooos | 16:1c8a8643260f | 141 | } |
Roooos | 16:1c8a8643260f | 142 | */ |
Roooos | 10:93957c339972 | 143 | |
Roooos | 10:93957c339972 | 144 | //----------------------------------------------------------------------------- |
Roooos | 10:93957c339972 | 145 | //The double-functions |
Roooos | 10:93957c339972 | 146 | |
Roooos | 10:93957c339972 | 147 | |
Roooos | 10:93957c339972 | 148 | |
Roooos | 10:93957c339972 | 149 | |
Roooos | 10:93957c339972 | 150 | // actual position of the motor 1 |
Roooos | 10:93957c339972 | 151 | double GetActualPosition() |
Roooos | 10:93957c339972 | 152 | { |
Roooos | 10:93957c339972 | 153 | //This function determines the actual position of the motor |
Roooos | 10:93957c339972 | 154 | //The count:radians relation is 8400:2pi |
Roooos | 10:93957c339972 | 155 | double EncoderCounts = Encoder.getPulses(); //number of counts |
Roooos | 10:93957c339972 | 156 | double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding |
Roooos | 10:93957c339972 | 157 | |
Roooos | 10:93957c339972 | 158 | return PositionMotor; |
Roooos | 10:93957c339972 | 159 | } |
Roooos | 10:93957c339972 | 160 | |
Roooos | 10:93957c339972 | 161 | double GetActualPosition2() //motor 2 |
Roooos | 10:93957c339972 | 162 | { |
Roooos | 10:93957c339972 | 163 | double Encoder2Counts = Encoder2.getPulses(); //number of counts |
Roooos | 10:93957c339972 | 164 | double PositionMotor2 = Encoder2Counts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding |
Roooos | 10:93957c339972 | 165 | |
Roooos | 10:93957c339972 | 166 | return PositionMotor2; |
Roooos | 10:93957c339972 | 167 | } |
Roooos | 10:93957c339972 | 168 | |
Roooos | 10:93957c339972 | 169 | ///The controller |
Roooos | 10:93957c339972 | 170 | double PID_Controller(double Error) |
Roooos | 10:93957c339972 | 171 | { |
Roooos | 10:93957c339972 | 172 | //Arm drive parameters |
Roooos | 10:93957c339972 | 173 | double Ts = 0.002; //Sampling time 100 Hz |
Roooos | 10:93957c339972 | 174 | double Kp = 19.8; // proportional gain |
Roooos | 10:93957c339972 | 175 | double Ki = 3.98; //Intergral gain |
Roooos | 10:93957c339972 | 176 | double Kd = 1.96; //Differential gain. |
Roooos | 10:93957c339972 | 177 | |
Roooos | 10:93957c339972 | 178 | static double ErrorIntegral = 0; |
Roooos | 10:93957c339972 | 179 | static double ErrorPrevious = Error; |
Roooos | 10:93957c339972 | 180 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Roooos | 10:93957c339972 | 181 | |
Roooos | 10:93957c339972 | 182 | //Proportional part: |
Roooos | 10:93957c339972 | 183 | double u_k = Kp * Error; |
Roooos | 10:93957c339972 | 184 | |
Roooos | 10:93957c339972 | 185 | //Integral part: |
Roooos | 10:93957c339972 | 186 | ErrorIntegral = ErrorIntegral + Error*Ts; |
Roooos | 10:93957c339972 | 187 | double u_i = Ki * ErrorIntegral; |
Roooos | 10:93957c339972 | 188 | |
Roooos | 10:93957c339972 | 189 | //Derivative part: |
Roooos | 10:93957c339972 | 190 | double ErrorDerivative = (Error - ErrorPrevious)/Ts; |
Roooos | 10:93957c339972 | 191 | double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); |
Roooos | 10:93957c339972 | 192 | double u_d = Kd * FilteredErrorDerivative; |
Roooos | 10:93957c339972 | 193 | ErrorPrevious = Error; |
Roooos | 10:93957c339972 | 194 | |
Roooos | 10:93957c339972 | 195 | // sum of parts and return it |
Roooos | 10:93957c339972 | 196 | return u_k + u_i + u_d; //This will become the MotorValue |
Roooos | 10:93957c339972 | 197 | } |
Roooos | 10:93957c339972 | 198 | |
Roooos | 10:93957c339972 | 199 | double PID_Controller2(double Error) |
Roooos | 10:93957c339972 | 200 | { |
Roooos | 10:93957c339972 | 201 | //Belt drive parameters |
Roooos | 10:93957c339972 | 202 | double Ts = 0.002; //Sampling time 100 Hz |
Roooos | 10:93957c339972 | 203 | double Kp = 11.1; // proportional gain |
Roooos | 10:93957c339972 | 204 | double Ki = 2.24; //Integral gain |
Roooos | 10:93957c339972 | 205 | double Kd = 1.1; //Differential gain |
Roooos | 10:93957c339972 | 206 | |
Roooos | 10:93957c339972 | 207 | static double ErrorIntegral = 0; |
Roooos | 10:93957c339972 | 208 | static double ErrorPrevious = Error; |
Roooos | 10:93957c339972 | 209 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Roooos | 10:93957c339972 | 210 | |
Roooos | 10:93957c339972 | 211 | //Proportional part: |
Roooos | 10:93957c339972 | 212 | double u_k = Kp * Error; |
Roooos | 10:93957c339972 | 213 | |
Roooos | 10:93957c339972 | 214 | //Integral part: |
Roooos | 10:93957c339972 | 215 | ErrorIntegral = ErrorIntegral + Error*Ts; |
Roooos | 10:93957c339972 | 216 | double u_i = Ki * ErrorIntegral; |
Roooos | 10:93957c339972 | 217 | |
Roooos | 10:93957c339972 | 218 | //Derivative part: |
Roooos | 10:93957c339972 | 219 | double ErrorDerivative = (Error - ErrorPrevious)/Ts; |
Roooos | 10:93957c339972 | 220 | double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); |
Roooos | 10:93957c339972 | 221 | double u_d = Kd * FilteredErrorDerivative; |
Roooos | 10:93957c339972 | 222 | ErrorPrevious = Error; |
Roooos | 10:93957c339972 | 223 | |
Roooos | 10:93957c339972 | 224 | // sum of parts and return it |
Roooos | 10:93957c339972 | 225 | return u_k + u_i + u_d; //This will become the MotorValue |
Roooos | 10:93957c339972 | 226 | } |
Roooos | 10:93957c339972 | 227 | |
Roooos | 10:93957c339972 | 228 | //Ticker function set motorvalues |
Roooos | 10:93957c339972 | 229 | void SetMotor(double MotorValue, double MotorValue2) |
Roooos | 10:93957c339972 | 230 | { |
Roooos | 10:93957c339972 | 231 | if (MotorValue >=0) { |
Roooos | 10:93957c339972 | 232 | DirectionPin = 1; |
Roooos | 10:93957c339972 | 233 | } else { |
Roooos | 10:93957c339972 | 234 | DirectionPin = 0; |
Roooos | 10:93957c339972 | 235 | } |
Roooos | 10:93957c339972 | 236 | |
Roooos | 10:93957c339972 | 237 | if (fabs(MotorValue)>1) { // if error more than 1 radian, full duty cycle |
Roooos | 10:93957c339972 | 238 | PwmPin = 1; |
Roooos | 10:93957c339972 | 239 | } else { |
Roooos | 10:93957c339972 | 240 | PwmPin = fabs(MotorValue); |
Roooos | 10:93957c339972 | 241 | } |
Roooos | 10:93957c339972 | 242 | |
Roooos | 10:93957c339972 | 243 | if (MotorValue2 >=0) { |
Roooos | 10:93957c339972 | 244 | DirectionPin2 = 1; |
Roooos | 10:93957c339972 | 245 | } else { |
Roooos | 10:93957c339972 | 246 | DirectionPin2 = 0; |
Roooos | 10:93957c339972 | 247 | } |
Roooos | 10:93957c339972 | 248 | |
Roooos | 10:93957c339972 | 249 | if (fabs(MotorValue2)>1) { // if error more than 1 radian, full duty cycle |
Roooos | 10:93957c339972 | 250 | PwmPin2 = 1; |
Roooos | 10:93957c339972 | 251 | } else { |
Roooos | 10:93957c339972 | 252 | PwmPin2 = fabs(MotorValue2); |
Roooos | 10:93957c339972 | 253 | } |
Roooos | 10:93957c339972 | 254 | } |
Roooos | 10:93957c339972 | 255 | |
Roooos | 10:93957c339972 | 256 | // ---------------------------------------------------------------------------- |
Roooos | 10:93957c339972 | 257 | //Ticker function |
Roooos | 10:93957c339972 | 258 | void MeasureAndControl(void) |
Roooos | 10:93957c339972 | 259 | { |
Roooos | 11:dcc416dbe3ea | 260 | |
Roooos | 15:97311058e012 | 261 | //double q1Ref = GetReferencePosition(); |
Roooos | 15:97311058e012 | 262 | //double q2Ref = GetReferencePosition2(); |
Roooos | 11:dcc416dbe3ea | 263 | |
Roooos | 10:93957c339972 | 264 | OperationStateMachine(); |
Roooos | 10:93957c339972 | 265 | double PositionMotor = GetActualPosition(); |
Roooos | 10:93957c339972 | 266 | double PositionMotor2 = GetActualPosition2(); |
Roooos | 15:97311058e012 | 267 | double MotorValue = PID_Controller(q1Ref - PositionMotor); // input is error |
Roooos | 15:97311058e012 | 268 | double MotorValue2 = PID_Controller2(q2Ref - PositionMotor2); |
Roooos | 10:93957c339972 | 269 | SetMotor(MotorValue, MotorValue2); |
Roooos | 10:93957c339972 | 270 | } |
Roooos | 10:93957c339972 | 271 | |
Roooos | 10:93957c339972 | 272 | |
Roooos | 10:93957c339972 | 273 | |
Roooos | 10:93957c339972 | 274 | //----------------------------------------------------------------------------- |
Roooos | 10:93957c339972 | 275 | int main() |
Roooos | 10:93957c339972 | 276 | { |
Roooos | 10:93957c339972 | 277 | LedGreen = 1; |
Roooos | 10:93957c339972 | 278 | LedRed = 1; |
Roooos | 10:93957c339972 | 279 | LedBlue = 1; |
Roooos | 10:93957c339972 | 280 | pc.baud(115200); |
Roooos | 10:93957c339972 | 281 | pc.printf("Hello World\n\r"); |
Roooos | 10:93957c339972 | 282 | TimerLoop.start(); |
Roooos | 10:93957c339972 | 283 | CurrentOperationState = OPWait; |
Roooos | 10:93957c339972 | 284 | PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!) |
Roooos | 10:93957c339972 | 285 | TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz |
Roooos | 10:93957c339972 | 286 | while (true) { |
Roooos | 10:93957c339972 | 287 | } |
Roooos | 10:93957c339972 | 288 | } |