Ruben Lucas / Mbed 2 deprecated Controller

control of polulu motor

Dependencies:   MODSERIAL QEI mbed biquadFilter

main.cpp@2:0a61483f4515, 2018-10-15 (annotated)

Committer:
rubenlucas
Date:
Mon Oct 15 15:08:54 2018 +0000
Revision:
2:0a61483f4515
Parent:
1:76e57b695115 
position reference not based anymore on velocity. script works as wanted

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