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@7:d63ec7fe96ae, 2018-10-26 (annotated)

Committer:
Sven_van_Wincoop
Date:
Fri Oct 26 10:52:12 2018 +0000
Revision:
7:d63ec7fe96ae
Parent:
5:7007230db09c
Child:
8:0ed8609252d3
Controller with suitbale values for robot

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 7:d63ec7fe96ae 46 PositionRef += 0.0005*(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 7:d63ec7fe96ae 54 PositionRef -= 0.0005*(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 7:d63ec7fe96ae 62 PositionRef += 0.0005*(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 7:d63ec7fe96ae 67 PositionRef -= 0.0005*(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 {
rubenlucas 4:0251290a2fc0 94
rubenlucas 4:0251290a2fc0 95 double Ts = 0.01; //Sampling time 100 Hz
Sven_van_Wincoop 7:d63ec7fe96ae 96 double Kp = 3.51; // proportional gain
Sven_van_Wincoop 7:d63ec7fe96ae 97 double Ki = 5;
Sven_van_Wincoop 7:d63ec7fe96ae 98 double Kd = 2.1; //integral gain ranging from 0-20.
rubenlucas 4:0251290a2fc0 99 static double ErrorIntegral = 0;
rubenlucas 5:7007230db09c 100 static double ErrorPrevious = Error;
rubenlucas 5:7007230db09c 101 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 1:76e57b695115 102
rubenlucas 4:0251290a2fc0 103 //Proportional part:
rubenlucas 1:76e57b695115 104 double u_k = Kp * Error;
rubenlucas 1:76e57b695115 105
rubenlucas 4:0251290a2fc0 106 //Integral part:
rubenlucas 4:0251290a2fc0 107 ErrorIntegral = ErrorIntegral + Error*Ts;
rubenlucas 4:0251290a2fc0 108 double u_i = Ki * ErrorIntegral;
rubenlucas 5:7007230db09c 109
rubenlucas 5:7007230db09c 110 //Derivative part:
rubenlucas 5:7007230db09c 111 double ErrorDerivative = (Error - ErrorPrevious)/Ts;
rubenlucas 5:7007230db09c 112 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 5:7007230db09c 113 double u_d = Kd * FilteredErrorDerivative;
rubenlucas 5:7007230db09c 114 ErrorPrevious = Error;
rubenlucas 5:7007230db09c 115
rubenlucas 5:7007230db09c 116 // sum of parts and return it
rubenlucas 5:7007230db09c 117 return u_k + u_i + u_d; //This will become the MotorValue
rubenlucas 1:76e57b695115 118 }
rubenlucas 1:76e57b695115 119
rubenlucas 1:76e57b695115 120 //Ticker function set motorvalues
rubenlucas 1:76e57b695115 121 void SetMotor(double MotorValue)
rubenlucas 1:76e57b695115 122 {
rubenlucas 1:76e57b695115 123 if (MotorValue >=0)
rubenlucas 1:76e57b695115 124 {
rubenlucas 1:76e57b695115 125 DirectionPin = 1;
rubenlucas 1:76e57b695115 126 }
rubenlucas 1:76e57b695115 127 else
rubenlucas 1:76e57b695115 128 {
rubenlucas 1:76e57b695115 129 DirectionPin = 0;
rubenlucas 1:76e57b695115 130 }
rubenlucas 0:ce44e2a8e87a 131
rubenlucas 2:0a61483f4515 132 if (fabs(MotorValue)>1) // if error more than 1 radian, full duty cycle
rubenlucas 1:76e57b695115 133 {
rubenlucas 2:0a61483f4515 134 PwmPin = 1;
rubenlucas 1:76e57b695115 135 }
rubenlucas 1:76e57b695115 136 else
rubenlucas 1:76e57b695115 137 {
rubenlucas 1:76e57b695115 138 PwmPin = fabs(MotorValue);
rubenlucas 1:76e57b695115 139 }
rubenlucas 1:76e57b695115 140 }
rubenlucas 0:ce44e2a8e87a 141
rubenlucas 0:ce44e2a8e87a 142 // ----------------------------------------------------------------------------
rubenlucas 1:76e57b695115 143 //Ticker function
rubenlucas 0:ce44e2a8e87a 144 void MeasureAndControl(void)
rubenlucas 0:ce44e2a8e87a 145 {
rubenlucas 0:ce44e2a8e87a 146 double PositionRef = GetReferencePosition();
rubenlucas 0:ce44e2a8e87a 147 double PositionMotor = GetActualPosition();
rubenlucas 5:7007230db09c 148 double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
rubenlucas 1:76e57b695115 149 SetMotor(MotorValue);
rubenlucas 0:ce44e2a8e87a 150
rubenlucas 1:76e57b695115 151 //for printing on screen
rubenlucas 0:ce44e2a8e87a 152 PosRefPrint = PositionRef;
rubenlucas 0:ce44e2a8e87a 153 PosMotorPrint = PositionMotor;
rubenlucas 4:0251290a2fc0 154 ErrorPrint = PositionRef - PositionMotor;
rubenlucas 1:76e57b695115 155
rubenlucas 0:ce44e2a8e87a 156 }
rubenlucas 0:ce44e2a8e87a 157
rubenlucas 0:ce44e2a8e87a 158
rubenlucas 0:ce44e2a8e87a 159
rubenlucas 0:ce44e2a8e87a 160 void PrintToScreen()
rubenlucas 0:ce44e2a8e87a 161 {
rubenlucas 0:ce44e2a8e87a 162 PrintFlag = true;
rubenlucas 0:ce44e2a8e87a 163 }
rubenlucas 0:ce44e2a8e87a 164
rubenlucas 0:ce44e2a8e87a 165
rubenlucas 0:ce44e2a8e87a 166 //-----------------------------------------------------------------------------
rubenlucas 0:ce44e2a8e87a 167 int main()
rubenlucas 0:ce44e2a8e87a 168 {
rubenlucas 0:ce44e2a8e87a 169 pc.baud(115200);
rubenlucas 0:ce44e2a8e87a 170 pc.printf("Hello World\n\r");
rubenlucas 1:76e57b695115 171 PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
Sven_van_Wincoop 7:d63ec7fe96ae 172 TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz
rubenlucas 1:76e57b695115 173 TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
rubenlucas 0:ce44e2a8e87a 174
rubenlucas 0:ce44e2a8e87a 175 while (true)
rubenlucas 0:ce44e2a8e87a 176 {
rubenlucas 1:76e57b695115 177 if(PrintFlag) // if-statement for printing every second four times to screen
rubenlucas 0:ce44e2a8e87a 178 {
Sven_van_Wincoop 7:d63ec7fe96ae 179 double KpPrint = 3.52;
Sven_van_Wincoop 7:d63ec7fe96ae 180 double KiPrint = 0.88;
Sven_van_Wincoop 7:d63ec7fe96ae 181 double KdPrint = 6.9;
rubenlucas 5:7007230db09c 182 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 183 PrintFlag = false;
rubenlucas 0:ce44e2a8e87a 184 }
rubenlucas 0:ce44e2a8e87a 185 }
rubenlucas 0:ce44e2a8e87a 186 }