Sven van Wincoop / Mbed 2 deprecated Controller

Controller for 1 motor with button control

Dependencies:   MODSERIAL QEI biquadFilter mbed

Fork of Controller by Ruben Lucas

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?

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