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