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@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?

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