Controller for 1 motor with button control
Dependencies: MODSERIAL QEI biquadFilter mbed
Fork of Controller by
Diff: main.cpp
- Revision:
- 0:ce44e2a8e87a
- Child:
- 1:76e57b695115
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 15 13:48:56 2018 +0000 @@ -0,0 +1,118 @@ +#include "mbed.h" +#include "math.h" +#include "MODSERIAL.h" +#include "QEI.h" + +//Tickers +Ticker TickerMeasureAndControl; +Ticker TickerSetMotor; +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 button1(D8); + +//Global variables +const double Ts = 0.01; //Sample time of Ticker measure and control (100 Hz) +volatile bool PrintFlag = false; + +volatile float PosRefPrint; // for printing value on screen +volatile float PosMotorPrint; // for printing value on screen +//----------------------------------------------------------------------------- +//The double-functions + +//Get reference position +double GetReferencePosition() +{ +// This function set the reference position to determine the position of the signal. +// Reference velocity is set as proportional to duty cycle. +// Positive velocity (if button is pressed) means clockwise(CW) rotation. + + + const double MaxVelocity = 6.28; //60 RPM max velocity in rad/s + double VelocityRef; // Reference Velocity + double ValuePot = Potmeter1.read(); // Read value from potmeter (range from 0-1) + static double PositionRef = 0; // Initial position value in rad + + if (button1) + { + VelocityRef = ValuePot*MaxVelocity; //CW + } + else + { + VelocityRef = -1*ValuePot*MaxVelocity; //CCW + } + + PositionRef = PositionRef + VelocityRef*Ts; + + 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*(6.283); // in rad (6.283 = pi) + + return PositionMotor; +} + + + +///The controller +//double P_Controller(double error) +//{ + +//} + +// ---------------------------------------------------------------------------- +//Ticker functions + +//Ticker function Data +void MeasureAndControl(void) +{ + double PositionRef = GetReferencePosition(); + double PositionMotor = GetActualPosition(); + + PosRefPrint = PositionRef; + PosMotorPrint = PositionMotor; +} + +//Ticker function set motorvalues +void SetMotor() +{ + +} + +void PrintToScreen() +{ + PrintFlag = true; +} + + +//----------------------------------------------------------------------------- +int main() +{ + pc.baud(115200); + pc.printf("Hello World\n\r"); + TickerMeasureAndControl.attach(&MeasureAndControl,0.01); //100 Hz + TickerSetMotor.attach(&SetMotor,0.0025); //Set value for motor with 400 Hz + TickerPrintToScreen.attach(&PrintToScreen,0.5); //Every second twice the values on screen + + while (true) + { + if(PrintFlag) + { + pc.printf("Pos ref = %f and Pos motor = %f\r",PosRefPrint,PosMotorPrint); + PrintFlag = false; + } + } +} \ No newline at end of file