Biorobotica TIC / Mbed 2 deprecated Motoraansturing

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:e8e408160110, 2018-09-26 (annotated)

Committer:
Hubertus
Date:
Wed Sep 26 13:45:54 2018 +0000
Revision:
0:e8e408160110
1 motor aangestuurd met Potmeter en RWM

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hubertus 0:e8e408160110 1 #include "mbed.h"
Hubertus 0:e8e408160110 2 #include "math.h"
Hubertus 0:e8e408160110 3 //#include "HIDScope.h"
Hubertus 0:e8e408160110 4
Hubertus 0:e8e408160110 5 // ------ Hardware Interfaces -------
Hubertus 0:e8e408160110 6
Hubertus 0:e8e408160110 7 const PinName motor1dir = D7;
Hubertus 0:e8e408160110 8 const PinName motor1PWM = D6;
Hubertus 0:e8e408160110 9 //const PinName motor2PWM = D5;
Hubertus 0:e8e408160110 10 //const PinName motor2dir = D4;
Hubertus 0:e8e408160110 11
Hubertus 0:e8e408160110 12 DigitalOut motor1direction(motor1dir);
Hubertus 0:e8e408160110 13 PwmOut motor1control(motor1PWM);
Hubertus 0:e8e408160110 14 //PwmOut motor2control(motor2PWM);
Hubertus 0:e8e408160110 15 //DigitalOut motor2direction(motor2dir);
Hubertus 0:e8e408160110 16
Hubertus 0:e8e408160110 17 const PinName button1name = D1;
Hubertus 0:e8e408160110 18 const PinName pot1name = A1;
Hubertus 0:e8e408160110 19 InterruptIn button1(button1name);
Hubertus 0:e8e408160110 20 AnalogIn potMeterIn(pot1name);
Hubertus 0:e8e408160110 21
Hubertus 0:e8e408160110 22 // ------- Constants
Hubertus 0:e8e408160110 23
Hubertus 0:e8e408160110 24 const float motorGain = 6.3f;
Hubertus 0:e8e408160110 25 const float maxVelocity = 6.3f; //radians per second
Hubertus 0:e8e408160110 26 const bool clockwise = true;
Hubertus 0:e8e408160110 27
Hubertus 0:e8e408160110 28 // ------ variables
Hubertus 0:e8e408160110 29
Hubertus 0:e8e408160110 30 volatile bool direction = clockwise;
Hubertus 0:e8e408160110 31
Hubertus 0:e8e408160110 32
Hubertus 0:e8e408160110 33 // ------- Objects used -------
Hubertus 0:e8e408160110 34
Hubertus 0:e8e408160110 35 // Ticker which controls the mother every 1/100 of a second.
Hubertus 0:e8e408160110 36 Ticker controlTicker;
Hubertus 0:e8e408160110 37
Hubertus 0:e8e408160110 38 Ticker debugTicker;
Hubertus 0:e8e408160110 39 Serial pc(USBTX, USBRX);
Hubertus 0:e8e408160110 40
Hubertus 0:e8e408160110 41 //------Functions
Hubertus 0:e8e408160110 42
Hubertus 0:e8e408160110 43 float getReferenceVelocity() {
Hubertus 0:e8e408160110 44 // Returns reference velocity in rad/s.
Hubertus 0:e8e408160110 45 return maxVelocity * potMeterIn;
Hubertus 0:e8e408160110 46 }
Hubertus 0:e8e408160110 47
Hubertus 0:e8e408160110 48 void setMotor1(float motorValue) {
Hubertus 0:e8e408160110 49 // Given motorValue<=1, writes the velocity to the pwm control.
Hubertus 0:e8e408160110 50 // MotorValues outside range are truncated to within range.
Hubertus 0:e8e408160110 51 motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
Hubertus 0:e8e408160110 52 }
Hubertus 0:e8e408160110 53 float feedForwardControl(float referenceVelocity) {
Hubertus 0:e8e408160110 54 // very simple linear feed-forward control
Hubertus 0:e8e408160110 55 // returns motorValue
Hubertus 0:e8e408160110 56 return referenceVelocity / motorGain;
Hubertus 0:e8e408160110 57 }
Hubertus 0:e8e408160110 58
Hubertus 0:e8e408160110 59 void measureAndControl(void) {
Hubertus 0:e8e408160110 60 // This function measures the potmeter position, extracts a
Hubertus 0:e8e408160110 61 // reference velocity from it, and controls the motor with
Hubertus 0:e8e408160110 62 // a simple FeedForward controller. Call this from a Ticker.
Hubertus 0:e8e408160110 63 float referenceVelocity = getReferenceVelocity();
Hubertus 0:e8e408160110 64 float motorValue = feedForwardControl(referenceVelocity);
Hubertus 0:e8e408160110 65 setMotor1(motorValue);
Hubertus 0:e8e408160110 66 }
Hubertus 0:e8e408160110 67
Hubertus 0:e8e408160110 68 void onButtonPress() {
Hubertus 0:e8e408160110 69 // reverses the direction
Hubertus 0:e8e408160110 70 motor1direction.write(direction = !direction);
Hubertus 0:e8e408160110 71 pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise");
Hubertus 0:e8e408160110 72 }
Hubertus 0:e8e408160110 73
Hubertus 0:e8e408160110 74 void onDebugTick() {
Hubertus 0:e8e408160110 75
Hubertus 0:e8e408160110 76 pc.printf("pot input: %f\r\n", potMeterIn.read());
Hubertus 0:e8e408160110 77 pc.printf("motorValue: %f\r\n", feedForwardControl(getReferenceVelocity()));
Hubertus 0:e8e408160110 78 pc.printf("\n\n\n");
Hubertus 0:e8e408160110 79
Hubertus 0:e8e408160110 80 }
Hubertus 0:e8e408160110 81
Hubertus 0:e8e408160110 82
Hubertus 0:e8e408160110 83 //----------- Main function
Hubertus 0:e8e408160110 84 int main()
Hubertus 0:e8e408160110 85 {
Hubertus 0:e8e408160110 86 pc.baud(115200);
Hubertus 0:e8e408160110 87
Hubertus 0:e8e408160110 88 button1.fall(&onButtonPress);
Hubertus 0:e8e408160110 89 controlTicker.attach(&measureAndControl, 1.0/300.0); //1f/100f is elke tijd dat hij deze functie weer aanroept
Hubertus 0:e8e408160110 90 debugTicker.attach(&onDebugTick, 1); // elke seconde geeft hij waardes weer op pc
Hubertus 0:e8e408160110 91
Hubertus 0:e8e408160110 92 while (true);
Hubertus 0:e8e408160110 93 }