Anne-Linde de Gooijer / Mbed 2 deprecated Motor

biorobotics 2 motoren werkend, button verandert de richting en potmeter verandert de snelheid #shoutout naar jankoekenpan

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM

main.cpp@7:be97dcdc1b8c, 2019-10-07 (annotated)

Committer:
S1933191
Date:
Mon Oct 07 12:54:29 2019 +0000
Revision:
7:be97dcdc1b8c
Parent:
6:fdf0974a33fc 
"schone versie", Motor1: button1 en potmeter1(direction snelheid), Motor2: button2 en potmeter2(direction snelheid)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
S1933191 4:37c22ab0f896 2 #include "math.h"
S1933191 4:37c22ab0f896 3 //#include "HIDScope.h"
S1933191 7:be97dcdc1b8c 4 // ---- Alles met een 1(of geen getal) gaat over motor 1----
S1933191 7:be97dcdc1b8c 5 // ---- Alles met een 2 gaat over motor 2----
S1933191 4:37c22ab0f896 6 // ------ Hardware Interfaces -------
S1933191 2:8a9df5b1b638 7
S1933191 7:be97dcdc1b8c 8 const PinName motor1dir = D7; //direction1 (cw,ccw)
S1933191 7:be97dcdc1b8c 9 const PinName motor1PWM = D6; //speed1
S1933191 7:be97dcdc1b8c 10 const PinName motor2PWM = D5; //speed2
S1933191 7:be97dcdc1b8c 11 const PinName motor2dir = D4; //direction2 (cw,ccw)
S1933191 2:8a9df5b1b638 12
S1933191 4:37c22ab0f896 13 DigitalOut motor1direction(motor1dir);
S1933191 4:37c22ab0f896 14 PwmOut motor1control(motor1PWM);
S1933191 5:2fb7355a44c0 15 PwmOut motor2control(motor2PWM);
S1933191 5:2fb7355a44c0 16 DigitalOut motor2direction(motor2dir);
S1933191 4:37c22ab0f896 17
S1933191 7:be97dcdc1b8c 18 const PinName button1name = D10;
S1933191 4:37c22ab0f896 19 const PinName pot1name = A0;
S1933191 5:2fb7355a44c0 20 const PinName button2name = D11;
S1933191 5:2fb7355a44c0 21 const PinName pot2name = A1;
S1933191 4:37c22ab0f896 22 InterruptIn button1(button1name);
S1933191 5:2fb7355a44c0 23 AnalogIn potMeterIn1(pot1name);
S1933191 5:2fb7355a44c0 24 InterruptIn button2(button2name);
S1933191 5:2fb7355a44c0 25 AnalogIn potMeterIn2(pot2name);
S1933191 2:8a9df5b1b638 26
S1933191 4:37c22ab0f896 27 // ------- Objects used -------
S1933191 4:37c22ab0f896 28
S1933191 4:37c22ab0f896 29 // Ticker which controls the mother every 1/100 of a second.
S1933191 4:37c22ab0f896 30 Ticker controlTicker;
S1933191 2:8a9df5b1b638 31
S1933191 4:37c22ab0f896 32 Ticker debugTicker;
S1933191 4:37c22ab0f896 33 Serial pc(USBTX, USBRX);
S1933191 4:37c22ab0f896 34
S1933191 4:37c22ab0f896 35 //HIDScope scope(2); //fuck you hidscope we're not using you!
S1933191 2:8a9df5b1b638 36
S1933191 2:8a9df5b1b638 37
S1933191 4:37c22ab0f896 38 // ------- Constants
S1933191 4:37c22ab0f896 39
S1933191 4:37c22ab0f896 40 const float motorGain = 8.4f;
S1933191 4:37c22ab0f896 41 const float maxVelocity = 8.4f; //radians per second
S1933191 4:37c22ab0f896 42 const bool clockwise = true;
S1933191 4:37c22ab0f896 43
S1933191 2:8a9df5b1b638 44
S1933191 4:37c22ab0f896 45 // ------ variables
S1933191 4:37c22ab0f896 46
S1933191 5:2fb7355a44c0 47 volatile bool direction1 = clockwise;
S1933191 5:2fb7355a44c0 48 volatile bool direction2 = clockwise;
S1933191 4:37c22ab0f896 49
S1933191 4:37c22ab0f896 50 // ------ functions
S1933191 4:37c22ab0f896 51
S1933191 4:37c22ab0f896 52 float getReferenceVelocity() {
S1933191 4:37c22ab0f896 53 // Returns reference velocity in rad/s.
S1933191 5:2fb7355a44c0 54 return maxVelocity * potMeterIn1 ;
S1933191 5:2fb7355a44c0 55 }
S1933191 5:2fb7355a44c0 56 float getReferenceVelocity2() {
S1933191 5:2fb7355a44c0 57 // Returns reference velocity in rad/s.
S1933191 5:2fb7355a44c0 58 return maxVelocity * potMeterIn2;
RobertoO 0:67c50348f842 59 }
S1933191 2:8a9df5b1b638 60
S1933191 5:2fb7355a44c0 61 void setMotor1(float motorValue1) {
S1933191 5:2fb7355a44c0 62 // Given motorValue1<=1, writes the velocity to the pwm control.
S1933191 4:37c22ab0f896 63 // MotorValues outside range are truncated to within range.
S1933191 5:2fb7355a44c0 64 motor1control.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
S1933191 5:2fb7355a44c0 65 }
S1933191 5:2fb7355a44c0 66
S1933191 5:2fb7355a44c0 67 void setMotor2(float motorValue2) {
S1933191 5:2fb7355a44c0 68 // Given motorValue2<=1, writes the velocity to the pwm control.
S1933191 5:2fb7355a44c0 69 // MotorValues outside range are truncated to within range.
S1933191 5:2fb7355a44c0 70 motor2control.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
S1933191 2:8a9df5b1b638 71 }
S1933191 4:37c22ab0f896 72
S1933191 4:37c22ab0f896 73 float feedForwardControl(float referenceVelocity) {
S1933191 4:37c22ab0f896 74 // very simple linear feed-forward control
S1933191 4:37c22ab0f896 75 // returns motorValue
S1933191 4:37c22ab0f896 76 return referenceVelocity / motorGain;
S1933191 4:37c22ab0f896 77 }
S1933191 5:2fb7355a44c0 78
S1933191 5:2fb7355a44c0 79 float feedForwardControl2(float referenceVelocity2) {
S1933191 5:2fb7355a44c0 80 // very simple linear feed-forward control
S1933191 5:2fb7355a44c0 81 // returns motorValue
S1933191 5:2fb7355a44c0 82 return referenceVelocity2 / motorGain;
S1933191 5:2fb7355a44c0 83 }
S1933191 4:37c22ab0f896 84
S1933191 4:37c22ab0f896 85 void measureAndControl(void) {
S1933191 4:37c22ab0f896 86 // This function measures the potmeter position, extracts a
S1933191 4:37c22ab0f896 87 // reference velocity from it, and controls the motor with
S1933191 4:37c22ab0f896 88 // a simple FeedForward controller. Call this from a Ticker.
S1933191 4:37c22ab0f896 89 float referenceVelocity = getReferenceVelocity();
S1933191 5:2fb7355a44c0 90 float motorValue1 = feedForwardControl(referenceVelocity);
S1933191 5:2fb7355a44c0 91 setMotor1(motorValue1);
S1933191 6:fdf0974a33fc 92
S1933191 5:2fb7355a44c0 93 float referenceVelocity2 = getReferenceVelocity2();
S1933191 5:2fb7355a44c0 94 float motorValue2 = feedForwardControl2(referenceVelocity2);
S1933191 5:2fb7355a44c0 95 setMotor2(motorValue2);
S1933191 5:2fb7355a44c0 96 }
S1933191 5:2fb7355a44c0 97
S1933191 4:37c22ab0f896 98 void onButtonPress() {
S1933191 4:37c22ab0f896 99 // reverses the direction
S1933191 5:2fb7355a44c0 100 motor1direction.write(direction1 = !direction1);
S1933191 5:2fb7355a44c0 101 pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
S1933191 2:8a9df5b1b638 102 }
S1933191 5:2fb7355a44c0 103
S1933191 5:2fb7355a44c0 104 void onButtonPress2() {
S1933191 5:2fb7355a44c0 105 // reverses the direction
S1933191 5:2fb7355a44c0 106 motor2direction.write(direction2 = !direction2);
S1933191 5:2fb7355a44c0 107 pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
S1933191 5:2fb7355a44c0 108 }
S1933191 4:37c22ab0f896 109 void onDebugTick() {
S1933191 4:37c22ab0f896 110
S1933191 5:2fb7355a44c0 111 pc.printf("pot input1: %f\r\n", potMeterIn1.read());
S1933191 5:2fb7355a44c0 112 pc.printf("motorValue1: %f\r\n", feedForwardControl(getReferenceVelocity()));
S1933191 4:37c22ab0f896 113 pc.printf("\n\n\n");
S1933191 4:37c22ab0f896 114
S1933191 4:37c22ab0f896 115 /*
S1933191 4:37c22ab0f896 116 scope.set(0, potMeterIn.read());
S1933191 4:37c22ab0f896 117 scope.set(1, feedForwardControl(getReferenceVelocity()));
S1933191 4:37c22ab0f896 118 scope.send();
S1933191 4:37c22ab0f896 119 */
S1933191 6:fdf0974a33fc 120 pc.printf("pot input2: %f\r\n", potMeterIn2.read());
S1933191 5:2fb7355a44c0 121 pc.printf("motorValue2: %f\r\n", feedForwardControl2(getReferenceVelocity2()));
S1933191 5:2fb7355a44c0 122 pc.printf("\n\n\n");
S1933191 5:2fb7355a44c0 123 }
S1933191 5:2fb7355a44c0 124
S1933191 4:37c22ab0f896 125 int main()
S1933191 4:37c22ab0f896 126 {
S1933191 6:fdf0974a33fc 127 pc.baud(115200);
S1933191 6:fdf0974a33fc 128
S1933191 6:fdf0974a33fc 129 button2.fall(&onButtonPress2);
S1933191 4:37c22ab0f896 130 controlTicker.attach(&measureAndControl, 1.0f/100.0f);
S1933191 4:37c22ab0f896 131 debugTicker.attach(&onDebugTick, 1);
S1933191 4:37c22ab0f896 132
S1933191 6:fdf0974a33fc 133 button1.fall(&onButtonPress);
S1933191 6:fdf0974a33fc 134
S1933191 2:8a9df5b1b638 135 }