Diff: main.cpp

Revision:

0:331597250051

Child:

1:92a60278860a

diff -r 000000000000 -r 331597250051 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 03 15:30:08 2017 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "math.h"
+
+DigitalOut gpo(D0);
+DigitalOut motorDirection(D4);
+DigitalOut motorSpeed(D5);
+AnalogIn potMeterIn(A1);
+InterruptIn button1(D3);
+Ticker ticker;
+
+MODSERIAL pc(USBTX, USBRX);
+
+
+float GetReferenceVelocity()
+{
+    // Returns reference velocity in rad/s. 
+    // Positive value means clockwise rotation.
+    const float maxVelocity=8.4; // in rad/s of course!  
+    float referenceVelocity;  // in rad/s
+    if (button1)   {
+        // Clockwise rotation  
+        referenceVelocity = potMeterIn * maxVelocity;
+        } 
+    else {
+        // Counterclockwise rotation
+        referenceVelocity = -1*potMeterIn * maxVelocity;
+          }
+    return referenceVelocity;
+}
+
+void setMotor(float motorValue) {
+        if (motorValue >= 0) 
+        {
+            //float motor1DirectionPin1 = 1;
+            motorDirection=1;
+        }
+        else
+        {
+            //float motor1DirectionPin1 = 0;
+            motorDirection=0;
+        }
+        
+        if (fabs(motorValue)>1)
+        {
+          //float motor1MagnitudePin1 = 1;
+            motorSpeed = 1;
+         }
+         else  
+         {
+             //float motor1MagnitudePin1 = fabs(motorValue); 
+             motorSpeed = fabs(motorValue); 
+        }        
+}
+
+float FeedForwardControl(float referenceVelocity)
+{
+    // very simple linear feed-forward control
+    const float MotorGain=8.4; // unit: (rad/s) / PWM
+    float motorValue = referenceVelocity / MotorGain;
+    return motorValue;
+}
+
+
+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);
+    setMotor(motorValue);
+}
+
+int main() {
+    pc.baud(115200);
+    //ticker.attach(MeasureAndControl, 0.01);
+    while(true){
+        wait(0.1);
+        
+        //pc.printf("%f\r\n",GetReferenceVelocity());
+        float v_ref = GetReferenceVelocity();
+        setMotor(v_ref);
+        pc.printf("%f \r\n", FeedForwardControl(v_ref));
+        motorDirection.write(motorDirection);
+        motorSpeed.write(motorSpeed);   //PWM Speed Control
+    }
+}
+    
\ No newline at end of file