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@1:76e57b695115, 2018-10-15 (annotated)

Committer:
rubenlucas
Date:
Mon Oct 15 14:57:07 2018 +0000
Revision:
1:76e57b695115
Parent:
0:ce44e2a8e87a
Child:
2:0a61483f4515
working including with motor and encoder. but potmeter1 still velocity control instead of position control

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 DigitalIn button1(D8);
rubenlucas 0:ce44e2a8e87a 19
rubenlucas 0:ce44e2a8e87a 20 //Global variables
rubenlucas 0:ce44e2a8e87a 21 const double Ts = 0.01; //Sample time of Ticker measure and control (100 Hz)
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 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 0:ce44e2a8e87a 35 // Reference velocity is set as proportional to duty cycle.
rubenlucas 0:ce44e2a8e87a 36 // Positive velocity (if button is pressed) means clockwise(CW) rotation.
rubenlucas 0:ce44e2a8e87a 37
rubenlucas 0:ce44e2a8e87a 38
rubenlucas 0:ce44e2a8e87a 39 const double MaxVelocity = 6.28; //60 RPM max velocity in rad/s
rubenlucas 0:ce44e2a8e87a 40 double VelocityRef; // Reference Velocity
rubenlucas 0:ce44e2a8e87a 41 double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
rubenlucas 0:ce44e2a8e87a 42 static double PositionRef = 0; // Initial position value in rad
rubenlucas 0:ce44e2a8e87a 43
rubenlucas 0:ce44e2a8e87a 44 if (button1)
rubenlucas 0:ce44e2a8e87a 45 {
rubenlucas 0:ce44e2a8e87a 46 VelocityRef = ValuePot*MaxVelocity; //CW
rubenlucas 0:ce44e2a8e87a 47 }
rubenlucas 0:ce44e2a8e87a 48 else
rubenlucas 0:ce44e2a8e87a 49 {
rubenlucas 0:ce44e2a8e87a 50 VelocityRef = -1*ValuePot*MaxVelocity; //CCW
rubenlucas 0:ce44e2a8e87a 51 }
rubenlucas 0:ce44e2a8e87a 52
rubenlucas 0:ce44e2a8e87a 53 PositionRef = PositionRef + VelocityRef*Ts;
rubenlucas 0:ce44e2a8e87a 54
rubenlucas 0:ce44e2a8e87a 55 return PositionRef; //rad
rubenlucas 0:ce44e2a8e87a 56 }
rubenlucas 0:ce44e2a8e87a 57
rubenlucas 0:ce44e2a8e87a 58 // actual position of the motor
rubenlucas 0:ce44e2a8e87a 59 double GetActualPosition()
rubenlucas 0:ce44e2a8e87a 60 {
rubenlucas 0:ce44e2a8e87a 61 //This function determines the actual position of the motor
rubenlucas 0:ce44e2a8e87a 62 //The count:radians relation is 8400:2pi
rubenlucas 0:ce44e2a8e87a 63 double EncoderCounts = Encoder.getPulses(); //number of counts
rubenlucas 0:ce44e2a8e87a 64 double PositionMotor = EncoderCounts/8400*(6.283); // in rad (6.283 = pi)
rubenlucas 0:ce44e2a8e87a 65
rubenlucas 0:ce44e2a8e87a 66 return PositionMotor;
rubenlucas 0:ce44e2a8e87a 67 }
rubenlucas 0:ce44e2a8e87a 68
rubenlucas 0:ce44e2a8e87a 69
rubenlucas 0:ce44e2a8e87a 70
rubenlucas 0:ce44e2a8e87a 71 ///The controller
rubenlucas 1:76e57b695115 72 double P_Controller(double Error)
rubenlucas 1:76e57b695115 73 {
rubenlucas 1:76e57b695115 74 double Kp = 35*Potmeter2.read(); // 35 is just a try
rubenlucas 1:76e57b695115 75
rubenlucas 1:76e57b695115 76 double u_k = Kp * Error;
rubenlucas 1:76e57b695115 77
rubenlucas 1:76e57b695115 78 return u_k; //This will become the MotorValue
rubenlucas 1:76e57b695115 79 }
rubenlucas 1:76e57b695115 80
rubenlucas 1:76e57b695115 81 //Ticker function set motorvalues
rubenlucas 1:76e57b695115 82 void SetMotor(double MotorValue)
rubenlucas 1:76e57b695115 83 {
rubenlucas 1:76e57b695115 84 if (MotorValue >=0)
rubenlucas 1:76e57b695115 85 {
rubenlucas 1:76e57b695115 86 DirectionPin = 1;
rubenlucas 1:76e57b695115 87 }
rubenlucas 1:76e57b695115 88 else
rubenlucas 1:76e57b695115 89 {
rubenlucas 1:76e57b695115 90 DirectionPin = 0;
rubenlucas 1:76e57b695115 91 }
rubenlucas 0:ce44e2a8e87a 92
rubenlucas 1:76e57b695115 93 if (fabs(MotorValue)>1)
rubenlucas 1:76e57b695115 94 {
rubenlucas 1:76e57b695115 95 PwmPin = 1; // if error more than 1 radian, full duty cycle
rubenlucas 1:76e57b695115 96 }
rubenlucas 1:76e57b695115 97 else
rubenlucas 1:76e57b695115 98 {
rubenlucas 1:76e57b695115 99 PwmPin = fabs(MotorValue);
rubenlucas 1:76e57b695115 100 }
rubenlucas 1:76e57b695115 101 }
rubenlucas 0:ce44e2a8e87a 102
rubenlucas 0:ce44e2a8e87a 103 // ----------------------------------------------------------------------------
rubenlucas 1:76e57b695115 104 //Ticker function
rubenlucas 0:ce44e2a8e87a 105 void MeasureAndControl(void)
rubenlucas 0:ce44e2a8e87a 106 {
rubenlucas 0:ce44e2a8e87a 107 double PositionRef = GetReferencePosition();
rubenlucas 0:ce44e2a8e87a 108 double PositionMotor = GetActualPosition();
rubenlucas 1:76e57b695115 109 double MotorValue = P_Controller(PositionRef - PositionMotor); // input is error
rubenlucas 1:76e57b695115 110 SetMotor(MotorValue);
rubenlucas 0:ce44e2a8e87a 111
rubenlucas 1:76e57b695115 112 //for printing on screen
rubenlucas 0:ce44e2a8e87a 113 PosRefPrint = PositionRef;
rubenlucas 0:ce44e2a8e87a 114 PosMotorPrint = PositionMotor;
rubenlucas 1:76e57b695115 115 ErrorPrint = MotorValue;
rubenlucas 1:76e57b695115 116
rubenlucas 0:ce44e2a8e87a 117 }
rubenlucas 0:ce44e2a8e87a 118
rubenlucas 0:ce44e2a8e87a 119
rubenlucas 0:ce44e2a8e87a 120
rubenlucas 0:ce44e2a8e87a 121 void PrintToScreen()
rubenlucas 0:ce44e2a8e87a 122 {
rubenlucas 0:ce44e2a8e87a 123 PrintFlag = true;
rubenlucas 0:ce44e2a8e87a 124 }
rubenlucas 0:ce44e2a8e87a 125
rubenlucas 0:ce44e2a8e87a 126
rubenlucas 0:ce44e2a8e87a 127 //-----------------------------------------------------------------------------
rubenlucas 0:ce44e2a8e87a 128 int main()
rubenlucas 0:ce44e2a8e87a 129 {
rubenlucas 0:ce44e2a8e87a 130 pc.baud(115200);
rubenlucas 0:ce44e2a8e87a 131 pc.printf("Hello World\n\r");
rubenlucas 1:76e57b695115 132 PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!)
rubenlucas 0:ce44e2a8e87a 133 TickerMeasureAndControl.attach(&MeasureAndControl,0.01); //100 Hz
rubenlucas 1:76e57b695115 134 TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen
rubenlucas 0:ce44e2a8e87a 135
rubenlucas 0:ce44e2a8e87a 136 while (true)
rubenlucas 0:ce44e2a8e87a 137 {
rubenlucas 1:76e57b695115 138 if(PrintFlag) // if-statement for printing every second four times to screen
rubenlucas 0:ce44e2a8e87a 139 {
rubenlucas 1:76e57b695115 140 double KpPrint = 35*Potmeter2.read();
rubenlucas 1:76e57b695115 141 pc.printf("Pos ref = %f, Pos motor = %f, Error = %f and Kp = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint);
rubenlucas 0:ce44e2a8e87a 142 PrintFlag = false;
rubenlucas 0:ce44e2a8e87a 143 }
rubenlucas 0:ce44e2a8e87a 144 }
rubenlucas 0:ce44e2a8e87a 145 }