Jan Boerman
/
MotorPotButton
Exercise #1 Feed Forward Control
main.cpp@0:936183552c73, 2016-10-07 (annotated)
- Committer:
- Jankoekenpan
- Date:
- Fri Oct 07 14:09:15 2016 +0000
- Revision:
- 0:936183552c73
Motor assignment #1; ; set velocity of the motor using a potmeter.; switch direction of the motor using a button.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jankoekenpan | 0:936183552c73 | 1 | #include "mbed.h" |
Jankoekenpan | 0:936183552c73 | 2 | #include "math.h" |
Jankoekenpan | 0:936183552c73 | 3 | //#include "HIDScope.h" |
Jankoekenpan | 0:936183552c73 | 4 | |
Jankoekenpan | 0:936183552c73 | 5 | // ------ Hardware Interfaces ------- |
Jankoekenpan | 0:936183552c73 | 6 | |
Jankoekenpan | 0:936183552c73 | 7 | const PinName motor1dir = D7; |
Jankoekenpan | 0:936183552c73 | 8 | const PinName motor1PWM = D6; |
Jankoekenpan | 0:936183552c73 | 9 | //const PinName motor2PWM = D5; |
Jankoekenpan | 0:936183552c73 | 10 | //const PinName motor2dir = D4; |
Jankoekenpan | 0:936183552c73 | 11 | |
Jankoekenpan | 0:936183552c73 | 12 | DigitalOut motor1direction(motor1dir); |
Jankoekenpan | 0:936183552c73 | 13 | PwmOut motor1control(motor1PWM); |
Jankoekenpan | 0:936183552c73 | 14 | //PwmOut motor2control(motor2PWM); |
Jankoekenpan | 0:936183552c73 | 15 | //DigitalOut motor2direction(motor2dir); |
Jankoekenpan | 0:936183552c73 | 16 | |
Jankoekenpan | 0:936183552c73 | 17 | const PinName button1name = D10; //for some reason D8 does not seem to work. wtf! |
Jankoekenpan | 0:936183552c73 | 18 | const PinName pot1name = A0; |
Jankoekenpan | 0:936183552c73 | 19 | InterruptIn button1(button1name); |
Jankoekenpan | 0:936183552c73 | 20 | AnalogIn potMeterIn(pot1name); |
Jankoekenpan | 0:936183552c73 | 21 | |
Jankoekenpan | 0:936183552c73 | 22 | // ------- Objects used ------- |
Jankoekenpan | 0:936183552c73 | 23 | |
Jankoekenpan | 0:936183552c73 | 24 | // Ticker which controls the mother every 1/100 of a second. |
Jankoekenpan | 0:936183552c73 | 25 | Ticker controlTicker; |
Jankoekenpan | 0:936183552c73 | 26 | |
Jankoekenpan | 0:936183552c73 | 27 | Ticker debugTicker; |
Jankoekenpan | 0:936183552c73 | 28 | Serial pc(USBTX, USBRX); |
Jankoekenpan | 0:936183552c73 | 29 | |
Jankoekenpan | 0:936183552c73 | 30 | //HIDScope scope(2); //fuck you hidscope we're not using you! |
Jankoekenpan | 0:936183552c73 | 31 | |
Jankoekenpan | 0:936183552c73 | 32 | |
Jankoekenpan | 0:936183552c73 | 33 | // ------- Constants |
Jankoekenpan | 0:936183552c73 | 34 | |
Jankoekenpan | 0:936183552c73 | 35 | const float motorGain = 8.4f; |
Jankoekenpan | 0:936183552c73 | 36 | const float maxVelocity = 8.4f; //radians per second |
Jankoekenpan | 0:936183552c73 | 37 | const bool clockwise = true; |
Jankoekenpan | 0:936183552c73 | 38 | |
Jankoekenpan | 0:936183552c73 | 39 | |
Jankoekenpan | 0:936183552c73 | 40 | // ------ variables |
Jankoekenpan | 0:936183552c73 | 41 | |
Jankoekenpan | 0:936183552c73 | 42 | volatile bool direction = clockwise; |
Jankoekenpan | 0:936183552c73 | 43 | |
Jankoekenpan | 0:936183552c73 | 44 | // ------ functions |
Jankoekenpan | 0:936183552c73 | 45 | |
Jankoekenpan | 0:936183552c73 | 46 | float getReferenceVelocity() { |
Jankoekenpan | 0:936183552c73 | 47 | // Returns reference velocity in rad/s. |
Jankoekenpan | 0:936183552c73 | 48 | return maxVelocity * potMeterIn; |
Jankoekenpan | 0:936183552c73 | 49 | } |
Jankoekenpan | 0:936183552c73 | 50 | |
Jankoekenpan | 0:936183552c73 | 51 | void setMotor1(float motorValue) { |
Jankoekenpan | 0:936183552c73 | 52 | // Given motorValue<=1, writes the velocity to the pwm control. |
Jankoekenpan | 0:936183552c73 | 53 | // MotorValues outside range are truncated to within range. |
Jankoekenpan | 0:936183552c73 | 54 | motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue)); |
Jankoekenpan | 0:936183552c73 | 55 | } |
Jankoekenpan | 0:936183552c73 | 56 | |
Jankoekenpan | 0:936183552c73 | 57 | float feedForwardControl(float referenceVelocity) { |
Jankoekenpan | 0:936183552c73 | 58 | // very simple linear feed-forward control |
Jankoekenpan | 0:936183552c73 | 59 | // returns motorValue |
Jankoekenpan | 0:936183552c73 | 60 | return referenceVelocity / motorGain; |
Jankoekenpan | 0:936183552c73 | 61 | } |
Jankoekenpan | 0:936183552c73 | 62 | |
Jankoekenpan | 0:936183552c73 | 63 | void measureAndControl(void) { |
Jankoekenpan | 0:936183552c73 | 64 | // This function measures the potmeter position, extracts a |
Jankoekenpan | 0:936183552c73 | 65 | // reference velocity from it, and controls the motor with |
Jankoekenpan | 0:936183552c73 | 66 | // a simple FeedForward controller. Call this from a Ticker. |
Jankoekenpan | 0:936183552c73 | 67 | float referenceVelocity = getReferenceVelocity(); |
Jankoekenpan | 0:936183552c73 | 68 | float motorValue = feedForwardControl(referenceVelocity); |
Jankoekenpan | 0:936183552c73 | 69 | setMotor1(motorValue); |
Jankoekenpan | 0:936183552c73 | 70 | } |
Jankoekenpan | 0:936183552c73 | 71 | |
Jankoekenpan | 0:936183552c73 | 72 | void onButtonPress() { |
Jankoekenpan | 0:936183552c73 | 73 | // reverses the direction |
Jankoekenpan | 0:936183552c73 | 74 | motor1direction.write(direction = !direction); |
Jankoekenpan | 0:936183552c73 | 75 | pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise"); |
Jankoekenpan | 0:936183552c73 | 76 | } |
Jankoekenpan | 0:936183552c73 | 77 | |
Jankoekenpan | 0:936183552c73 | 78 | void onDebugTick() { |
Jankoekenpan | 0:936183552c73 | 79 | |
Jankoekenpan | 0:936183552c73 | 80 | pc.printf("pot input: %f\r\n", potMeterIn.read()); |
Jankoekenpan | 0:936183552c73 | 81 | pc.printf("motorValue: %f\r\n", feedForwardControl(getReferenceVelocity())); |
Jankoekenpan | 0:936183552c73 | 82 | pc.printf("\n\n\n"); |
Jankoekenpan | 0:936183552c73 | 83 | |
Jankoekenpan | 0:936183552c73 | 84 | /* |
Jankoekenpan | 0:936183552c73 | 85 | scope.set(0, potMeterIn.read()); |
Jankoekenpan | 0:936183552c73 | 86 | scope.set(1, feedForwardControl(getReferenceVelocity())); |
Jankoekenpan | 0:936183552c73 | 87 | scope.send(); |
Jankoekenpan | 0:936183552c73 | 88 | */ |
Jankoekenpan | 0:936183552c73 | 89 | } |
Jankoekenpan | 0:936183552c73 | 90 | |
Jankoekenpan | 0:936183552c73 | 91 | int main() |
Jankoekenpan | 0:936183552c73 | 92 | { |
Jankoekenpan | 0:936183552c73 | 93 | pc.baud(115200); |
Jankoekenpan | 0:936183552c73 | 94 | |
Jankoekenpan | 0:936183552c73 | 95 | button1.fall(&onButtonPress); |
Jankoekenpan | 0:936183552c73 | 96 | controlTicker.attach(&measureAndControl, 1.0f/100.0f); |
Jankoekenpan | 0:936183552c73 | 97 | debugTicker.attach(&onDebugTick, 1); |
Jankoekenpan | 0:936183552c73 | 98 | |
Jankoekenpan | 0:936183552c73 | 99 | while (true); |
Jankoekenpan | 0:936183552c73 | 100 | } |