Exercise #1 Feed Forward Control
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Fri Jul 29 2022 23:05:57 by
1.7.2