Controller for 1 motor with button control
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of Controller by
Controller.cpp@10:fb52590deb29, 2018-10-31 (annotated)
- Committer:
- Sven_van_Wincoop
- Date:
- Wed Oct 31 12:09:46 2018 +0000
- Revision:
- 10:fb52590deb29
Controller with values adjusted for Ts=0.002
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Sven_van_Wincoop | 10:fb52590deb29 | 1 | #include "mbed.h" |
Sven_van_Wincoop | 10:fb52590deb29 | 2 | #include "math.h" |
Sven_van_Wincoop | 10:fb52590deb29 | 3 | #include "MODSERIAL.h" |
Sven_van_Wincoop | 10:fb52590deb29 | 4 | #include "QEI.h" |
Sven_van_Wincoop | 10:fb52590deb29 | 5 | #include "BiQuad.h" |
Sven_van_Wincoop | 10:fb52590deb29 | 6 | |
Sven_van_Wincoop | 10:fb52590deb29 | 7 | //Tickers |
Sven_van_Wincoop | 10:fb52590deb29 | 8 | Ticker TickerMeasureAndControl; |
Sven_van_Wincoop | 10:fb52590deb29 | 9 | Ticker TickerPrintToScreen; |
Sven_van_Wincoop | 10:fb52590deb29 | 10 | |
Sven_van_Wincoop | 10:fb52590deb29 | 11 | //Communication |
Sven_van_Wincoop | 10:fb52590deb29 | 12 | MODSERIAL pc(USBTX,USBRX); |
Sven_van_Wincoop | 10:fb52590deb29 | 13 | QEI Encoder(D10,D11,NC,32); |
Sven_van_Wincoop | 10:fb52590deb29 | 14 | |
Sven_van_Wincoop | 10:fb52590deb29 | 15 | //Global pin variables |
Sven_van_Wincoop | 10:fb52590deb29 | 16 | PwmOut PwmPin(D5); |
Sven_van_Wincoop | 10:fb52590deb29 | 17 | DigitalOut DirectionPin(D4); |
Sven_van_Wincoop | 10:fb52590deb29 | 18 | AnalogIn Potmeter1(A1); |
Sven_van_Wincoop | 10:fb52590deb29 | 19 | AnalogIn Potmeter2(A0); |
Sven_van_Wincoop | 10:fb52590deb29 | 20 | DigitalIn BUT1(D8); |
Sven_van_Wincoop | 10:fb52590deb29 | 21 | DigitalIn BUT2(D9); |
Sven_van_Wincoop | 10:fb52590deb29 | 22 | DigitalIn button(SW2); |
Sven_van_Wincoop | 10:fb52590deb29 | 23 | //Global variables |
Sven_van_Wincoop | 10:fb52590deb29 | 24 | volatile bool PrintFlag = false; |
Sven_van_Wincoop | 10:fb52590deb29 | 25 | |
Sven_van_Wincoop | 10:fb52590deb29 | 26 | //Global variables for printing on screen |
Sven_van_Wincoop | 10:fb52590deb29 | 27 | volatile float PosRefPrint; // for printing value on screen |
Sven_van_Wincoop | 10:fb52590deb29 | 28 | volatile float PosMotorPrint; // for printing value on screen |
Sven_van_Wincoop | 10:fb52590deb29 | 29 | volatile float ErrorPrint; |
Sven_van_Wincoop | 10:fb52590deb29 | 30 | |
Sven_van_Wincoop | 10:fb52590deb29 | 31 | //----------------------------------------------------------------------------- |
Sven_van_Wincoop | 10:fb52590deb29 | 32 | //The double-functions |
Sven_van_Wincoop | 10:fb52590deb29 | 33 | |
Sven_van_Wincoop | 10:fb52590deb29 | 34 | //Get reference position |
Sven_van_Wincoop | 10:fb52590deb29 | 35 | double GetReferencePosition() |
Sven_van_Wincoop | 10:fb52590deb29 | 36 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 37 | // This function set the reference position to determine the position of the signal. |
Sven_van_Wincoop | 10:fb52590deb29 | 38 | // a positive position is defined as a counterclockwise (CCW) rotation |
Sven_van_Wincoop | 10:fb52590deb29 | 39 | static double PositionRef = 0; |
Sven_van_Wincoop | 10:fb52590deb29 | 40 | if (button) |
Sven_van_Wincoop | 10:fb52590deb29 | 41 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 42 | if (BUT2 == false) |
Sven_van_Wincoop | 10:fb52590deb29 | 43 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 44 | if (PositionRef <= 0.5*(6.2830)) |
Sven_van_Wincoop | 10:fb52590deb29 | 45 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 46 | PositionRef += 0.001*(6.2830); |
Sven_van_Wincoop | 10:fb52590deb29 | 47 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 48 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 49 | |
Sven_van_Wincoop | 10:fb52590deb29 | 50 | if (BUT1 == false) |
Sven_van_Wincoop | 10:fb52590deb29 | 51 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 52 | if (PositionRef >= -0.5*(6.2830)) |
Sven_van_Wincoop | 10:fb52590deb29 | 53 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 54 | PositionRef -= 0.001*(6.2830); |
Sven_van_Wincoop | 10:fb52590deb29 | 55 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 56 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 57 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 58 | else |
Sven_van_Wincoop | 10:fb52590deb29 | 59 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 60 | if (PositionRef <= 0) |
Sven_van_Wincoop | 10:fb52590deb29 | 61 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 62 | PositionRef += 0.001*(6.2830); |
Sven_van_Wincoop | 10:fb52590deb29 | 63 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 64 | |
Sven_van_Wincoop | 10:fb52590deb29 | 65 | if (PositionRef >= 0) |
Sven_van_Wincoop | 10:fb52590deb29 | 66 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 67 | PositionRef -= 0.001*(6.2830); |
Sven_van_Wincoop | 10:fb52590deb29 | 68 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 69 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 70 | // double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1) |
Sven_van_Wincoop | 10:fb52590deb29 | 71 | |
Sven_van_Wincoop | 10:fb52590deb29 | 72 | |
Sven_van_Wincoop | 10:fb52590deb29 | 73 | //double PositionRef = 0.8*6.2830*ValuePot - 0.8*3.1415; // position reference ranging from -0.4 rotations till 0.4 rotations |
Sven_van_Wincoop | 10:fb52590deb29 | 74 | |
Sven_van_Wincoop | 10:fb52590deb29 | 75 | return PositionRef; //rad |
Sven_van_Wincoop | 10:fb52590deb29 | 76 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 77 | |
Sven_van_Wincoop | 10:fb52590deb29 | 78 | // actual position of the motor |
Sven_van_Wincoop | 10:fb52590deb29 | 79 | double GetActualPosition() |
Sven_van_Wincoop | 10:fb52590deb29 | 80 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 81 | //This function determines the actual position of the motor |
Sven_van_Wincoop | 10:fb52590deb29 | 82 | //The count:radians relation is 8400:2pi |
Sven_van_Wincoop | 10:fb52590deb29 | 83 | double EncoderCounts = Encoder.getPulses(); //number of counts |
Sven_van_Wincoop | 10:fb52590deb29 | 84 | double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding |
Sven_van_Wincoop | 10:fb52590deb29 | 85 | |
Sven_van_Wincoop | 10:fb52590deb29 | 86 | return PositionMotor; |
Sven_van_Wincoop | 10:fb52590deb29 | 87 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 88 | |
Sven_van_Wincoop | 10:fb52590deb29 | 89 | |
Sven_van_Wincoop | 10:fb52590deb29 | 90 | |
Sven_van_Wincoop | 10:fb52590deb29 | 91 | ///The controller |
Sven_van_Wincoop | 10:fb52590deb29 | 92 | double PID_Controller(double Error) |
Sven_van_Wincoop | 10:fb52590deb29 | 93 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 94 | //Belt drive parameters |
Sven_van_Wincoop | 10:fb52590deb29 | 95 | double Ts = 0.002; //Sampling time 100 Hz |
Sven_van_Wincoop | 10:fb52590deb29 | 96 | double Kp = 11.1; // proportional gain. Model value: 11.1 |
Sven_van_Wincoop | 10:fb52590deb29 | 97 | double Ki = 22.81; //Integral gain. Model value: 11.2 |
Sven_van_Wincoop | 10:fb52590deb29 | 98 | double Kd = 1.7; //Differential gain. Model value: 5.5 |
Sven_van_Wincoop | 10:fb52590deb29 | 99 | |
Sven_van_Wincoop | 10:fb52590deb29 | 100 | //Arm drive parameters |
Sven_van_Wincoop | 10:fb52590deb29 | 101 | //double Ts = 0.002; //Sampling time 100 Hz |
Sven_van_Wincoop | 10:fb52590deb29 | 102 | //double Kp = 19.8; // proportional gain |
Sven_van_Wincoop | 10:fb52590deb29 | 103 | //double Ki = 40.9; //Intergral gain. Model value: 19.9 |
Sven_van_Wincoop | 10:fb52590deb29 | 104 | //double Kd = 3; //Differential gain. Model value: 9.8 |
Sven_van_Wincoop | 10:fb52590deb29 | 105 | |
Sven_van_Wincoop | 10:fb52590deb29 | 106 | static double ErrorIntegral = 0; |
Sven_van_Wincoop | 10:fb52590deb29 | 107 | static double ErrorPrevious = Error; |
Sven_van_Wincoop | 10:fb52590deb29 | 108 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Sven_van_Wincoop | 10:fb52590deb29 | 109 | |
Sven_van_Wincoop | 10:fb52590deb29 | 110 | //Proportional part: |
Sven_van_Wincoop | 10:fb52590deb29 | 111 | double u_k = Kp * Error; |
Sven_van_Wincoop | 10:fb52590deb29 | 112 | |
Sven_van_Wincoop | 10:fb52590deb29 | 113 | //Integral part: |
Sven_van_Wincoop | 10:fb52590deb29 | 114 | ErrorIntegral = ErrorIntegral + Error*Ts; |
Sven_van_Wincoop | 10:fb52590deb29 | 115 | double u_i = Ki * ErrorIntegral; // |
Sven_van_Wincoop | 10:fb52590deb29 | 116 | |
Sven_van_Wincoop | 10:fb52590deb29 | 117 | //Derivative part: |
Sven_van_Wincoop | 10:fb52590deb29 | 118 | double ErrorDerivative = (Error - ErrorPrevious)/Ts;//Kd must be made smaller to compensate for the higher Ts |
Sven_van_Wincoop | 10:fb52590deb29 | 119 | double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); |
Sven_van_Wincoop | 10:fb52590deb29 | 120 | double u_d = Kd * FilteredErrorDerivative; |
Sven_van_Wincoop | 10:fb52590deb29 | 121 | ErrorPrevious = Error; |
Sven_van_Wincoop | 10:fb52590deb29 | 122 | |
Sven_van_Wincoop | 10:fb52590deb29 | 123 | // sum of parts and return it |
Sven_van_Wincoop | 10:fb52590deb29 | 124 | return u_k + u_i + u_d; //This will become the MotorValue |
Sven_van_Wincoop | 10:fb52590deb29 | 125 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 126 | |
Sven_van_Wincoop | 10:fb52590deb29 | 127 | //Ticker function set motorvalues |
Sven_van_Wincoop | 10:fb52590deb29 | 128 | void SetMotor(double MotorValue) |
Sven_van_Wincoop | 10:fb52590deb29 | 129 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 130 | if (MotorValue >=0) |
Sven_van_Wincoop | 10:fb52590deb29 | 131 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 132 | DirectionPin = 1; |
Sven_van_Wincoop | 10:fb52590deb29 | 133 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 134 | else |
Sven_van_Wincoop | 10:fb52590deb29 | 135 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 136 | DirectionPin = 0; |
Sven_van_Wincoop | 10:fb52590deb29 | 137 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 138 | |
Sven_van_Wincoop | 10:fb52590deb29 | 139 | if (fabs(MotorValue)>1) // if error more than 1 radian, full duty cycle |
Sven_van_Wincoop | 10:fb52590deb29 | 140 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 141 | PwmPin = 1; |
Sven_van_Wincoop | 10:fb52590deb29 | 142 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 143 | else |
Sven_van_Wincoop | 10:fb52590deb29 | 144 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 145 | PwmPin = fabs(MotorValue); |
Sven_van_Wincoop | 10:fb52590deb29 | 146 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 147 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 148 | |
Sven_van_Wincoop | 10:fb52590deb29 | 149 | // ---------------------------------------------------------------------------- |
Sven_van_Wincoop | 10:fb52590deb29 | 150 | //Ticker function |
Sven_van_Wincoop | 10:fb52590deb29 | 151 | void MeasureAndControl(void) |
Sven_van_Wincoop | 10:fb52590deb29 | 152 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 153 | double PositionRef = GetReferencePosition(); |
Sven_van_Wincoop | 10:fb52590deb29 | 154 | double PositionMotor = GetActualPosition(); |
Sven_van_Wincoop | 10:fb52590deb29 | 155 | double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error |
Sven_van_Wincoop | 10:fb52590deb29 | 156 | SetMotor(MotorValue); |
Sven_van_Wincoop | 10:fb52590deb29 | 157 | |
Sven_van_Wincoop | 10:fb52590deb29 | 158 | //for printing on screen |
Sven_van_Wincoop | 10:fb52590deb29 | 159 | PosRefPrint = PositionRef; |
Sven_van_Wincoop | 10:fb52590deb29 | 160 | PosMotorPrint = PositionMotor; |
Sven_van_Wincoop | 10:fb52590deb29 | 161 | ErrorPrint = PositionRef - PositionMotor; |
Sven_van_Wincoop | 10:fb52590deb29 | 162 | |
Sven_van_Wincoop | 10:fb52590deb29 | 163 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 164 | |
Sven_van_Wincoop | 10:fb52590deb29 | 165 | |
Sven_van_Wincoop | 10:fb52590deb29 | 166 | |
Sven_van_Wincoop | 10:fb52590deb29 | 167 | void PrintToScreen() |
Sven_van_Wincoop | 10:fb52590deb29 | 168 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 169 | PrintFlag = true; |
Sven_van_Wincoop | 10:fb52590deb29 | 170 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 171 | |
Sven_van_Wincoop | 10:fb52590deb29 | 172 | |
Sven_van_Wincoop | 10:fb52590deb29 | 173 | //----------------------------------------------------------------------------- |
Sven_van_Wincoop | 10:fb52590deb29 | 174 | int main() |
Sven_van_Wincoop | 10:fb52590deb29 | 175 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 176 | pc.baud(115200); |
Sven_van_Wincoop | 10:fb52590deb29 | 177 | pc.printf("Hello World\n\r"); |
Sven_van_Wincoop | 10:fb52590deb29 | 178 | PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!) |
Sven_van_Wincoop | 10:fb52590deb29 | 179 | TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz |
Sven_van_Wincoop | 10:fb52590deb29 | 180 | TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen |
Sven_van_Wincoop | 10:fb52590deb29 | 181 | |
Sven_van_Wincoop | 10:fb52590deb29 | 182 | while (true) |
Sven_van_Wincoop | 10:fb52590deb29 | 183 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 184 | if(PrintFlag) // if-statement for printing every second four times to screen |
Sven_van_Wincoop | 10:fb52590deb29 | 185 | { |
Sven_van_Wincoop | 10:fb52590deb29 | 186 | double KpPrint = 3.52; |
Sven_van_Wincoop | 10:fb52590deb29 | 187 | double KiPrint = 0.88; |
Sven_van_Wincoop | 10:fb52590deb29 | 188 | double KdPrint = 6.9; |
Sven_van_Wincoop | 10:fb52590deb29 | 189 | pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f, Ki = %f and Kd = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint,KdPrint); |
Sven_van_Wincoop | 10:fb52590deb29 | 190 | PrintFlag = false; |
Sven_van_Wincoop | 10:fb52590deb29 | 191 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 192 | } |
Sven_van_Wincoop | 10:fb52590deb29 | 193 | } |