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

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