Diff: main.cpp

Revision:

0:936183552c73

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 07 14:09:15 2016 +0000
@@ -0,0 +1,100 @@
+#include "mbed.h"
+#include "math.h"
+//#include "HIDScope.h"
+
+// ------ Hardware Interfaces -------
+
+const PinName motor1dir = D7;
+const PinName motor1PWM = D6;
+//const PinName motor2PWM = D5;
+//const PinName motor2dir = D4;
+
+DigitalOut motor1direction(motor1dir);
+PwmOut motor1control(motor1PWM);
+//PwmOut motor2control(motor2PWM);
+//DigitalOut motor2direction(motor2dir);
+
+const PinName button1name = D10; //for some reason D8 does not seem to work. wtf!
+const PinName pot1name = A0;
+InterruptIn button1(button1name);
+AnalogIn potMeterIn(pot1name);
+
+// ------- Objects used -------
+
+// Ticker which controls the mother every 1/100 of a second.
+Ticker controlTicker;
+
+Ticker debugTicker;
+Serial pc(USBTX, USBRX);
+
+//HIDScope scope(2); //fuck you hidscope we're not using you!
+
+
+// ------- Constants
+
+const float motorGain = 8.4f;
+const float maxVelocity = 8.4f; //radians per second
+const bool clockwise = true;
+
+
+// ------ variables
+
+volatile bool direction = clockwise;
+
+// ------ functions
+
+float getReferenceVelocity() {
+    // Returns reference velocity in rad/s. 
+    return maxVelocity * potMeterIn;
+}
+
+void setMotor1(float motorValue) {
+    // Given motorValue<=1, writes the velocity to the pwm control.
+    // MotorValues outside range are truncated to within range.
+    motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
+}
+
+float feedForwardControl(float referenceVelocity) {
+    // very simple linear feed-forward control
+    // returns motorValue
+    return referenceVelocity / motorGain;
+}
+
+void measureAndControl(void) {
+    // This function measures the potmeter position, extracts a
+    // reference velocity from it, and controls the motor with 
+    // a simple FeedForward controller. Call this from a Ticker.
+    float referenceVelocity = getReferenceVelocity();
+    float motorValue = feedForwardControl(referenceVelocity);
+    setMotor1(motorValue);
+}
+
+void onButtonPress() {
+    // reverses the direction
+    motor1direction.write(direction = !direction);
+    pc.printf("direction: %s\r\n\n", direction ? "clockwise" : "counter clockwise");
+}
+
+void onDebugTick() {
+    
+    pc.printf("pot input: %f\r\n", potMeterIn.read());
+    pc.printf("motorValue: %f\r\n", feedForwardControl(getReferenceVelocity()));
+    pc.printf("\n\n\n");
+    
+    /*
+    scope.set(0, potMeterIn.read());
+    scope.set(1, feedForwardControl(getReferenceVelocity()));
+    scope.send();
+    */
+}
+
+int main()
+{
+    pc.baud(115200);
+    
+    button1.fall(&onButtonPress);
+    controlTicker.attach(&measureAndControl, 1.0f/100.0f);
+    debugTicker.attach(&onDebugTick, 1);
+    
+    while (true);
+}
\ No newline at end of file