Diff: main.cpp

Revision:

0:1816743ba8ba

Child:

1:659489c32e75

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 04 12:46:37 2017 +0000
@@ -0,0 +1,81 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "math.h"
+
+
+
+DigitalOut gpo(D0);
+DigitalOut led(LED_BLUE);
+DigitalOut motor1DC(D6);
+DigitalOut motor1PWM(D7);
+DigitalOut motor2DC(D4);
+DigitalOut motor2PWM(D5);
+
+AnalogIn   potMeterIn(A0);
+DigitalIn   button1(D11);
+
+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;
+}
+
+
+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 SetMotor1(float motorValue)
+{
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    if (motorValue >=0) motor1DC=1;
+        else motor1DC=0;
+    if (fabs(motorValue)>1) motor1PWM = 1;
+        else motor1PWM = fabs(motorValue);
+}
+
+int main()
+{
+    pc.baud(115200);
+    
+    QEI Encoder(D12,D13,NC,32); // Input for the Encoder
+    
+    while (true) {
+        
+    float v = GetReferenceVelocity();
+    float controlValue = FeedForwardControl(v);
+    SetMotor1(controlValue);
+    
+    int counts;
+    counts = Encoder.getPulses();
+    pc.printf("\r number of counts: %i\n",counts);
+        
+    pc.printf("\t\r reference velocity is: %f. Motor Value is: %f \t\r number of counts: %i\n",v,controlValue,counts);
+    }
+}
+
+