Sven van Wincoop / Mbed 2 deprecated Controller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL QEI biquadFilter mbed

Fork of Controller by Ruben Lucas

main.cpp@9:34e3dd8548c0, 2018-10-29 (annotated)

Committer:
Sven_van_Wincoop
Date:
Mon Oct 29 12:12:18 2018 +0000
Revision:
9:34e3dd8548c0
Parent:
8:0ed8609252d3 
Controller with final controller values.

Who changed what in which revision?

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