Feedforward controller
Dependencies: MODSERIAL QEI mbed
Fork of Tut5_motor_controller by
main.cpp
- Committer:
- Arnoud113
- Date:
- 2017-10-06
- Revision:
- 4:983b50758735
- Parent:
- 3:cc3766838777
- Child:
- 5:6c16e9335262
File content as of revision 4:983b50758735:
#include "mbed.h" #include "QEI.h" #include "MODSERIAL.h" #include "math.h" DigitalOut gpo(D0); DigitalOut ledb(LED_BLUE); DigitalOut ledr(LED_RED); DigitalOut ledg(LED_GREEN); DigitalOut motor1DC(D7); PwmOut motor1PWM(D6); DigitalOut motor2DC(D4); PwmOut motor2PWM(D5); AnalogIn potMeterIn1(A0); AnalogIn potMeterIn2(A1); DigitalIn button1(D11); MODSERIAL pc(USBTX,USBRX); QEI Encoder(D12,D13,NC,32); Ticker controller; 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 = potMeterIn1 * maxVelocity; ledr = 1; ledb = 0; } else { // Counterclockwise rotation referenceVelocity = -1*potMeterIn1 * maxVelocity; ledb = 1; ledr = 0; } 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; } float GetReferencePosition() { int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position float referencePosition; referencePosition = potMeterIn2 * potmultiplier; return referencePosition; } 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); } void MeasureAndControl(void) { float referenceVelocity = GetReferenceVelocity(); int referencePosition = GetReferencePosition(); int counts = Encoder.getPulses(); if(counts < referencePosition) { float motorValue = FeedForwardControl(referenceVelocity); SetMotor1(motorValue); } else { int b = -1; float motorValue = b * FeedForwardControl(referenceVelocity); SetMotor1(motorValue); } } int main() { pc.baud(115200); QEI Encoder(D12,D13,NC,32); ledr = 1; ledb = 1; ledg = 1; controller.attach(&MeasureAndControl, 0.1); while(1) { int counts = Encoder.getPulses(); float rV = GetReferenceVelocity(); float mV = FeedForwardControl(rV); int rP = GetReferencePosition(); pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",mV,rP,rV,counts); } }