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@12:cae29e41ac2e, 2018-10-31 (annotated)

Committer:
Roooos
Date:
Wed Oct 31 21:21:13 2018 +0000
Revision:
12:cae29e41ac2e
Parent:
11:dcc416dbe3ea
Child:
13:9125d359619c
met OPSET;

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 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 11:dcc416dbe3ea 34 enum States {OPWait, OPSet, 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 11:dcc416dbe3ea 40
Roooos 10:93957c339972 41 switch (CurrentOperationState) {
Roooos 10:93957c339972 42 case OPWait:
Roooos 10:93957c339972 43 LedRed = 0; //red
Roooos 11:dcc416dbe3ea 44 LedGreen = 1;
Roooos 11:dcc416dbe3ea 45 LedBlue = 1;
Roooos 12:cae29e41ac2e 46 if (BUT1 == false) {
Roooos 12:cae29e41ac2e 47 CurrentOperationState = OPState1;
Roooos 12:cae29e41ac2e 48 TimerLoop.reset();
Roooos 12:cae29e41ac2e 49 }
Roooos 12:cae29e41ac2e 50 if (button_sw3 == false) {
Roooos 11:dcc416dbe3ea 51 CurrentOperationState = OPSet;
Roooos 12:cae29e41ac2e 52 TimreLoop.reset();
Roooos 11:dcc416dbe3ea 53 }
Roooos 11:dcc416dbe3ea 54 break;
Roooos 11:dcc416dbe3ea 55
Roooos 11:dcc416dbe3ea 56 case OPSet:
Roooos 11:dcc416dbe3ea 57 LedRed = 1; //turqoise
Roooos 11:dcc416dbe3ea 58 LedGreen = 0;
Roooos 11:dcc416dbe3ea 59 LedBlue = 0;
Roooos 11:dcc416dbe3ea 60 if (PositionRef2 > -0.853*(6.380)) {
Roooos 11:dcc416dbe3ea 61 PositionRef2 -= 0.0005*(6.380);
Roooos 11:dcc416dbe3ea 62 }
Roooos 11:dcc416dbe3ea 63 if (PositionRef2 < -0.853*(6.380) && TimerLoop > 2) {
Roooos 10:93957c339972 64 CurrentOperationState = OPState1;
Roooos 10:93957c339972 65 TimerLoop.reset();
Roooos 10:93957c339972 66 }
Roooos 10:93957c339972 67 break;
Roooos 11:dcc416dbe3ea 68
Roooos 10:93957c339972 69 case OPState1:
Roooos 11:dcc416dbe3ea 70 LedRed = 0; //yellow
Roooos 11:dcc416dbe3ea 71 LedGreen = 0;
Roooos 11:dcc416dbe3ea 72 LedBlue = 1;
Roooos 11:dcc416dbe3ea 73 if (PositionRef < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
Roooos 10:93957c339972 74 PositionRef += 0.0005*(6.380);
Roooos 10:93957c339972 75 }
Roooos 11:dcc416dbe3ea 76 if ((PositionRef > 0.48*(6.380)) && (TimerLoop >= 4.0)) {
Roooos 10:93957c339972 77 CurrentOperationState = OPState2;
Roooos 10:93957c339972 78 TimerLoop.reset();
Roooos 10:93957c339972 79 }
Roooos 10:93957c339972 80 break;
Roooos 11:dcc416dbe3ea 81
Roooos 10:93957c339972 82 case OPState2:
Roooos 11:dcc416dbe3ea 83 LedRed = 0; //white
Roooos 11:dcc416dbe3ea 84 LedGreen = 0;
Roooos 11:dcc416dbe3ea 85 LedBlue = 0;
Roooos 11:dcc416dbe3ea 86 if (PositionRef2 > -0.40*(6.380)) {
Roooos 10:93957c339972 87 PositionRef2 -= 0.0005*(6.380);
Roooos 10:93957c339972 88 }
Roooos 11:dcc416dbe3ea 89 if ((PositionRef2 < -0.40*(6.380)) && (TimerLoop >= 4.0)) {
Roooos 10:93957c339972 90 CurrentOperationState = OPState3;
Roooos 10:93957c339972 91 TimerLoop.reset();
Roooos 10:93957c339972 92 }
Roooos 10:93957c339972 93 break;
Roooos 11:dcc416dbe3ea 94
Roooos 10:93957c339972 95 case OPState3:
Roooos 11:dcc416dbe3ea 96 LedRed = 0; //pink
Roooos 11:dcc416dbe3ea 97 LedGreen = 1;
Roooos 11:dcc416dbe3ea 98 LedBlue = 0;
Roooos 11:dcc416dbe3ea 99 if (PositionRef > -0.15*(6.380)) {
Roooos 10:93957c339972 100 PositionRef -= 0.0005*(6.380);
Roooos 10:93957c339972 101 }
Roooos 11:dcc416dbe3ea 102 if (PositionRef2 > -0.75*(6.380)) {
Roooos 11:dcc416dbe3ea 103 PositionRef2 -= 0.0002*(6.380);
Roooos 10:93957c339972 104 }
Roooos 11:dcc416dbe3ea 105 if ((PositionRef < -0.15*(6.380)) && (PositionRef2 < -0.75*(6.380)) && (TimerLoop >= 4.0)) {
Roooos 10:93957c339972 106 CurrentOperationState = OPState4;
Roooos 10:93957c339972 107 TimerLoop.reset();
Roooos 10:93957c339972 108 }
Roooos 10:93957c339972 109 break;
Roooos 11:dcc416dbe3ea 110
Roooos 10:93957c339972 111 case OPState4:
Roooos 11:dcc416dbe3ea 112 LedRed = 1; //blue
Roooos 11:dcc416dbe3ea 113 LedGreen = 1;
Roooos 11:dcc416dbe3ea 114 LedBlue = 0;
Roooos 11:dcc416dbe3ea 115 if (PositionRef2 > -1.4*(6.380)) {
Roooos 11:dcc416dbe3ea 116 PositionRef2 -= 0.005*(6.380);
Roooos 10:93957c339972 117 }
Roooos 11:dcc416dbe3ea 118 if ((PositionRef2 < -1.4*(6.380)) && (TimerLoop > 4)) {
Roooos 11:dcc416dbe3ea 119 TimerLoop.reset();
Roooos 10:93957c339972 120 CurrentOperationState = OPHoming;
Roooos 10:93957c339972 121 }
Roooos 10:93957c339972 122 break;
Roooos 11:dcc416dbe3ea 123
Roooos 10:93957c339972 124 case OPHoming:
Roooos 10:93957c339972 125 LedGreen = 0; //Green
Roooos 10:93957c339972 126 LedBlue = 1;
Roooos 10:93957c339972 127 LedRed = 1;
Roooos 11:dcc416dbe3ea 128 if (PositionRef < 0) {
Roooos 11:dcc416dbe3ea 129 PositionRef += 0.003*(6.380);
Roooos 11:dcc416dbe3ea 130 }
Roooos 11:dcc416dbe3ea 131 if (PositionRef2 < 0) {
Roooos 11:dcc416dbe3ea 132 PositionRef2 += 0.001*(6.380);
Roooos 11:dcc416dbe3ea 133 }
Roooos 11:dcc416dbe3ea 134 if ((PositionRef > 0) && (PositionRef2 > 0) && (TimerLoop > 4)) {
Roooos 11:dcc416dbe3ea 135 CurrentOperationState = OPWait;
Roooos 11:dcc416dbe3ea 136 }
Roooos 10:93957c339972 137 break;
Roooos 10:93957c339972 138 }
Roooos 10:93957c339972 139 }
Roooos 10:93957c339972 140
Roooos 10:93957c339972 141
Roooos 10:93957c339972 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 10:93957c339972 261 //double PositionRef = GetReferencePosition();
Roooos 10:93957c339972 262 //double PositionRef2 = 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 10:93957c339972 267 double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
Roooos 10:93957c339972 268 double MotorValue2 = PID_Controller2(PositionRef2 - 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 }