![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Feedforward controller
Dependencies: MODSERIAL QEI mbed
Fork of Tut5_motor_controller by
main.cpp
- Committer:
- Arnoud113
- Date:
- 2017-10-04
- Revision:
- 0:1816743ba8ba
- Child:
- 1:659489c32e75
File content as of revision 0:1816743ba8ba:
#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); } }