Ruben Lucas / Mbed 2 deprecated StateMachinePTR

StateMachine PTR

Dependencies:   MODSERIAL QEI MovingAverage biquadFilter mbed

StateMachinePTR.cpp@2:c2ae5044ec82, 2018-10-29 (annotated)

Committer:
rubenlucas
Date:
Mon Oct 29 14:50:50 2018 +0000
Revision:
2:c2ae5044ec82
Child:
3:97cac1d5ba8a
first publish state machine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rubenlucas 2:c2ae5044ec82 1 #include "mbed.h"
rubenlucas 2:c2ae5044ec82 2 #include "math.h"
rubenlucas 2:c2ae5044ec82 3 #include "MODSERIAL.h"
rubenlucas 2:c2ae5044ec82 4 #include "QEI.h"
rubenlucas 2:c2ae5044ec82 5 #include "BiQuad.h"
rubenlucas 2:c2ae5044ec82 6
rubenlucas 2:c2ae5044ec82 7 //states
rubenlucas 2:c2ae5044ec82 8 enum States {Waiting, Homing, Emergency,EMGCal,MotorCal,Operation,Demo};
rubenlucas 2:c2ae5044ec82 9
rubenlucas 2:c2ae5044ec82 10 // Global variables
rubenlucas 2:c2ae5044ec82 11 States CurrentState;
rubenlucas 2:c2ae5044ec82 12 Ticker TickerLoop;
rubenlucas 2:c2ae5044ec82 13 Timer TimerLoop;
rubenlucas 2:c2ae5044ec82 14
rubenlucas 2:c2ae5044ec82 15 //Communication
rubenlucas 2:c2ae5044ec82 16 MODSERIAL pc(USBTX,USBRX);
rubenlucas 2:c2ae5044ec82 17 QEI Encoder(D10,D11,NC,32);
rubenlucas 2:c2ae5044ec82 18
rubenlucas 2:c2ae5044ec82 19 //Global pin variables Motor 1
rubenlucas 2:c2ae5044ec82 20 PwmOut PwmPin(D5);
rubenlucas 2:c2ae5044ec82 21 DigitalOut DirectionPin(D4);
rubenlucas 2:c2ae5044ec82 22 AnalogIn Potmeter1(A1);
rubenlucas 2:c2ae5044ec82 23 AnalogIn Potmeter2(A0);
rubenlucas 2:c2ae5044ec82 24
rubenlucas 2:c2ae5044ec82 25 //Output LED
rubenlucas 2:c2ae5044ec82 26 DigitalOut LedRed (LED_RED);
rubenlucas 2:c2ae5044ec82 27 DigitalOut LedGreen (LED_GREEN);
rubenlucas 2:c2ae5044ec82 28 DigitalOut LedBlue (LED_BLUE);
rubenlucas 2:c2ae5044ec82 29
rubenlucas 2:c2ae5044ec82 30
rubenlucas 2:c2ae5044ec82 31 // Buttons
rubenlucas 2:c2ae5044ec82 32 DigitalIn Button1(SW2);
rubenlucas 2:c2ae5044ec82 33 DigitalIn Button2(SW3);
rubenlucas 2:c2ae5044ec82 34 DigitalIn ButtonHome(D8);
rubenlucas 2:c2ae5044ec82 35
rubenlucas 2:c2ae5044ec82 36
rubenlucas 2:c2ae5044ec82 37 //Global variables for printing on screen
rubenlucas 2:c2ae5044ec82 38 volatile bool PrintFlag = false;
rubenlucas 2:c2ae5044ec82 39 volatile float PosRefPrint; // for printing value on screen
rubenlucas 2:c2ae5044ec82 40 volatile float PosMotorPrint; // for printing value on screen
rubenlucas 2:c2ae5044ec82 41 volatile float ErrorPrint;
rubenlucas 2:c2ae5044ec82 42 volatile int PrintCount = 0;
rubenlucas 2:c2ae5044ec82 43 volatile float MotorValuePrint;
rubenlucas 2:c2ae5044ec82 44
rubenlucas 2:c2ae5044ec82 45 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 46
rubenlucas 2:c2ae5044ec82 47 //Functions
rubenlucas 2:c2ae5044ec82 48 void StateMachine ()
rubenlucas 2:c2ae5044ec82 49 {
rubenlucas 2:c2ae5044ec82 50 switch (CurrentState)
rubenlucas 2:c2ae5044ec82 51 {
rubenlucas 2:c2ae5044ec82 52 case Waiting:
rubenlucas 2:c2ae5044ec82 53 LedRed = 0;
rubenlucas 2:c2ae5044ec82 54 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 55 LedBlue = 1; //Yellow
rubenlucas 2:c2ae5044ec82 56 break;
rubenlucas 2:c2ae5044ec82 57
rubenlucas 2:c2ae5044ec82 58 case Homing:
rubenlucas 2:c2ae5044ec82 59 LedRed = 0;
rubenlucas 2:c2ae5044ec82 60 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 61 LedBlue = 0; //White
rubenlucas 2:c2ae5044ec82 62 if (TimerLoop.read() >= 5.0f)
rubenlucas 2:c2ae5044ec82 63 {CurrentState = Waiting;}
rubenlucas 2:c2ae5044ec82 64
rubenlucas 2:c2ae5044ec82 65 break;
rubenlucas 2:c2ae5044ec82 66
rubenlucas 2:c2ae5044ec82 67 case Emergency:
rubenlucas 2:c2ae5044ec82 68 LedRed = 0;
rubenlucas 2:c2ae5044ec82 69 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 70 LedBlue = 1; //Red
rubenlucas 2:c2ae5044ec82 71 break;
rubenlucas 2:c2ae5044ec82 72
rubenlucas 2:c2ae5044ec82 73 case EMGCal:
rubenlucas 2:c2ae5044ec82 74 LedRed = 0;
rubenlucas 2:c2ae5044ec82 75 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 76 LedBlue = 0; //Pink
rubenlucas 2:c2ae5044ec82 77 break;
rubenlucas 2:c2ae5044ec82 78
rubenlucas 2:c2ae5044ec82 79 case MotorCal:
rubenlucas 2:c2ae5044ec82 80 LedRed = 1;
rubenlucas 2:c2ae5044ec82 81 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 82 LedBlue = 0; //Turqoise
rubenlucas 2:c2ae5044ec82 83 break;
rubenlucas 2:c2ae5044ec82 84
rubenlucas 2:c2ae5044ec82 85 case Operation:
rubenlucas 2:c2ae5044ec82 86 LedRed = 1;
rubenlucas 2:c2ae5044ec82 87 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 88 LedBlue = 1; //Green
rubenlucas 2:c2ae5044ec82 89 break;
rubenlucas 2:c2ae5044ec82 90
rubenlucas 2:c2ae5044ec82 91 case Demo:
rubenlucas 2:c2ae5044ec82 92 LedRed = 1;
rubenlucas 2:c2ae5044ec82 93 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 94 LedBlue = 0; //Blue
rubenlucas 2:c2ae5044ec82 95 break;
rubenlucas 2:c2ae5044ec82 96
rubenlucas 2:c2ae5044ec82 97
rubenlucas 2:c2ae5044ec82 98 }
rubenlucas 2:c2ae5044ec82 99 }
rubenlucas 2:c2ae5044ec82 100
rubenlucas 2:c2ae5044ec82 101 //-----------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 102 //The double-functions
rubenlucas 2:c2ae5044ec82 103
rubenlucas 2:c2ae5044ec82 104 //Get reference position
rubenlucas 2:c2ae5044ec82 105 double GetReferencePosition()
rubenlucas 2:c2ae5044ec82 106 {
rubenlucas 2:c2ae5044ec82 107 // This function set the reference position to determine the position of the signal.
rubenlucas 2:c2ae5044ec82 108 // a positive position is defined as a counterclockwise (CCW) rotation
rubenlucas 2:c2ae5044ec82 109
rubenlucas 2:c2ae5044ec82 110
rubenlucas 2:c2ae5044ec82 111
rubenlucas 2:c2ae5044ec82 112 double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
rubenlucas 2:c2ae5044ec82 113
rubenlucas 2:c2ae5044ec82 114
rubenlucas 2:c2ae5044ec82 115 double PositionRef = 3*6.2830*ValuePot - 3*3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations
rubenlucas 2:c2ae5044ec82 116
rubenlucas 2:c2ae5044ec82 117 return PositionRef; //rad
rubenlucas 2:c2ae5044ec82 118 }
rubenlucas 2:c2ae5044ec82 119
rubenlucas 2:c2ae5044ec82 120 // actual position of the motor
rubenlucas 2:c2ae5044ec82 121 double GetActualPosition()
rubenlucas 2:c2ae5044ec82 122 {
rubenlucas 2:c2ae5044ec82 123 //This function determines the actual position of the motor
rubenlucas 2:c2ae5044ec82 124 //The count:radians relation is 8400:2pi
rubenlucas 2:c2ae5044ec82 125 double EncoderCounts = Encoder.getPulses(); //number of counts
rubenlucas 2:c2ae5044ec82 126 double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
rubenlucas 2:c2ae5044ec82 127
rubenlucas 2:c2ae5044ec82 128 return PositionMotor;
rubenlucas 2:c2ae5044ec82 129 }
rubenlucas 2:c2ae5044ec82 130
rubenlucas 2:c2ae5044ec82 131
rubenlucas 2:c2ae5044ec82 132
rubenlucas 2:c2ae5044ec82 133 ///The controller
rubenlucas 2:c2ae5044ec82 134 double PID_Controller(double Error)
rubenlucas 2:c2ae5044ec82 135 {
rubenlucas 2:c2ae5044ec82 136
rubenlucas 2:c2ae5044ec82 137 double Ts = 0.005; //Sampling time 100 Hz
rubenlucas 2:c2ae5044ec82 138 double Kp = 5.34; // proportional gain
rubenlucas 2:c2ae5044ec82 139 double Ki = 2.0;//integral gain
rubenlucas 2:c2ae5044ec82 140 double Kd = 6.9; //derivative gain
rubenlucas 2:c2ae5044ec82 141 static double ErrorIntegral = 0;
rubenlucas 2:c2ae5044ec82 142 static double ErrorPrevious = Error;
rubenlucas 2:c2ae5044ec82 143 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 2:c2ae5044ec82 144
rubenlucas 2:c2ae5044ec82 145 //Proportional part:
rubenlucas 2:c2ae5044ec82 146 double u_k = Kp * Error;
rubenlucas 2:c2ae5044ec82 147
rubenlucas 2:c2ae5044ec82 148 //Integral part:
rubenlucas 2:c2ae5044ec82 149 ErrorIntegral = ErrorIntegral + Error*Ts;
rubenlucas 2:c2ae5044ec82 150 double u_i = Ki * ErrorIntegral;
rubenlucas 2:c2ae5044ec82 151
rubenlucas 2:c2ae5044ec82 152 //Derivative part:
rubenlucas 2:c2ae5044ec82 153 double ErrorDerivative = (Error - ErrorPrevious)/Ts;
rubenlucas 2:c2ae5044ec82 154 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 2:c2ae5044ec82 155 double u_d = Kd * FilteredErrorDerivative;
rubenlucas 2:c2ae5044ec82 156 ErrorPrevious = Error;
rubenlucas 2:c2ae5044ec82 157
rubenlucas 2:c2ae5044ec82 158 return u_k + u_i + u_d; //This will become the MotorValue
rubenlucas 2:c2ae5044ec82 159 }
rubenlucas 2:c2ae5044ec82 160
rubenlucas 2:c2ae5044ec82 161 //Ticker function set motorvalues
rubenlucas 2:c2ae5044ec82 162 void SetMotor(double MotorValue)
rubenlucas 2:c2ae5044ec82 163 {
rubenlucas 2:c2ae5044ec82 164 if (MotorValue >=0)
rubenlucas 2:c2ae5044ec82 165 {
rubenlucas 2:c2ae5044ec82 166 DirectionPin = 1;
rubenlucas 2:c2ae5044ec82 167 }
rubenlucas 2:c2ae5044ec82 168 else
rubenlucas 2:c2ae5044ec82 169 {
rubenlucas 2:c2ae5044ec82 170 DirectionPin = 0;
rubenlucas 2:c2ae5044ec82 171 }
rubenlucas 2:c2ae5044ec82 172
rubenlucas 2:c2ae5044ec82 173 if (fabs(MotorValue)>3) // if error more than 1 radian, full duty cycle
rubenlucas 2:c2ae5044ec82 174 {
rubenlucas 2:c2ae5044ec82 175 PwmPin = 1;
rubenlucas 2:c2ae5044ec82 176 }
rubenlucas 2:c2ae5044ec82 177 else
rubenlucas 2:c2ae5044ec82 178 {
rubenlucas 2:c2ae5044ec82 179 PwmPin = 0; //fabs(MotorValue);
rubenlucas 2:c2ae5044ec82 180 }
rubenlucas 2:c2ae5044ec82 181 }
rubenlucas 2:c2ae5044ec82 182
rubenlucas 2:c2ae5044ec82 183 void SetMotorOff()
rubenlucas 2:c2ae5044ec82 184 {
rubenlucas 2:c2ae5044ec82 185 PwmPin = 0; // Turn motor off
rubenlucas 2:c2ae5044ec82 186 }
rubenlucas 2:c2ae5044ec82 187
rubenlucas 2:c2ae5044ec82 188 //-----------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 189 void MeasureAndControl(void)
rubenlucas 2:c2ae5044ec82 190 {
rubenlucas 2:c2ae5044ec82 191 double PositionRef = GetReferencePosition();
rubenlucas 2:c2ae5044ec82 192 double PositionMotor = GetActualPosition();
rubenlucas 2:c2ae5044ec82 193 double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
rubenlucas 2:c2ae5044ec82 194 SetMotor(MotorValue);
rubenlucas 2:c2ae5044ec82 195
rubenlucas 2:c2ae5044ec82 196 //for printing on screen
rubenlucas 2:c2ae5044ec82 197 PosRefPrint = PositionRef;
rubenlucas 2:c2ae5044ec82 198 PosMotorPrint = PositionMotor;
rubenlucas 2:c2ae5044ec82 199 ErrorPrint = PositionRef - PositionMotor;
rubenlucas 2:c2ae5044ec82 200 MotorValuePrint = MotorValue;
rubenlucas 2:c2ae5044ec82 201 }
rubenlucas 2:c2ae5044ec82 202
rubenlucas 2:c2ae5044ec82 203
rubenlucas 2:c2ae5044ec82 204
rubenlucas 2:c2ae5044ec82 205 void PrintToScreen()
rubenlucas 2:c2ae5044ec82 206 {
rubenlucas 2:c2ae5044ec82 207 PrintCount++;
rubenlucas 2:c2ae5044ec82 208 if (PrintCount == 100) // printing with 2 Hz
rubenlucas 2:c2ae5044ec82 209 {PrintFlag = true; PrintCount = 0;}
rubenlucas 2:c2ae5044ec82 210 }
rubenlucas 2:c2ae5044ec82 211
rubenlucas 2:c2ae5044ec82 212
rubenlucas 2:c2ae5044ec82 213 // Execution function
rubenlucas 2:c2ae5044ec82 214 void LoopFunction()
rubenlucas 2:c2ae5044ec82 215 {
rubenlucas 2:c2ae5044ec82 216 // buttons
rubenlucas 2:c2ae5044ec82 217 if (Button1.read() == false) // if pressed
rubenlucas 2:c2ae5044ec82 218 {CurrentState = Operation; TimerLoop.reset();}
rubenlucas 2:c2ae5044ec82 219
rubenlucas 2:c2ae5044ec82 220 if (Button2.read() == false) // if pressed
rubenlucas 2:c2ae5044ec82 221 {CurrentState = Demo; TimerLoop.reset();}
rubenlucas 2:c2ae5044ec82 222
rubenlucas 2:c2ae5044ec82 223 if (ButtonHome.read() == false) // if pressed
rubenlucas 2:c2ae5044ec82 224 {CurrentState = Homing; TimerLoop.reset();}
rubenlucas 2:c2ae5044ec82 225 //functions
rubenlucas 2:c2ae5044ec82 226 StateMachine(); //determine states
rubenlucas 2:c2ae5044ec82 227 if (CurrentState >= 4)
rubenlucas 2:c2ae5044ec82 228 {MeasureAndControl(); PrintToScreen();}
rubenlucas 2:c2ae5044ec82 229 else
rubenlucas 2:c2ae5044ec82 230 {SetMotorOff();}
rubenlucas 2:c2ae5044ec82 231 }
rubenlucas 2:c2ae5044ec82 232
rubenlucas 2:c2ae5044ec82 233 int main()
rubenlucas 2:c2ae5044ec82 234 {
rubenlucas 2:c2ae5044ec82 235 pc.baud(115200);
rubenlucas 2:c2ae5044ec82 236 pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
rubenlucas 2:c2ae5044ec82 237 TimerLoop.start();
rubenlucas 2:c2ae5044ec82 238 CurrentState = Waiting; // set initial state as Waiting
rubenlucas 2:c2ae5044ec82 239 TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
rubenlucas 2:c2ae5044ec82 240
rubenlucas 2:c2ae5044ec82 241 while (true)
rubenlucas 2:c2ae5044ec82 242 {
rubenlucas 2:c2ae5044ec82 243 if(PrintFlag) // if-statement for printing every second four times to screen
rubenlucas 2:c2ae5044ec82 244 {
rubenlucas 2:c2ae5044ec82 245
rubenlucas 2:c2ae5044ec82 246 pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, motorvalue = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,MotorValuePrint);
rubenlucas 2:c2ae5044ec82 247 PrintFlag = false;
rubenlucas 2:c2ae5044ec82 248 }
rubenlucas 2:c2ae5044ec82 249 }
rubenlucas 2:c2ae5044ec82 250
rubenlucas 2:c2ae5044ec82 251 }