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@5:7007230db09c, 2018-10-19 (annotated)

Committer:
rubenlucas
Date:
Fri Oct 19 13:03:56 2018 +0000
Revision:
5:7007230db09c
Parent:
4:0251290a2fc0
Child:
7:d63ec7fe96ae
PID-controller, is different then PI, oscillates much

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);
rubenlucas 0:ce44e2a8e87a 20
rubenlucas 0:ce44e2a8e87a 21 //Global variables
rubenlucas 0:ce44e2a8e87a 22 volatile bool PrintFlag = false;
rubenlucas 0:ce44e2a8e87a 23
rubenlucas 1:76e57b695115 24 //Global variables for printing on screen
rubenlucas 0:ce44e2a8e87a 25 volatile float PosRefPrint; // for printing value on screen
rubenlucas 0:ce44e2a8e87a 26 volatile float PosMotorPrint; // for printing value on screen
rubenlucas 1:76e57b695115 27 volatile float ErrorPrint;
rubenlucas 4:0251290a2fc0 28
rubenlucas 0:ce44e2a8e87a 29 //-----------------------------------------------------------------------------
rubenlucas 0:ce44e2a8e87a 30 //The double-functions
rubenlucas 0:ce44e2a8e87a 31
rubenlucas 0:ce44e2a8e87a 32 //Get reference position
rubenlucas 0:ce44e2a8e87a 33 double GetReferencePosition()
rubenlucas 0:ce44e2a8e87a 34 {
rubenlucas 0:ce44e2a8e87a 35 // This function set the reference position to determine the position of the signal.
rubenlucas 3:f1fc5216f6a5 36 // a positive position is defined as a counterclockwise (CCW) rotation
rubenlucas 0:ce44e2a8e87a 37
rubenlucas 0:ce44e2a8e87a 38
rubenlucas 2:0a61483f4515 39
rubenlucas 0:ce44e2a8e87a 40 double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
rubenlucas 0:ce44e2a8e87a 41
rubenlucas 0:ce44e2a8e87a 42
rubenlucas 2:0a61483f4515 43 double PositionRef = 3*6.2830*ValuePot - 3*3.1415; // position reference ranging from -1.5 rotations till 1.5 rotations
rubenlucas 0:ce44e2a8e87a 44
rubenlucas 0:ce44e2a8e87a 45 return PositionRef; //rad
rubenlucas 0:ce44e2a8e87a 46 }
rubenlucas 0:ce44e2a8e87a 47
rubenlucas 0:ce44e2a8e87a 48 // actual position of the motor
rubenlucas 0:ce44e2a8e87a 49 double GetActualPosition()
rubenlucas 0:ce44e2a8e87a 50 {
rubenlucas 0:ce44e2a8e87a 51 //This function determines the actual position of the motor
rubenlucas 0:ce44e2a8e87a 52 //The count:radians relation is 8400:2pi
rubenlucas 0:ce44e2a8e87a 53 double EncoderCounts = Encoder.getPulses(); //number of counts
rubenlucas 3:f1fc5216f6a5 54 double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding
rubenlucas 0:ce44e2a8e87a 55
rubenlucas 0:ce44e2a8e87a 56 return PositionMotor;
rubenlucas 0:ce44e2a8e87a 57 }
rubenlucas 0:ce44e2a8e87a 58
rubenlucas 0:ce44e2a8e87a 59
rubenlucas 0:ce44e2a8e87a 60
rubenlucas 0:ce44e2a8e87a 61 ///The controller
rubenlucas 5:7007230db09c 62 double PID_Controller(double Error)
rubenlucas 1:76e57b695115 63 {
rubenlucas 4:0251290a2fc0 64
rubenlucas 4:0251290a2fc0 65 double Ts = 0.01; //Sampling time 100 Hz
rubenlucas 4:0251290a2fc0 66 double Kp = 2; // proportional gain
rubenlucas 5:7007230db09c 67 double Ki = 0.85;
rubenlucas 5:7007230db09c 68 double Kd = 20*Potmeter2.read(); //integral gain ranging from 0-20.
rubenlucas 4:0251290a2fc0 69 static double ErrorIntegral = 0;
rubenlucas 5:7007230db09c 70 static double ErrorPrevious = Error;
rubenlucas 5:7007230db09c 71 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 1:76e57b695115 72
rubenlucas 4:0251290a2fc0 73 //Proportional part:
rubenlucas 1:76e57b695115 74 double u_k = Kp * Error;
rubenlucas 1:76e57b695115 75
rubenlucas 4:0251290a2fc0 76 //Integral part:
rubenlucas 4:0251290a2fc0 77 ErrorIntegral = ErrorIntegral + Error*Ts;
rubenlucas 4:0251290a2fc0 78 double u_i = Ki * ErrorIntegral;
rubenlucas 5:7007230db09c 79
rubenlucas 5:7007230db09c 80 //Derivative part:
rubenlucas 5:7007230db09c 81 double ErrorDerivative = (Error - ErrorPrevious)/Ts;
rubenlucas 5:7007230db09c 82 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 5:7007230db09c 83 double u_d = Kd * FilteredErrorDerivative;
rubenlucas 5:7007230db09c 84 ErrorPrevious = Error;
rubenlucas 5:7007230db09c 85
rubenlucas 5:7007230db09c 86 // sum of parts and return it
rubenlucas 5:7007230db09c 87 return u_k + u_i + u_d; //This will become the MotorValue
rubenlucas 1:76e57b695115 88 }
rubenlucas 1:76e57b695115 89
rubenlucas 1:76e57b695115 90 //Ticker function set motorvalues
rubenlucas 1:76e57b695115 91 void SetMotor(double MotorValue)
rubenlucas 1:76e57b695115 92 {
rubenlucas 1:76e57b695115 93 if (MotorValue >=0)
rubenlucas 1:76e57b695115 94 {
rubenlucas 1:76e57b695115 95 DirectionPin = 1;
rubenlucas 1:76e57b695115 96 }
rubenlucas 1:76e57b695115 97 else
rubenlucas 1:76e57b695115 98 {
rubenlucas 1:76e57b695115 99 DirectionPin = 0;
rubenlucas 1:76e57b695115 100 }
rubenlucas 0:ce44e2a8e87a 101
rubenlucas 2:0a61483f4515 102 if (fabs(MotorValue)>1) // if error more than 1 radian, full duty cycle
rubenlucas 1:76e57b695115 103 {
rubenlucas 2:0a61483f4515 104 PwmPin = 1;
rubenlucas 1:76e57b695115 105 }
rubenlucas 1:76e57b695115 106 else
rubenlucas 1:76e57b695115 107 {
rubenlucas 1:76e57b695115 108 PwmPin = fabs(MotorValue);
rubenlucas 1:76e57b695115 109 }
rubenlucas 1:76e57b695115 110 }
rubenlucas 0:ce44e2a8e87a 111
rubenlucas 0:ce44e2a8e87a 112 // ----------------------------------------------------------------------------
rubenlucas 1:76e57b695115 113 //Ticker function
rubenlucas 0:ce44e2a8e87a 114 void MeasureAndControl(void)
rubenlucas 0:ce44e2a8e87a 115 {
rubenlucas 0:ce44e2a8e87a 116 double PositionRef = GetReferencePosition();
rubenlucas 0:ce44e2a8e87a 117 double PositionMotor = GetActualPosition();
rubenlucas 5:7007230db09c 118 double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error
rubenlucas 1:76e57b695115 119 SetMotor(MotorValue);
rubenlucas 0:ce44e2a8e87a 120
rubenlucas 1:76e57b695115 121 //for printing on screen
rubenlucas 0:ce44e2a8e87a 122 PosRefPrint = PositionRef;
rubenlucas 0:ce44e2a8e87a 123 PosMotorPrint = PositionMotor;
rubenlucas 4:0251290a2fc0 124 ErrorPrint = PositionRef - PositionMotor;
rubenlucas 1:76e57b695115 125
rubenlucas 0:ce44e2a8e87a 126 }
rubenlucas 0:ce44e2a8e87a 127
rubenlucas 0:ce44e2a8e87a 128
rubenlucas 0:ce44e2a8e87a 129
rubenlucas 0:ce44e2a8e87a 130 void PrintToScreen()
rubenlucas 0:ce44e2a8e87a 131 {
rubenlucas 0:ce44e2a8e87a 132 PrintFlag = true;
rubenlucas 0:ce44e2a8e87a 133 }
rubenlucas 0:ce44e2a8e87a 134
rubenlucas 0:ce44e2a8e87a 135
rubenlucas 0:ce44e2a8e87a 136 //-----------------------------------------------------------------------------
rubenlucas 0:ce44e2a8e87a 137 int main()
rubenlucas 0:ce44e2a8e87a 138 {
rubenlucas 0:ce44e2a8e87a 139 pc.baud(115200);
rubenlucas 0:ce44e2a8e87a 140 pc.printf("Hello World\n\r");
rubenlucas 1:76e57b695115 141 PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
rubenlucas 0:ce44e2a8e87a 142 TickerMeasureAndControl.attach(&MeasureAndControl,0.01); //100 Hz
rubenlucas 1:76e57b695115 143 TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
rubenlucas 0:ce44e2a8e87a 144
rubenlucas 0:ce44e2a8e87a 145 while (true)
rubenlucas 0:ce44e2a8e87a 146 {
rubenlucas 1:76e57b695115 147 if(PrintFlag) // if-statement for printing every second four times to screen
rubenlucas 0:ce44e2a8e87a 148 {
rubenlucas 4:0251290a2fc0 149 double KpPrint = 2;
rubenlucas 5:7007230db09c 150 double KiPrint = 0.86;
rubenlucas 5:7007230db09c 151 double KdPrint = 20*Potmeter2.read();
rubenlucas 5:7007230db09c 152 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 153 PrintFlag = false;
rubenlucas 0:ce44e2a8e87a 154 }
rubenlucas 0:ce44e2a8e87a 155 }
rubenlucas 0:ce44e2a8e87a 156 }