Diff: main.cpp

Revision:

0:e6749361c960

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 12 14:21:37 2017 +0000
@@ -0,0 +1,142 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include <math.h>
+#include "HIDScope.h"
+#include "QEI.h"
+
+//Debugging tools
+MODSERIAL pc(USBTX, USBRX);
+
+//Declaring pins
+AnalogIn potMeterIn(A1);
+DigitalIn button1(D2);
+InterruptIn button2(D1);
+
+const PinName encoder1 = D12;
+const PinName encoder2 = D13;
+
+DigitalOut motor1DirectionPin(D4);
+PwmOut motor1MagnitudePin(D5);
+
+//Used for reading out encoder
+QEI qei(encoder1, encoder2, NC, 32); //32 is pulses per revolution
+HIDScope scope(2);
+
+
+//Declaring variables 
+float maxVelocity = 8.4f; // in rad/s
+float motorGain = 8.4f; // In rad/s
+const float sampletime = 1.0f/10.0f;
+bool clockwise = true;
+const double pi = 3.14159265358979323846;
+volatile bool direction  = clockwise; 
+volatile double lastEncoderRead=0; //In radians
+
+Ticker encoderTicker;
+Ticker motorControl;
+
+volatile bool setRotation = true; 
+
+float GetReferenceVelocity()
+{
+    // Returns reference velocity in rad/s. 
+    // Positive value means clockwise rotation.
+    //pc.printf("Begin GetreferenceVelocity\r\n");    
+    float referenceVelocity;  // in rad/s
+    if (button1)   
+        {
+        // Clockwise rotation      
+        referenceVelocity = potMeterIn * maxVelocity;  
+        } 
+    
+    else 
+        {
+        // Counterclockwise rotation       
+        referenceVelocity = -1*potMeterIn * maxVelocity;   
+        }
+    //pc.printf("End Getreferencevelocity\r\n");
+    return referenceVelocity;
+}
+
+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
+    //pc.printf("Begin Setmotor1\r\n"); 
+    if (motorValue >=0) motor1DirectionPin=1;
+        else motor1DirectionPin=0;
+    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
+        else motor1MagnitudePin = fabs(motorValue);
+    //pc.printf("End Setmotor1\r\n");
+}
+
+float FeedForwardControl(float referenceVelocity)
+{
+    //pc.printf("Begin FeedforwardControl\r\n");
+    // very simple linear feed-forward control
+    float motorValue = referenceVelocity / motorGain;
+    //pc.printf("End Feedforwardcontrol\r\n");
+    return motorValue;
+    
+    
+}
+
+void MeasureAndControl(void)
+{
+    //pc.printf("Begin MeasureAndControl\r\n");
+    // 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);
+    //pc.printf("End MeasureAndControl\r\n");
+}
+
+void encoderTick()
+{
+    int pulses = qei.getPulses();
+    double radians = (pulses/(32.0f*132.25f))*2.0f*pi; 
+    double degree = (pulses/(32.0f*131.25f))*365;
+    pc.printf("radians: %d\r\n", radians);
+    double limit = 360;
+    
+    if(degree > limit);
+    {   
+        setRotation =false; 
+    }
+    if (degree <-limit);
+    {
+        setRotation = false;
+    }
+    
+    scope.set(0, radians);
+    scope.set(1, degree);
+    scope.send();
+    lastEncoderRead = radians; 
+    
+}
+ 
+void status(){
+    pc.printf("pot input: %f\r\n", potMeterIn.read()); 
+    pc.printf("\n\n");
+    qei.reset();
+    }
+
+int main(){
+    pc.printf("Hello World!\r\n");
+    
+    qei.reset();
+    encoderTicker.attach(&encoderTick,sampletime);
+    motorControl.attach(&MeasureAndControl, 1.0f/10.0f);
+    button2.fall(&status);
+    
+    while (setRotation) 
+    {
+        MeasureAndControl();
+        wait(0.1f);
+    }
+    return 0;
+}