![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Feedforward controller
Dependencies: MODSERIAL QEI mbed
Fork of Tut5_motor_controller by
Diff: main.cpp
- Revision:
- 0:1816743ba8ba
- Child:
- 1:659489c32e75
diff -r 000000000000 -r 1816743ba8ba main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 04 12:46:37 2017 +0000 @@ -0,0 +1,81 @@ +#include "mbed.h" +#include "QEI.h" +#include "MODSERIAL.h" +#include "math.h" + + + +DigitalOut gpo(D0); +DigitalOut led(LED_BLUE); +DigitalOut motor1DC(D6); +DigitalOut motor1PWM(D7); +DigitalOut motor2DC(D4); +DigitalOut motor2PWM(D5); + +AnalogIn potMeterIn(A0); +DigitalIn button1(D11); + +MODSERIAL pc(USBTX,USBRX); + +float GetReferenceVelocity() +{ + // Returns reference velocity in rad/s. + // Positive value means clockwise rotation. + + const float maxVelocity=8.4; // in rad/s of course! + + float referenceVelocity; // in rad/s + + if (button1) { + // Clockwise rotation + referenceVelocity = potMeterIn * maxVelocity; + } + else { + // Counterclockwise rotation + referenceVelocity = -1*potMeterIn * maxVelocity; + } + return referenceVelocity; +} + + +float FeedForwardControl(float &referenceVelocity) +{ + // very simple linear feed-forward control + const float MotorGain=8.4; // unit: (rad/s) / PWM + float motorValue = referenceVelocity / MotorGain; + return motorValue; +} + +void SetMotor1(float motorValue) +{ + // Given -1<=motorValue<=1, this sets the PWM and direction + // bits for motor 1. Positive value makes motor rotating + // clockwise. motorValues outside range are truncated to + // within range + if (motorValue >=0) motor1DC=1; + else motor1DC=0; + if (fabs(motorValue)>1) motor1PWM = 1; + else motor1PWM = fabs(motorValue); +} + +int main() +{ + pc.baud(115200); + + QEI Encoder(D12,D13,NC,32); // Input for the Encoder + + while (true) { + + float v = GetReferenceVelocity(); + float controlValue = FeedForwardControl(v); + SetMotor1(controlValue); + + int counts; + counts = Encoder.getPulses(); + pc.printf("\r number of counts: %i\n",counts); + + pc.printf("\t\r reference velocity is: %f. Motor Value is: %f \t\r number of counts: %i\n",v,controlValue,counts); + } +} + +