![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Feedforward controller
Dependencies: MODSERIAL QEI mbed
Fork of Tut5_motor_controller by
main.cpp@0:1816743ba8ba, 2017-10-04 (annotated)
- Committer:
- Arnoud113
- Date:
- Wed Oct 04 12:46:37 2017 +0000
- Revision:
- 0:1816743ba8ba
- Child:
- 1:659489c32e75
Original versioin which runs the engine to one side and stops the engine when but 1 in pressed. However, the engine keeps on running with the button pressed when the controlling pot meter is turned to zero value.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Arnoud113 | 0:1816743ba8ba | 1 | #include "mbed.h" |
Arnoud113 | 0:1816743ba8ba | 2 | #include "QEI.h" |
Arnoud113 | 0:1816743ba8ba | 3 | #include "MODSERIAL.h" |
Arnoud113 | 0:1816743ba8ba | 4 | #include "math.h" |
Arnoud113 | 0:1816743ba8ba | 5 | |
Arnoud113 | 0:1816743ba8ba | 6 | |
Arnoud113 | 0:1816743ba8ba | 7 | |
Arnoud113 | 0:1816743ba8ba | 8 | DigitalOut gpo(D0); |
Arnoud113 | 0:1816743ba8ba | 9 | DigitalOut led(LED_BLUE); |
Arnoud113 | 0:1816743ba8ba | 10 | DigitalOut motor1DC(D6); |
Arnoud113 | 0:1816743ba8ba | 11 | DigitalOut motor1PWM(D7); |
Arnoud113 | 0:1816743ba8ba | 12 | DigitalOut motor2DC(D4); |
Arnoud113 | 0:1816743ba8ba | 13 | DigitalOut motor2PWM(D5); |
Arnoud113 | 0:1816743ba8ba | 14 | |
Arnoud113 | 0:1816743ba8ba | 15 | AnalogIn potMeterIn(A0); |
Arnoud113 | 0:1816743ba8ba | 16 | DigitalIn button1(D11); |
Arnoud113 | 0:1816743ba8ba | 17 | |
Arnoud113 | 0:1816743ba8ba | 18 | MODSERIAL pc(USBTX,USBRX); |
Arnoud113 | 0:1816743ba8ba | 19 | |
Arnoud113 | 0:1816743ba8ba | 20 | float GetReferenceVelocity() |
Arnoud113 | 0:1816743ba8ba | 21 | { |
Arnoud113 | 0:1816743ba8ba | 22 | // Returns reference velocity in rad/s. |
Arnoud113 | 0:1816743ba8ba | 23 | // Positive value means clockwise rotation. |
Arnoud113 | 0:1816743ba8ba | 24 | |
Arnoud113 | 0:1816743ba8ba | 25 | const float maxVelocity=8.4; // in rad/s of course! |
Arnoud113 | 0:1816743ba8ba | 26 | |
Arnoud113 | 0:1816743ba8ba | 27 | float referenceVelocity; // in rad/s |
Arnoud113 | 0:1816743ba8ba | 28 | |
Arnoud113 | 0:1816743ba8ba | 29 | if (button1) { |
Arnoud113 | 0:1816743ba8ba | 30 | // Clockwise rotation |
Arnoud113 | 0:1816743ba8ba | 31 | referenceVelocity = potMeterIn * maxVelocity; |
Arnoud113 | 0:1816743ba8ba | 32 | } |
Arnoud113 | 0:1816743ba8ba | 33 | else { |
Arnoud113 | 0:1816743ba8ba | 34 | // Counterclockwise rotation |
Arnoud113 | 0:1816743ba8ba | 35 | referenceVelocity = -1*potMeterIn * maxVelocity; |
Arnoud113 | 0:1816743ba8ba | 36 | } |
Arnoud113 | 0:1816743ba8ba | 37 | return referenceVelocity; |
Arnoud113 | 0:1816743ba8ba | 38 | } |
Arnoud113 | 0:1816743ba8ba | 39 | |
Arnoud113 | 0:1816743ba8ba | 40 | |
Arnoud113 | 0:1816743ba8ba | 41 | float FeedForwardControl(float &referenceVelocity) |
Arnoud113 | 0:1816743ba8ba | 42 | { |
Arnoud113 | 0:1816743ba8ba | 43 | // very simple linear feed-forward control |
Arnoud113 | 0:1816743ba8ba | 44 | const float MotorGain=8.4; // unit: (rad/s) / PWM |
Arnoud113 | 0:1816743ba8ba | 45 | float motorValue = referenceVelocity / MotorGain; |
Arnoud113 | 0:1816743ba8ba | 46 | return motorValue; |
Arnoud113 | 0:1816743ba8ba | 47 | } |
Arnoud113 | 0:1816743ba8ba | 48 | |
Arnoud113 | 0:1816743ba8ba | 49 | void SetMotor1(float motorValue) |
Arnoud113 | 0:1816743ba8ba | 50 | { |
Arnoud113 | 0:1816743ba8ba | 51 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Arnoud113 | 0:1816743ba8ba | 52 | // bits for motor 1. Positive value makes motor rotating |
Arnoud113 | 0:1816743ba8ba | 53 | // clockwise. motorValues outside range are truncated to |
Arnoud113 | 0:1816743ba8ba | 54 | // within range |
Arnoud113 | 0:1816743ba8ba | 55 | if (motorValue >=0) motor1DC=1; |
Arnoud113 | 0:1816743ba8ba | 56 | else motor1DC=0; |
Arnoud113 | 0:1816743ba8ba | 57 | if (fabs(motorValue)>1) motor1PWM = 1; |
Arnoud113 | 0:1816743ba8ba | 58 | else motor1PWM = fabs(motorValue); |
Arnoud113 | 0:1816743ba8ba | 59 | } |
Arnoud113 | 0:1816743ba8ba | 60 | |
Arnoud113 | 0:1816743ba8ba | 61 | int main() |
Arnoud113 | 0:1816743ba8ba | 62 | { |
Arnoud113 | 0:1816743ba8ba | 63 | pc.baud(115200); |
Arnoud113 | 0:1816743ba8ba | 64 | |
Arnoud113 | 0:1816743ba8ba | 65 | QEI Encoder(D12,D13,NC,32); // Input for the Encoder |
Arnoud113 | 0:1816743ba8ba | 66 | |
Arnoud113 | 0:1816743ba8ba | 67 | while (true) { |
Arnoud113 | 0:1816743ba8ba | 68 | |
Arnoud113 | 0:1816743ba8ba | 69 | float v = GetReferenceVelocity(); |
Arnoud113 | 0:1816743ba8ba | 70 | float controlValue = FeedForwardControl(v); |
Arnoud113 | 0:1816743ba8ba | 71 | SetMotor1(controlValue); |
Arnoud113 | 0:1816743ba8ba | 72 | |
Arnoud113 | 0:1816743ba8ba | 73 | int counts; |
Arnoud113 | 0:1816743ba8ba | 74 | counts = Encoder.getPulses(); |
Arnoud113 | 0:1816743ba8ba | 75 | pc.printf("\r number of counts: %i\n",counts); |
Arnoud113 | 0:1816743ba8ba | 76 | |
Arnoud113 | 0:1816743ba8ba | 77 | pc.printf("\t\r reference velocity is: %f. Motor Value is: %f \t\r number of counts: %i\n",v,controlValue,counts); |
Arnoud113 | 0:1816743ba8ba | 78 | } |
Arnoud113 | 0:1816743ba8ba | 79 | } |
Arnoud113 | 0:1816743ba8ba | 80 | |
Arnoud113 | 0:1816743ba8ba | 81 |