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