Controller for 1 motor with button control
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of Controller by
Diff: Controller.cpp
- Revision:
- 10:fb52590deb29
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.cpp Wed Oct 31 12:09:46 2018 +0000 @@ -0,0 +1,193 @@ +#include "mbed.h" +#include "math.h" +#include "MODSERIAL.h" +#include "QEI.h" +#include "BiQuad.h" + +//Tickers +Ticker TickerMeasureAndControl; +Ticker TickerPrintToScreen; + +//Communication +MODSERIAL pc(USBTX,USBRX); +QEI Encoder(D10,D11,NC,32); + +//Global pin variables +PwmOut PwmPin(D5); +DigitalOut DirectionPin(D4); +AnalogIn Potmeter1(A1); +AnalogIn Potmeter2(A0); +DigitalIn BUT1(D8); +DigitalIn BUT2(D9); +DigitalIn button(SW2); +//Global variables +volatile bool PrintFlag = false; + +//Global variables for printing on screen +volatile float PosRefPrint; // for printing value on screen +volatile float PosMotorPrint; // for printing value on screen +volatile float ErrorPrint; + +//----------------------------------------------------------------------------- +//The double-functions + +//Get reference position +double GetReferencePosition() +{ +// This function set the reference position to determine the position of the signal. +// a positive position is defined as a counterclockwise (CCW) rotation + static double PositionRef = 0; + if (button) + { + if (BUT2 == false) + { + if (PositionRef <= 0.5*(6.2830)) + { + PositionRef += 0.001*(6.2830); + } + } + + if (BUT1 == false) + { + if (PositionRef >= -0.5*(6.2830)) + { + PositionRef -= 0.001*(6.2830); + } + } + } + else + { + if (PositionRef <= 0) + { + PositionRef += 0.001*(6.2830); + } + + if (PositionRef >= 0) + { + PositionRef -= 0.001*(6.2830); + } + } + // double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1) + + + //double PositionRef = 0.8*6.2830*ValuePot - 0.8*3.1415; // position reference ranging from -0.4 rotations till 0.4 rotations + + return PositionRef; //rad +} + +// actual position of the motor +double GetActualPosition() +{ + //This function determines the actual position of the motor + //The count:radians relation is 8400:2pi + double EncoderCounts = Encoder.getPulses(); //number of counts + double PositionMotor = EncoderCounts/8400*(2*6.283); // in rad (6.283 = 2pi), 2* is for compensating X4 --> X2 encoding + + return PositionMotor; +} + + + +///The controller +double PID_Controller(double Error) +{ + //Belt drive parameters + double Ts = 0.002; //Sampling time 100 Hz + double Kp = 11.1; // proportional gain. Model value: 11.1 + double Ki = 22.81; //Integral gain. Model value: 11.2 + double Kd = 1.7; //Differential gain. Model value: 5.5 + + //Arm drive parameters + //double Ts = 0.002; //Sampling time 100 Hz + //double Kp = 19.8; // proportional gain + //double Ki = 40.9; //Intergral gain. Model value: 19.9 + //double Kd = 3; //Differential gain. Model value: 9.8 + + static double ErrorIntegral = 0; + static double ErrorPrevious = Error; + static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); + + //Proportional part: + double u_k = Kp * Error; + + //Integral part: + ErrorIntegral = ErrorIntegral + Error*Ts; + double u_i = Ki * ErrorIntegral; // + + //Derivative part: + double ErrorDerivative = (Error - ErrorPrevious)/Ts;//Kd must be made smaller to compensate for the higher Ts + double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative); + double u_d = Kd * FilteredErrorDerivative; + ErrorPrevious = Error; + + // sum of parts and return it + return u_k + u_i + u_d; //This will become the MotorValue +} + +//Ticker function set motorvalues +void SetMotor(double MotorValue) +{ + if (MotorValue >=0) + { + DirectionPin = 1; + } + else + { + DirectionPin = 0; + } + + if (fabs(MotorValue)>1) // if error more than 1 radian, full duty cycle + { + PwmPin = 1; + } + else + { + PwmPin = fabs(MotorValue); + } +} + +// ---------------------------------------------------------------------------- +//Ticker function +void MeasureAndControl(void) +{ + double PositionRef = GetReferencePosition(); + double PositionMotor = GetActualPosition(); + double MotorValue = PID_Controller(PositionRef - PositionMotor); // input is error + SetMotor(MotorValue); + + //for printing on screen + PosRefPrint = PositionRef; + PosMotorPrint = PositionMotor; + ErrorPrint = PositionRef - PositionMotor; + +} + + + +void PrintToScreen() +{ + PrintFlag = true; +} + + +//----------------------------------------------------------------------------- +int main() +{ + pc.baud(115200); + pc.printf("Hello World\n\r"); + PwmPin.period_us(60); // 16.66667 kHz (default period is too slow!) + TickerMeasureAndControl.attach(&MeasureAndControl,0.002); //500 Hz + TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen + + while (true) + { + if(PrintFlag) // if-statement for printing every second four times to screen + { + double KpPrint = 3.52; + double KiPrint = 0.88; + double KdPrint = 6.9; + pc.printf("Pos ref = %f, Pos motor = %f, Error = %f, Kp = %f, Ki = %f and Kd = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint,KiPrint,KdPrint); + PrintFlag = false; + } + } +} \ No newline at end of file