Thijs de Kleijn
/
Motorserialcontrol
Biorobotics, basic motor control with button and potmeter
main.cpp@0:762bb19ac7d2, 2017-10-06 (annotated)
- Committer:
- Frimzenner
- Date:
- Fri Oct 06 10:19:50 2017 +0000
- Revision:
- 0:762bb19ac7d2
Motor control with potmeter and button;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Frimzenner | 0:762bb19ac7d2 | 1 | #include "mbed.h" |
Frimzenner | 0:762bb19ac7d2 | 2 | #include "math.h" |
Frimzenner | 0:762bb19ac7d2 | 3 | |
Frimzenner | 0:762bb19ac7d2 | 4 | DigitalIn button1(D8); |
Frimzenner | 0:762bb19ac7d2 | 5 | AnalogIn potMeterIn(A0); |
Frimzenner | 0:762bb19ac7d2 | 6 | DigitalOut direction(D7); |
Frimzenner | 0:762bb19ac7d2 | 7 | PwmOut mmp(D6); |
Frimzenner | 0:762bb19ac7d2 | 8 | |
Frimzenner | 0:762bb19ac7d2 | 9 | float valpow; |
Frimzenner | 0:762bb19ac7d2 | 10 | int toggle = 0; |
Frimzenner | 0:762bb19ac7d2 | 11 | |
Frimzenner | 0:762bb19ac7d2 | 12 | |
Frimzenner | 0:762bb19ac7d2 | 13 | int main() |
Frimzenner | 0:762bb19ac7d2 | 14 | { |
Frimzenner | 0:762bb19ac7d2 | 15 | while(true) { |
Frimzenner | 0:762bb19ac7d2 | 16 | |
Frimzenner | 0:762bb19ac7d2 | 17 | if (button1 == false) { |
Frimzenner | 0:762bb19ac7d2 | 18 | toggle = !toggle; |
Frimzenner | 0:762bb19ac7d2 | 19 | wait(0.3f); |
Frimzenner | 0:762bb19ac7d2 | 20 | } |
Frimzenner | 0:762bb19ac7d2 | 21 | if (toggle == 0) { |
Frimzenner | 0:762bb19ac7d2 | 22 | direction = 0; |
Frimzenner | 0:762bb19ac7d2 | 23 | } else if (toggle == 1) { |
Frimzenner | 0:762bb19ac7d2 | 24 | direction = 1; |
Frimzenner | 0:762bb19ac7d2 | 25 | } |
Frimzenner | 0:762bb19ac7d2 | 26 | valpow = potMeterIn.read()/1023; |
Frimzenner | 0:762bb19ac7d2 | 27 | mmp.write(valpow); |
Frimzenner | 0:762bb19ac7d2 | 28 | } |
Frimzenner | 0:762bb19ac7d2 | 29 | //return; |
Frimzenner | 0:762bb19ac7d2 | 30 | } |
Frimzenner | 0:762bb19ac7d2 | 31 | |
Frimzenner | 0:762bb19ac7d2 | 32 | /* |
Frimzenner | 0:762bb19ac7d2 | 33 | float GetReferenceVelocity() |
Frimzenner | 0:762bb19ac7d2 | 34 | { |
Frimzenner | 0:762bb19ac7d2 | 35 | // Returns reference velocity in rad/s. |
Frimzenner | 0:762bb19ac7d2 | 36 | // Positive value means clockwise rotation. |
Frimzenner | 0:762bb19ac7d2 | 37 | const float maxVelocity=8.4; // in rad/s of course! |
Frimzenner | 0:762bb19ac7d2 | 38 | float referenceVelocity; // in rad/s |
Frimzenner | 0:762bb19ac7d2 | 39 | if (button1) { |
Frimzenner | 0:762bb19ac7d2 | 40 | // Clockwise rotation |
Frimzenner | 0:762bb19ac7d2 | 41 | referenceVelocity = potMeterIn * maxVelocity; |
Frimzenner | 0:762bb19ac7d2 | 42 | } else { |
Frimzenner | 0:762bb19ac7d2 | 43 | // Counterclockwise rotation |
Frimzenner | 0:762bb19ac7d2 | 44 | referenceVelocity = -1*potMeterIn * maxVelocity; |
Frimzenner | 0:762bb19ac7d2 | 45 | } |
Frimzenner | 0:762bb19ac7d2 | 46 | return referenceVelocity; |
Frimzenner | 0:762bb19ac7d2 | 47 | } |
Frimzenner | 0:762bb19ac7d2 | 48 | |
Frimzenner | 0:762bb19ac7d2 | 49 | void SetMotor1(float motorValue) |
Frimzenner | 0:762bb19ac7d2 | 50 | { |
Frimzenner | 0:762bb19ac7d2 | 51 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Frimzenner | 0:762bb19ac7d2 | 52 | // bits for motor 1. Positive value makes motor rotating |
Frimzenner | 0:762bb19ac7d2 | 53 | // clockwise. motorValues outside range are truncated to |
Frimzenner | 0:762bb19ac7d2 | 54 | // within range |
Frimzenner | 0:762bb19ac7d2 | 55 | if (motorValue >=0) { |
Frimzenner | 0:762bb19ac7d2 | 56 | motor1DirectionPin = 1; |
Frimzenner | 0:762bb19ac7d2 | 57 | } else { |
Frimzenner | 0:762bb19ac7d2 | 58 | motor1DirectionPin = 0; |
Frimzenner | 0:762bb19ac7d2 | 59 | } |
Frimzenner | 0:762bb19ac7d2 | 60 | if (fabs(motorValue)>1) { |
Frimzenner | 0:762bb19ac7d2 | 61 | motor1MagnitudePin = 1; |
Frimzenner | 0:762bb19ac7d2 | 62 | } else { |
Frimzenner | 0:762bb19ac7d2 | 63 | motor1MagnitudePin = fabs(motorValue); |
Frimzenner | 0:762bb19ac7d2 | 64 | } |
Frimzenner | 0:762bb19ac7d2 | 65 | } |
Frimzenner | 0:762bb19ac7d2 | 66 | |
Frimzenner | 0:762bb19ac7d2 | 67 | float FeedForwardControl(float referenceVelocity) |
Frimzenner | 0:762bb19ac7d2 | 68 | { |
Frimzenner | 0:762bb19ac7d2 | 69 | // very simple linear feed-forward control |
Frimzenner | 0:762bb19ac7d2 | 70 | const float MotorGain=8.4; // unit: (rad/s) / PWM |
Frimzenner | 0:762bb19ac7d2 | 71 | float motorValue = referenceVelocity / MotorGain; |
Frimzenner | 0:762bb19ac7d2 | 72 | return motorValue; |
Frimzenner | 0:762bb19ac7d2 | 73 | } |
Frimzenner | 0:762bb19ac7d2 | 74 | |
Frimzenner | 0:762bb19ac7d2 | 75 | void MeasureAndControl(void) |
Frimzenner | 0:762bb19ac7d2 | 76 | { |
Frimzenner | 0:762bb19ac7d2 | 77 | // This function measures the potmeter position, extracts a |
Frimzenner | 0:762bb19ac7d2 | 78 | // reference velocity from it, and controls the motor with |
Frimzenner | 0:762bb19ac7d2 | 79 | // a simple FeedForward controller. Call this from a Ticker. |
Frimzenner | 0:762bb19ac7d2 | 80 | float referenceVelocity = GetReferenceVelocity(); |
Frimzenner | 0:762bb19ac7d2 | 81 | float motorValue = FeedForwardControl(referenceVelocity); |
Frimzenner | 0:762bb19ac7d2 | 82 | SetMotor1(motorValue); |
Frimzenner | 0:762bb19ac7d2 | 83 | } |
Frimzenner | 0:762bb19ac7d2 | 84 | */ |