Nu werkt het wel, opdracht 1 is af
Dependencies: MODSERIAL mbed QEI feed_forward
Fork of feed_forward by
main.cpp@1:92a60278860a, 2017-10-03 (annotated)
- Committer:
- DiondeGreef
- Date:
- Tue Oct 03 15:35:26 2017 +0000
- Revision:
- 1:92a60278860a
- Parent:
- 0:331597250051
- Child:
- 2:a64e3c37b571
Nu werkt het wel, kut Roy
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RoyvZ | 0:331597250051 | 1 | #include "mbed.h" |
RoyvZ | 0:331597250051 | 2 | #include "MODSERIAL.h" |
RoyvZ | 0:331597250051 | 3 | #include "math.h" |
RoyvZ | 0:331597250051 | 4 | |
RoyvZ | 0:331597250051 | 5 | DigitalOut gpo(D0); |
RoyvZ | 0:331597250051 | 6 | DigitalOut motorDirection(D4); |
DiondeGreef | 1:92a60278860a | 7 | PwmOut motorSpeed(D5); |
RoyvZ | 0:331597250051 | 8 | AnalogIn potMeterIn(A1); |
RoyvZ | 0:331597250051 | 9 | InterruptIn button1(D3); |
RoyvZ | 0:331597250051 | 10 | Ticker ticker; |
RoyvZ | 0:331597250051 | 11 | |
RoyvZ | 0:331597250051 | 12 | MODSERIAL pc(USBTX, USBRX); |
RoyvZ | 0:331597250051 | 13 | |
RoyvZ | 0:331597250051 | 14 | |
RoyvZ | 0:331597250051 | 15 | float GetReferenceVelocity() |
RoyvZ | 0:331597250051 | 16 | { |
RoyvZ | 0:331597250051 | 17 | // Returns reference velocity in rad/s. |
RoyvZ | 0:331597250051 | 18 | // Positive value means clockwise rotation. |
RoyvZ | 0:331597250051 | 19 | const float maxVelocity=8.4; // in rad/s of course! |
RoyvZ | 0:331597250051 | 20 | float referenceVelocity; // in rad/s |
RoyvZ | 0:331597250051 | 21 | if (button1) { |
RoyvZ | 0:331597250051 | 22 | // Clockwise rotation |
RoyvZ | 0:331597250051 | 23 | referenceVelocity = potMeterIn * maxVelocity; |
RoyvZ | 0:331597250051 | 24 | } |
RoyvZ | 0:331597250051 | 25 | else { |
RoyvZ | 0:331597250051 | 26 | // Counterclockwise rotation |
RoyvZ | 0:331597250051 | 27 | referenceVelocity = -1*potMeterIn * maxVelocity; |
RoyvZ | 0:331597250051 | 28 | } |
RoyvZ | 0:331597250051 | 29 | return referenceVelocity; |
RoyvZ | 0:331597250051 | 30 | } |
RoyvZ | 0:331597250051 | 31 | |
RoyvZ | 0:331597250051 | 32 | void setMotor(float motorValue) { |
RoyvZ | 0:331597250051 | 33 | if (motorValue >= 0) |
RoyvZ | 0:331597250051 | 34 | { |
RoyvZ | 0:331597250051 | 35 | //float motor1DirectionPin1 = 1; |
RoyvZ | 0:331597250051 | 36 | motorDirection=1; |
RoyvZ | 0:331597250051 | 37 | } |
RoyvZ | 0:331597250051 | 38 | else |
RoyvZ | 0:331597250051 | 39 | { |
RoyvZ | 0:331597250051 | 40 | //float motor1DirectionPin1 = 0; |
RoyvZ | 0:331597250051 | 41 | motorDirection=0; |
RoyvZ | 0:331597250051 | 42 | } |
RoyvZ | 0:331597250051 | 43 | |
RoyvZ | 0:331597250051 | 44 | if (fabs(motorValue)>1) |
RoyvZ | 0:331597250051 | 45 | { |
RoyvZ | 0:331597250051 | 46 | //float motor1MagnitudePin1 = 1; |
RoyvZ | 0:331597250051 | 47 | motorSpeed = 1; |
RoyvZ | 0:331597250051 | 48 | } |
RoyvZ | 0:331597250051 | 49 | else |
RoyvZ | 0:331597250051 | 50 | { |
RoyvZ | 0:331597250051 | 51 | //float motor1MagnitudePin1 = fabs(motorValue); |
RoyvZ | 0:331597250051 | 52 | motorSpeed = fabs(motorValue); |
RoyvZ | 0:331597250051 | 53 | } |
RoyvZ | 0:331597250051 | 54 | } |
RoyvZ | 0:331597250051 | 55 | |
RoyvZ | 0:331597250051 | 56 | float FeedForwardControl(float referenceVelocity) |
RoyvZ | 0:331597250051 | 57 | { |
RoyvZ | 0:331597250051 | 58 | // very simple linear feed-forward control |
RoyvZ | 0:331597250051 | 59 | const float MotorGain=8.4; // unit: (rad/s) / PWM |
RoyvZ | 0:331597250051 | 60 | float motorValue = referenceVelocity / MotorGain; |
RoyvZ | 0:331597250051 | 61 | return motorValue; |
RoyvZ | 0:331597250051 | 62 | } |
RoyvZ | 0:331597250051 | 63 | |
RoyvZ | 0:331597250051 | 64 | |
RoyvZ | 0:331597250051 | 65 | void MeasureAndControl(void) |
RoyvZ | 0:331597250051 | 66 | { |
RoyvZ | 0:331597250051 | 67 | // This function measures the potmeter position, extracts a |
RoyvZ | 0:331597250051 | 68 | // reference velocity from it, and controls the motor with |
RoyvZ | 0:331597250051 | 69 | // a simple FeedForward controller. Call this from a Ticker. |
RoyvZ | 0:331597250051 | 70 | float referenceVelocity = GetReferenceVelocity(); |
RoyvZ | 0:331597250051 | 71 | float motorValue = FeedForwardControl(referenceVelocity); |
RoyvZ | 0:331597250051 | 72 | setMotor(motorValue); |
RoyvZ | 0:331597250051 | 73 | } |
RoyvZ | 0:331597250051 | 74 | |
RoyvZ | 0:331597250051 | 75 | int main() { |
RoyvZ | 0:331597250051 | 76 | pc.baud(115200); |
RoyvZ | 0:331597250051 | 77 | //ticker.attach(MeasureAndControl, 0.01); |
RoyvZ | 0:331597250051 | 78 | while(true){ |
RoyvZ | 0:331597250051 | 79 | wait(0.1); |
RoyvZ | 0:331597250051 | 80 | |
RoyvZ | 0:331597250051 | 81 | //pc.printf("%f\r\n",GetReferenceVelocity()); |
RoyvZ | 0:331597250051 | 82 | float v_ref = GetReferenceVelocity(); |
RoyvZ | 0:331597250051 | 83 | setMotor(v_ref); |
RoyvZ | 0:331597250051 | 84 | pc.printf("%f \r\n", FeedForwardControl(v_ref)); |
RoyvZ | 0:331597250051 | 85 | motorDirection.write(motorDirection); |
RoyvZ | 0:331597250051 | 86 | motorSpeed.write(motorSpeed); //PWM Speed Control |
RoyvZ | 0:331597250051 | 87 | } |
RoyvZ | 0:331597250051 | 88 | } |
RoyvZ | 0:331597250051 | 89 |