Roos Wijngaarden / Mbed 2 deprecated OperationMode

Operation Mode State Machine plus homing for StateMachinePTR

Dependencies:   MODSERIAL QEI biquadFilter mbed

Fork of Controller by Sven van Wincoop

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?

UserRevisionLine numberNew 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 }