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@4:0251290a2fc0, 2018-10-19 (annotated)

Committer:
rubenlucas
Date:
Fri Oct 19 12:02:58 2018 +0000
Revision:
4:0251290a2fc0
Parent:
3:f1fc5216f6a5
Child:
5:7007230db09c
Child:
6:10ac8c7cac26
works PI-Controllers, Error is reduced to almost zero, takes way longer to achieve.

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