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@0:ce44e2a8e87a, 2018-10-15 (annotated)

Committer:
rubenlucas
Date:
Mon Oct 15 13:48:56 2018 +0000
Revision:
0:ce44e2a8e87a
Child:
1:76e57b695115
Works correctly, Prints Pos_ref and pos_motor to screen. Pos_motor still 0, no motor controlled yet

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 TickerSetMotor;
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 DigitalIn button1(D8);
rubenlucas 0:ce44e2a8e87a 20
rubenlucas 0:ce44e2a8e87a 21 //Global variables
rubenlucas 0:ce44e2a8e87a 22 const double Ts = 0.01; //Sample time of Ticker measure and control (100 Hz)
rubenlucas 0:ce44e2a8e87a 23 volatile bool PrintFlag = false;
rubenlucas 0:ce44e2a8e87a 24
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 0:ce44e2a8e87a 27 //-----------------------------------------------------------------------------
rubenlucas 0:ce44e2a8e87a 28 //The double-functions
rubenlucas 0:ce44e2a8e87a 29
rubenlucas 0:ce44e2a8e87a 30 //Get reference position
rubenlucas 0:ce44e2a8e87a 31 double GetReferencePosition()
rubenlucas 0:ce44e2a8e87a 32 {
rubenlucas 0:ce44e2a8e87a 33 // This function set the reference position to determine the position of the signal.
rubenlucas 0:ce44e2a8e87a 34 // Reference velocity is set as proportional to duty cycle.
rubenlucas 0:ce44e2a8e87a 35 // Positive velocity (if button is pressed) means clockwise(CW) rotation.
rubenlucas 0:ce44e2a8e87a 36
rubenlucas 0:ce44e2a8e87a 37
rubenlucas 0:ce44e2a8e87a 38 const double MaxVelocity = 6.28; //60 RPM max velocity in rad/s
rubenlucas 0:ce44e2a8e87a 39 double VelocityRef; // Reference Velocity
rubenlucas 0:ce44e2a8e87a 40 double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1)
rubenlucas 0:ce44e2a8e87a 41 static double PositionRef = 0; // Initial position value in rad
rubenlucas 0:ce44e2a8e87a 42
rubenlucas 0:ce44e2a8e87a 43 if (button1)
rubenlucas 0:ce44e2a8e87a 44 {
rubenlucas 0:ce44e2a8e87a 45 VelocityRef = ValuePot*MaxVelocity; //CW
rubenlucas 0:ce44e2a8e87a 46 }
rubenlucas 0:ce44e2a8e87a 47 else
rubenlucas 0:ce44e2a8e87a 48 {
rubenlucas 0:ce44e2a8e87a 49 VelocityRef = -1*ValuePot*MaxVelocity; //CCW
rubenlucas 0:ce44e2a8e87a 50 }
rubenlucas 0:ce44e2a8e87a 51
rubenlucas 0:ce44e2a8e87a 52 PositionRef = PositionRef + VelocityRef*Ts;
rubenlucas 0:ce44e2a8e87a 53
rubenlucas 0:ce44e2a8e87a 54 return PositionRef; //rad
rubenlucas 0:ce44e2a8e87a 55 }
rubenlucas 0:ce44e2a8e87a 56
rubenlucas 0:ce44e2a8e87a 57 // actual position of the motor
rubenlucas 0:ce44e2a8e87a 58 double GetActualPosition()
rubenlucas 0:ce44e2a8e87a 59 {
rubenlucas 0:ce44e2a8e87a 60 //This function determines the actual position of the motor
rubenlucas 0:ce44e2a8e87a 61 //The count:radians relation is 8400:2pi
rubenlucas 0:ce44e2a8e87a 62 double EncoderCounts = Encoder.getPulses(); //number of counts
rubenlucas 0:ce44e2a8e87a 63 double PositionMotor = EncoderCounts/8400*(6.283); // in rad (6.283 = pi)
rubenlucas 0:ce44e2a8e87a 64
rubenlucas 0:ce44e2a8e87a 65 return PositionMotor;
rubenlucas 0:ce44e2a8e87a 66 }
rubenlucas 0:ce44e2a8e87a 67
rubenlucas 0:ce44e2a8e87a 68
rubenlucas 0:ce44e2a8e87a 69
rubenlucas 0:ce44e2a8e87a 70 ///The controller
rubenlucas 0:ce44e2a8e87a 71 //double P_Controller(double error)
rubenlucas 0:ce44e2a8e87a 72 //{
rubenlucas 0:ce44e2a8e87a 73
rubenlucas 0:ce44e2a8e87a 74 //}
rubenlucas 0:ce44e2a8e87a 75
rubenlucas 0:ce44e2a8e87a 76 // ----------------------------------------------------------------------------
rubenlucas 0:ce44e2a8e87a 77 //Ticker functions
rubenlucas 0:ce44e2a8e87a 78
rubenlucas 0:ce44e2a8e87a 79 //Ticker function Data
rubenlucas 0:ce44e2a8e87a 80 void MeasureAndControl(void)
rubenlucas 0:ce44e2a8e87a 81 {
rubenlucas 0:ce44e2a8e87a 82 double PositionRef = GetReferencePosition();
rubenlucas 0:ce44e2a8e87a 83 double PositionMotor = GetActualPosition();
rubenlucas 0:ce44e2a8e87a 84
rubenlucas 0:ce44e2a8e87a 85 PosRefPrint = PositionRef;
rubenlucas 0:ce44e2a8e87a 86 PosMotorPrint = PositionMotor;
rubenlucas 0:ce44e2a8e87a 87 }
rubenlucas 0:ce44e2a8e87a 88
rubenlucas 0:ce44e2a8e87a 89 //Ticker function set motorvalues
rubenlucas 0:ce44e2a8e87a 90 void SetMotor()
rubenlucas 0:ce44e2a8e87a 91 {
rubenlucas 0:ce44e2a8e87a 92
rubenlucas 0:ce44e2a8e87a 93 }
rubenlucas 0:ce44e2a8e87a 94
rubenlucas 0:ce44e2a8e87a 95 void PrintToScreen()
rubenlucas 0:ce44e2a8e87a 96 {
rubenlucas 0:ce44e2a8e87a 97 PrintFlag = true;
rubenlucas 0:ce44e2a8e87a 98 }
rubenlucas 0:ce44e2a8e87a 99
rubenlucas 0:ce44e2a8e87a 100
rubenlucas 0:ce44e2a8e87a 101 //-----------------------------------------------------------------------------
rubenlucas 0:ce44e2a8e87a 102 int main()
rubenlucas 0:ce44e2a8e87a 103 {
rubenlucas 0:ce44e2a8e87a 104 pc.baud(115200);
rubenlucas 0:ce44e2a8e87a 105 pc.printf("Hello World\n\r");
rubenlucas 0:ce44e2a8e87a 106 TickerMeasureAndControl.attach(&MeasureAndControl,0.01); //100 Hz
rubenlucas 0:ce44e2a8e87a 107 TickerSetMotor.attach(&SetMotor,0.0025); //Set value for motor with 400 Hz
rubenlucas 0:ce44e2a8e87a 108 TickerPrintToScreen.attach(&PrintToScreen,0.5); //Every second twice the values on screen
rubenlucas 0:ce44e2a8e87a 109
rubenlucas 0:ce44e2a8e87a 110 while (true)
rubenlucas 0:ce44e2a8e87a 111 {
rubenlucas 0:ce44e2a8e87a 112 if(PrintFlag)
rubenlucas 0:ce44e2a8e87a 113 {
rubenlucas 0:ce44e2a8e87a 114 pc.printf("Pos ref = %f and Pos motor = %f\r",PosRefPrint,PosMotorPrint);
rubenlucas 0:ce44e2a8e87a 115 PrintFlag = false;
rubenlucas 0:ce44e2a8e87a 116 }
rubenlucas 0:ce44e2a8e87a 117 }
rubenlucas 0:ce44e2a8e87a 118 }