Exercise #1 Feed Forward Control

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 //#include "HIDScope.h"
00004 
00005 // ------ Hardware Interfaces -------
00006 
00007 const PinName motor1dir = D7;
00008 const PinName motor1PWM = D6;
00009 //const PinName motor2PWM = D5;
00010 //const PinName motor2dir = D4;
00011 
00012 DigitalOut motor1direction(motor1dir);
00013 PwmOut motor1control(motor1PWM);
00014 //PwmOut motor2control(motor2PWM);
00015 //DigitalOut motor2direction(motor2dir);
00016 
00017 const PinName button1name = D10; //for some reason D8 does not seem to work. wtf!
00018 const PinName pot1name = A0;
00019 InterruptIn button1(button1name);
00020 AnalogIn potMeterIn(pot1name);
00021 
00022 // ------- Objects used -------
00023 
00024 // Ticker which controls the mother every 1/100 of a second.
00025 Ticker controlTicker;
00026 
00027 Ticker debugTicker;
00028 Serial pc(USBTX, USBRX);
00029 
00030 //HIDScope scope(2); //fuck you hidscope we're not using you!
00031 
00032 
00033 // ------- Constants
00034 
00035 const float motorGain = 8.4f;
00036 const float maxVelocity = 8.4f; //radians per second
00037 const bool clockwise = true;
00038 
00039 
00040 // ------ variables
00041 
00042 volatile bool direction = clockwise;
00043 
00044 // ------ functions
00045 
00046 float getReferenceVelocity() {
00047     // Returns reference velocity in rad/s. 
00048     return maxVelocity * potMeterIn;
00049 }
00050 
00051 void setMotor1(float motorValue) {
00052     // Given motorValue<=1, writes the velocity to the pwm control.
00053     // MotorValues outside range are truncated to within range.
00054     motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
00055 }
00056 
00057 float feedForwardControl(float referenceVelocity) {
00058     // very simple linear feed-forward control
00059     // returns motorValue
00060     return referenceVelocity / motorGain;
00061 }
00062 
00063 void measureAndControl(void) {
00064     // This function measures the potmeter position, extracts a
00065     // reference velocity from it, and controls the motor with 
00066     // a simple FeedForward controller. Call this from a Ticker.
00067     float referenceVelocity = getReferenceVelocity();
00068     float motorValue = feedForwardControl(referenceVelocity);
00069     setMotor1(motorValue);
00070 }
00071 
00072 void onButtonPress() {
00073     // reverses the direction
00074     motor1direction.write(direction = !direction);
00075     pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise");
00076 }
00077 
00078 void onDebugTick() {
00079     
00080     pc.printf("pot input: %f\r\n", potMeterIn.read());
00081     pc.printf("motorValue: %f\r\n", feedForwardControl(getReferenceVelocity()));
00082     pc.printf("\n\n\n");
00083     
00084     /*
00085     scope.set(0, potMeterIn.read());
00086     scope.set(1, feedForwardControl(getReferenceVelocity()));
00087     scope.send();
00088     */
00089 }
00090 
00091 int main()
00092 {
00093     pc.baud(115200);
00094     
00095     button1.fall(&onButtonPress);
00096     controlTicker.attach(&measureAndControl, 1.0f/100.0f);
00097     debugTicker.attach(&onDebugTick, 1);
00098     
00099     while (true);
00100 }