Feedforward controller
Dependencies: MODSERIAL QEI mbed
Fork of Tut5_motor_controller by
Diff: main.cpp
- Revision:
- 4:983b50758735
- Parent:
- 3:cc3766838777
- Child:
- 5:6c16e9335262
diff -r cc3766838777 -r 983b50758735 main.cpp --- a/main.cpp Thu Oct 05 13:10:39 2017 +0000 +++ b/main.cpp Fri Oct 06 09:05:35 2017 +0000 @@ -15,11 +15,12 @@ PwmOut motor2PWM(D5); AnalogIn potMeterIn1(A0); -AnalogIn PotMeterIn2(A1); +AnalogIn potMeterIn2(A1); DigitalIn button1(D11); MODSERIAL pc(USBTX,USBRX); +QEI Encoder(D12,D13,NC,32); Ticker controller; @@ -56,6 +57,15 @@ 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 @@ -71,8 +81,23 @@ void MeasureAndControl(void) { float referenceVelocity = GetReferenceVelocity(); - float motorValue = FeedForwardControl(referenceVelocity); - SetMotor1(motorValue); + 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); + } + } @@ -93,8 +118,9 @@ int counts = Encoder.getPulses(); float rV = GetReferenceVelocity(); float mV = FeedForwardControl(rV); + int rP = GetReferencePosition(); - pc.printf("\r reference velocity: %f. Motor Value is: %f number of counts: %i\n",mV,rV,counts); + pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",mV,rP,rV,counts); } }