control of polulu motor
Dependencies: MODSERIAL QEI mbed biquadFilter
Diff: main.cpp
- Revision:
- 1:76e57b695115
- Parent:
- 0:ce44e2a8e87a
- Child:
- 2:0a61483f4515
--- a/main.cpp Mon Oct 15 13:48:56 2018 +0000 +++ b/main.cpp Mon Oct 15 14:57:07 2018 +0000 @@ -5,7 +5,6 @@ //Tickers Ticker TickerMeasureAndControl; -Ticker TickerSetMotor; Ticker TickerPrintToScreen; //Communication MODSERIAL pc(USBTX,USBRX); @@ -22,8 +21,10 @@ const double Ts = 0.01; //Sample time of Ticker measure and control (100 Hz) 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 @@ -68,29 +69,54 @@ ///The controller -//double P_Controller(double error) -//{ +double P_Controller(double Error) +{ + double Kp = 35*Potmeter2.read(); // 35 is just a try + + double u_k = Kp * Error; + + return u_k; //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) + { + PwmPin = 1; // if error more than 1 radian, full duty cycle + } + else + { + PwmPin = fabs(MotorValue); + } +} // ---------------------------------------------------------------------------- -//Ticker functions - -//Ticker function Data +//Ticker function void MeasureAndControl(void) { double PositionRef = GetReferencePosition(); double PositionMotor = GetActualPosition(); + double MotorValue = P_Controller(PositionRef - PositionMotor); // input is error + SetMotor(MotorValue); + //for printing on screen PosRefPrint = PositionRef; PosMotorPrint = PositionMotor; + ErrorPrint = MotorValue; + } -//Ticker function set motorvalues -void SetMotor() -{ -} void PrintToScreen() { @@ -103,15 +129,16 @@ { 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.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 + TickerPrintToScreen.attach(&PrintToScreen,0.25); //Every second four times the values on screen while (true) { - if(PrintFlag) + if(PrintFlag) // if-statement for printing every second four times to screen { - pc.printf("Pos ref = %f and Pos motor = %f\r",PosRefPrint,PosMotorPrint); + double KpPrint = 35*Potmeter2.read(); + pc.printf("Pos ref = %f, Pos motor = %f, Error = %f and Kp = %f\r",PosRefPrint,PosMotorPrint,ErrorPrint,KpPrint); PrintFlag = false; } }