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@3:f1fc5216f6a5, 2018-10-19 (annotated)

Committer:
rubenlucas
Date:
Fri Oct 19 11:04:43 2018 +0000
Revision:
3:f1fc5216f6a5
Parent:
2:0a61483f4515
Child:
4:0251290a2fc0
Correct P-Controller, motor makes same amount of rotations as printed on screen.

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