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@14:771cd444764b, 2018-11-01 (annotated)

Committer:
Roooos
Date:
Thu Nov 01 09:26:12 2018 +0000
Revision:
14:771cd444764b
Parent:
13:9125d359619c
Child:
15:97311058e012
juiste tijden

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 switch (CurrentOperationState) {
Roooos 10:93957c339972 40 case OPWait:
Roooos 10:93957c339972 41 LedRed = 0; //red
Roooos 11:dcc416dbe3ea 42 LedGreen = 1;
Roooos 11:dcc416dbe3ea 43 LedBlue = 1;
Roooos 12:cae29e41ac2e 44 if (BUT1 == false) {
Roooos 12:cae29e41ac2e 45 CurrentOperationState = OPState1;
Roooos 12:cae29e41ac2e 46 TimerLoop.reset();
Roooos 12:cae29e41ac2e 47 }
Roooos 12:cae29e41ac2e 48 if (button_sw3 == false) {
Roooos 11:dcc416dbe3ea 49 CurrentOperationState = OPSet;
Roooos 13:9125d359619c 50 TimerLoop.reset();
Roooos 11:dcc416dbe3ea 51 }
Roooos 11:dcc416dbe3ea 52 break;
Roooos 11:dcc416dbe3ea 53
Roooos 11:dcc416dbe3ea 54 case OPSet:
Roooos 11:dcc416dbe3ea 55 LedRed = 1; //turqoise
Roooos 11:dcc416dbe3ea 56 LedGreen = 0;
Roooos 11:dcc416dbe3ea 57 LedBlue = 0;
Roooos 13:9125d359619c 58 if (PositionRef2 > -0.733*(6.380)) {
Roooos 11:dcc416dbe3ea 59 PositionRef2 -= 0.0005*(6.380);
Roooos 11:dcc416dbe3ea 60 }
Roooos 13:9125d359619c 61 if (PositionRef2 < -0.733*(6.380) && TimerLoop > 2) {
Roooos 13:9125d359619c 62 CurrentOperationState = OPWait;
Roooos 10:93957c339972 63 TimerLoop.reset();
Roooos 10:93957c339972 64 }
Roooos 10:93957c339972 65 break;
Roooos 11:dcc416dbe3ea 66
Roooos 10:93957c339972 67 case OPState1:
Roooos 11:dcc416dbe3ea 68 LedRed = 0; //yellow
Roooos 11:dcc416dbe3ea 69 LedGreen = 0;
Roooos 11:dcc416dbe3ea 70 LedBlue = 1;
Roooos 11:dcc416dbe3ea 71 if (PositionRef < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
Roooos 10:93957c339972 72 PositionRef += 0.0005*(6.380);
Roooos 10:93957c339972 73 }
Roooos 14:771cd444764b 74 if ((PositionRef > 0.48*(6.380)) && (TimerLoop >= 2.0)) {
Roooos 10:93957c339972 75 CurrentOperationState = OPState2;
Roooos 10:93957c339972 76 TimerLoop.reset();
Roooos 10:93957c339972 77 }
Roooos 10:93957c339972 78 break;
Roooos 11:dcc416dbe3ea 79
Roooos 10:93957c339972 80 case OPState2:
Roooos 11:dcc416dbe3ea 81 LedRed = 0; //white
Roooos 11:dcc416dbe3ea 82 LedGreen = 0;
Roooos 11:dcc416dbe3ea 83 LedBlue = 0;
Roooos 13:9125d359619c 84 if (PositionRef2 > -1.133*(6.380)) {
Roooos 10:93957c339972 85 PositionRef2 -= 0.0005*(6.380);
Roooos 10:93957c339972 86 }
Roooos 14:771cd444764b 87 if ((PositionRef2 < -1.133*(6.380)) && (TimerLoop >= 2.0)) {
Roooos 10:93957c339972 88 CurrentOperationState = OPState3;
Roooos 10:93957c339972 89 TimerLoop.reset();
Roooos 10:93957c339972 90 }
Roooos 10:93957c339972 91 break;
Roooos 11:dcc416dbe3ea 92
Roooos 10:93957c339972 93 case OPState3:
Roooos 11:dcc416dbe3ea 94 LedRed = 0; //pink
Roooos 11:dcc416dbe3ea 95 LedGreen = 1;
Roooos 11:dcc416dbe3ea 96 LedBlue = 0;
Roooos 11:dcc416dbe3ea 97 if (PositionRef > -0.15*(6.380)) {
Roooos 10:93957c339972 98 PositionRef -= 0.0005*(6.380);
Roooos 10:93957c339972 99 }
Roooos 13:9125d359619c 100 if (PositionRef2 > -1.483*(6.380)) {
Roooos 14:771cd444764b 101 PositionRef2 -= 0.0003*(6.380);
Roooos 10:93957c339972 102 }
Roooos 14:771cd444764b 103 if ((PositionRef < -0.15*(6.380)) && (PositionRef2 < -1.483*(6.380)) && (TimerLoop >= 3.0)) {
Roooos 10:93957c339972 104 CurrentOperationState = OPState4;
Roooos 10:93957c339972 105 TimerLoop.reset();
Roooos 10:93957c339972 106 }
Roooos 10:93957c339972 107 break;
Roooos 11:dcc416dbe3ea 108
Roooos 10:93957c339972 109 case OPState4:
Roooos 11:dcc416dbe3ea 110 LedRed = 1; //blue
Roooos 11:dcc416dbe3ea 111 LedGreen = 1;
Roooos 11:dcc416dbe3ea 112 LedBlue = 0;
Roooos 13:9125d359619c 113 if (PositionRef2 > -2.133*(6.380)) {
Roooos 11:dcc416dbe3ea 114 PositionRef2 -= 0.005*(6.380);
Roooos 10:93957c339972 115 }
Roooos 14:771cd444764b 116 if ((PositionRef2 < -2.133*(6.380)) && (TimerLoop > 0.5)) {
Roooos 11:dcc416dbe3ea 117 TimerLoop.reset();
Roooos 10:93957c339972 118 CurrentOperationState = OPHoming;
Roooos 10:93957c339972 119 }
Roooos 10:93957c339972 120 break;
Roooos 11:dcc416dbe3ea 121
Roooos 10:93957c339972 122 case OPHoming:
Roooos 10:93957c339972 123 LedGreen = 0; //Green
Roooos 10:93957c339972 124 LedBlue = 1;
Roooos 10:93957c339972 125 LedRed = 1;
Roooos 11:dcc416dbe3ea 126 if (PositionRef < 0) {
Roooos 11:dcc416dbe3ea 127 PositionRef += 0.003*(6.380);
Roooos 11:dcc416dbe3ea 128 }
Roooos 13:9125d359619c 129 if (PositionRef2 < -0.733*(6.380)) {
Roooos 11:dcc416dbe3ea 130 PositionRef2 += 0.001*(6.380);
Roooos 11:dcc416dbe3ea 131 }
Roooos 14:771cd444764b 132 if ((PositionRef > 0) && (PositionRef2 > -0.733*(6.380)) && (TimerLoop > 3)) {
Roooos 11:dcc416dbe3ea 133 CurrentOperationState = OPWait;
Roooos 11:dcc416dbe3ea 134 }
Roooos 10:93957c339972 135 break;
Roooos 10:93957c339972 136 }
Roooos 10:93957c339972 137 }
Roooos 10:93957c339972 138
Roooos 10:93957c339972 139
Roooos 10:93957c339972 140
Roooos 10:93957c339972 141
Roooos 10:93957c339972 142 //-----------------------------------------------------------------------------
Roooos 10:93957c339972 143 //The double-functions
Roooos 10:93957c339972 144
Roooos 10:93957c339972 145
Roooos 10:93957c339972 146
Roooos 10:93957c339972 147
Roooos 10:93957c339972 148 // actual position of the motor 1
Roooos 10:93957c339972 149 double GetActualPosition()
Roooos 10:93957c339972 150 {
Roooos 10:93957c339972 151 //This function determines the actual position of the motor
Roooos 10:93957c339972 152 //The count:radians relation is 8400:2pi
Roooos 10:93957c339972 153 double EncoderCounts = Encoder.getPulses(); //number of counts
Roooos 10:93957c339972 154 double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
Roooos 10:93957c339972 155
Roooos 10:93957c339972 156 return PositionMotor;
Roooos 10:93957c339972 157 }
Roooos 10:93957c339972 158
Roooos 10:93957c339972 159 double GetActualPosition2() //motor 2
Roooos 10:93957c339972 160 {
Roooos 10:93957c339972 161 double Encoder2Counts = Encoder2.getPulses(); //number of counts
Roooos 10:93957c339972 162 double PositionMotor2 = Encoder2Counts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
Roooos 10:93957c339972 163
Roooos 10:93957c339972 164 return PositionMotor2;
Roooos 10:93957c339972 165 }
Roooos 10:93957c339972 166
Roooos 10:93957c339972 167 ///The controller
Roooos 10:93957c339972 168 double PID_Controller(double Error)
Roooos 10:93957c339972 169 {
Roooos 10:93957c339972 170 //Arm drive parameters
Roooos 10:93957c339972 171 double Ts = 0.002; //Sampling time 100 Hz
Roooos 10:93957c339972 172 double Kp = 19.8; // proportional gain
Roooos 10:93957c339972 173 double Ki = 3.98; //Intergral gain
Roooos 10:93957c339972 174 double Kd = 1.96; //Differential gain.
Roooos 10:93957c339972 175
Roooos 10:93957c339972 176 static double ErrorIntegral = 0;
Roooos 10:93957c339972 177 static double ErrorPrevious = Error;
Roooos 10:93957c339972 178 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Roooos 10:93957c339972 179
Roooos 10:93957c339972 180 //Proportional part:
Roooos 10:93957c339972 181 double u_k = Kp * Error;
Roooos 10:93957c339972 182
Roooos 10:93957c339972 183 //Integral part:
Roooos 10:93957c339972 184 ErrorIntegral = ErrorIntegral + Error*Ts;
Roooos 10:93957c339972 185 double u_i = Ki * ErrorIntegral;
Roooos 10:93957c339972 186
Roooos 10:93957c339972 187 //Derivative part:
Roooos 10:93957c339972 188 double ErrorDerivative = (Error - ErrorPrevious)/Ts;
Roooos 10:93957c339972 189 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
Roooos 10:93957c339972 190 double u_d = Kd * FilteredErrorDerivative;
Roooos 10:93957c339972 191 ErrorPrevious = Error;
Roooos 10:93957c339972 192
Roooos 10:93957c339972 193 // sum of parts and return it
Roooos 10:93957c339972 194 return u_k + u_i + u_d; //This will become the MotorValue
Roooos 10:93957c339972 195 }
Roooos 10:93957c339972 196
Roooos 10:93957c339972 197 double PID_Controller2(double Error)
Roooos 10:93957c339972 198 {
Roooos 10:93957c339972 199 //Belt drive parameters
Roooos 10:93957c339972 200 double Ts = 0.002; //Sampling time 100 Hz
Roooos 10:93957c339972 201 double Kp = 11.1; // proportional gain
Roooos 10:93957c339972 202 double Ki = 2.24; //Integral gain
Roooos 10:93957c339972 203 double Kd = 1.1; //Differential gain
Roooos 10:93957c339972 204
Roooos 10:93957c339972 205 static double ErrorIntegral = 0;
Roooos 10:93957c339972 206 static double ErrorPrevious = Error;
Roooos 10:93957c339972 207 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Roooos 10:93957c339972 208
Roooos 10:93957c339972 209 //Proportional part:
Roooos 10:93957c339972 210 double u_k = Kp * Error;
Roooos 10:93957c339972 211
Roooos 10:93957c339972 212 //Integral part:
Roooos 10:93957c339972 213 ErrorIntegral = ErrorIntegral + Error*Ts;
Roooos 10:93957c339972 214 double u_i = Ki * ErrorIntegral;
Roooos 10:93957c339972 215
Roooos 10:93957c339972 216 //Derivative part:
Roooos 10:93957c339972 217 double ErrorDerivative = (Error - ErrorPrevious)/Ts;
Roooos 10:93957c339972 218 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
Roooos 10:93957c339972 219 double u_d = Kd * FilteredErrorDerivative;
Roooos 10:93957c339972 220 ErrorPrevious = Error;
Roooos 10:93957c339972 221
Roooos 10:93957c339972 222 // sum of parts and return it
Roooos 10:93957c339972 223 return u_k + u_i + u_d; //This will become the MotorValue
Roooos 10:93957c339972 224 }
Roooos 10:93957c339972 225
Roooos 10:93957c339972 226 //Ticker function set motorvalues
Roooos 10:93957c339972 227 void SetMotor(double MotorValue, double MotorValue2)
Roooos 10:93957c339972 228 {
Roooos 10:93957c339972 229 if (MotorValue >=0) {
Roooos 10:93957c339972 230 DirectionPin = 1;
Roooos 10:93957c339972 231 } else {
Roooos 10:93957c339972 232 DirectionPin = 0;
Roooos 10:93957c339972 233 }
Roooos 10:93957c339972 234
Roooos 10:93957c339972 235 if (fabs(MotorValue)>1) { // if error more than 1 radian, full duty cycle
Roooos 10:93957c339972 236 PwmPin = 1;
Roooos 10:93957c339972 237 } else {
Roooos 10:93957c339972 238 PwmPin = fabs(MotorValue);
Roooos 10:93957c339972 239 }
Roooos 10:93957c339972 240
Roooos 10:93957c339972 241 if (MotorValue2 >=0) {
Roooos 10:93957c339972 242 DirectionPin2 = 1;
Roooos 10:93957c339972 243 } else {
Roooos 10:93957c339972 244 DirectionPin2 = 0;
Roooos 10:93957c339972 245 }
Roooos 10:93957c339972 246
Roooos 10:93957c339972 247 if (fabs(MotorValue2)>1) { // if error more than 1 radian, full duty cycle
Roooos 10:93957c339972 248 PwmPin2 = 1;
Roooos 10:93957c339972 249 } else {
Roooos 10:93957c339972 250 PwmPin2 = fabs(MotorValue2);
Roooos 10:93957c339972 251 }
Roooos 10:93957c339972 252 }
Roooos 10:93957c339972 253
Roooos 10:93957c339972 254 // ----------------------------------------------------------------------------
Roooos 10:93957c339972 255 //Ticker function
Roooos 10:93957c339972 256 void MeasureAndControl(void)
Roooos 10:93957c339972 257 {
Roooos 11:dcc416dbe3ea 258
Roooos 10:93957c339972 259 //double PositionRef = GetReferencePosition();
Roooos 10:93957c339972 260 //double PositionRef2 = GetReferencePosition2();
Roooos 11:dcc416dbe3ea 261
Roooos 10:93957c339972 262 OperationStateMachine();
Roooos 10:93957c339972 263 double PositionMotor = GetActualPosition();
Roooos 10:93957c339972 264 double PositionMotor2 = GetActualPosition2();
Roooos 10:93957c339972 265 double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
Roooos 10:93957c339972 266 double MotorValue2 = PID_Controller2(PositionRef2 - PositionMotor2);
Roooos 10:93957c339972 267 SetMotor(MotorValue, MotorValue2);
Roooos 10:93957c339972 268 }
Roooos 10:93957c339972 269
Roooos 10:93957c339972 270
Roooos 10:93957c339972 271
Roooos 10:93957c339972 272 //-----------------------------------------------------------------------------
Roooos 10:93957c339972 273 int main()
Roooos 10:93957c339972 274 {
Roooos 10:93957c339972 275 LedGreen = 1;
Roooos 10:93957c339972 276 LedRed = 1;
Roooos 10:93957c339972 277 LedBlue = 1;
Roooos 10:93957c339972 278 pc.baud(115200);
Roooos 10:93957c339972 279 pc.printf("Hello World\n\r");
Roooos 10:93957c339972 280 TimerLoop.start();
Roooos 10:93957c339972 281 CurrentOperationState = OPWait;
Roooos 10:93957c339972 282 PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
Roooos 10:93957c339972 283 TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
Roooos 10:93957c339972 284 while (true) {
Roooos 10:93957c339972 285 }
Roooos 10:93957c339972 286 }