Diff: main.cpp

Revision:

2:665df4abd084

Parent:

1:f26a53da33ed

Child:

3:1ab2d2b1705f

--- a/main.cpp	Mon Oct 10 13:39:01 2016 +0000
+++ b/main.cpp	Mon Oct 17 12:53:03 2016 +0000
@@ -1,92 +1,125 @@
 #include "mbed.h"
+#include "MODSERIAL.h"
 #include "QEI.h"
 #include "math.h"
-
+#include "HIDScope.h"
 
-InterruptIn button1(D3);
-AnalogIn potMeterIn(A5);
-DigitalOut motor1DirectionPin(D4);
+//Defining ports
+DigitalOut motor1DirectionPin (D4);
 PwmOut motor1MagnitudePin(D5);
-
-Ticker foutprinter1;
-Ticker foutprinter2;
-Ticker callMotor;
+DigitalIn button(D2);
+AnalogIn potmeter(A0);
 Serial pc(USBTX,USBRX);
-
-volatile float motorValue=0.0;
-volatile float referenceVelocity=0.0;  // in rad/s
-const float maxVelocity=8.4; // in rad/s of course!
-const float MotorGain=8.4; // unit: (rad/s) / PWM
+QEI Encoder(D12,D13,NC,32);
+HIDScope scope(1);
 
-
-void foutprint1()
-{
- pc.printf("Draairichting = %i\n\r", motor1DirectionPin);
-}
+// Setting tickers and printers
+Ticker tick;
+Ticker callMotor;
+Ticker pos;
+Ticker encoderTicker;
 
-
-
-void foutprint2()
-{
- pc.printf("Ref Vel = %f\n\r", referenceVelocity)   ;
-}
-
-
+const float pi = 3.14159265359;
+const float ts = 1.0/1000.0;
+const int VELOCITY_CHANNEL = 0;
 
-float getReferenceVelocity()
-{
-    // Returns reference velocity in rad/s. 
+//Get reference velocity
+float GetReferenceVelocity() {
+    // Returns reference velocity in rad/s.
     // Positive value means clockwise rotation.
-    if (button1)
-        {
-          // Clockwise rotation
-            referenceVelocity = potMeterIn * maxVelocity;
-        } 
+    const float maxVelocity = 8.4; //Als de potmeter van 0 tot 1 gaat (als die maar tot 0.25 gaat, dan max velocity 4x zo groot als motorgain maken
+    volatile float referenceVelocity;  // in rad/s
+    if (button) //nog even kijken voor wanneer die + en - is
+    {
+        // Clockwise rotation
+        referenceVelocity = potmeter * maxVelocity;
+    } 
     else
-        {
+    {
         // Counterclockwise rotation
-        referenceVelocity = -1*potMeterIn * maxVelocity;
-        }
+        referenceVelocity = -1*potmeter * maxVelocity;
+    }
     return referenceVelocity;
 }
 
-float FeedForwardControl(float referenceVelocity)
-{
-    // very simple linear feed-forward control
- motorValue = referenceVelocity / MotorGain;
-    return motorValue;
-}    
-    
-void SetMotor1(float motorValue)
-{
-    // Given -1<=motorValue<=1, this sets the PWM and direction
+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.0) motor1DirectionPin=1;
-        else motor1DirectionPin=0;
-    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
-        else motor1MagnitudePin = fabs(motorValue);       
+    if (motorValue >=0)
+        {
+            motor1DirectionPin=1;
+        }
+    else
+        {
+            motor1DirectionPin=0;
+        }
+    if (fabs(motorValue)>1) 
+    {
+        motor1MagnitudePin = 1;
+    }
+    else
+    {
+        motor1MagnitudePin = 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)
-{
+void MeasureAndControl(void) {
     // This function measures the potmeter position, extracts a
-    // reference velocity from it, and controls the motor with 
+    // reference velocity from it, and controls the motor with        
     // a simple FeedForward controller. Call this from a Ticker.
-    referenceVelocity = getReferenceVelocity();
-    motorValue = FeedForwardControl(referenceVelocity);
+    float referenceVelocity = GetReferenceVelocity();
+    float motorValue = FeedForwardControl(referenceVelocity);
     SetMotor1(motorValue);
 }
+   
+        
+float getPosition() {
+        //getting the position of the motor to take derivative to have a
+        //different velocity signal.
+        const float ts = 1.0/1000.0;
+        volatile float pulses = Encoder.getPulses();
+        volatile float revolutions = Encoder.getRevolutions();
+        volatile float position = (pulses / 2 * 32) * 360; //formula from QEI library
+    return position;
+}       
  
+void encoderTick() {
+  int pulses = Encoder.getPulses();
+   
+   //calculate the total angle that is traveled so far.
+    double radians = (pulses / ts) * ((2*pi)/32);
+
+    //update lastEncoderRead such that it can be used for next time
+    double lastEncoderRead = radians;
+    
+    //approximate the derivative for the angular velocity.
+    double xdif = radians - lastEncoderRead;
+    double xderiv = xdif / ts;
+   
+    
+    
+    //send velocity to HIDScope
+   scope.set(VELOCITY_CHANNEL, radians);
+    scope.send();
+    }
+
+   
+        
 int main()
-{        
-motor1MagnitudePin.period(1.0);
-foutprinter1.attach(foutprint1,1.0f);
-foutprinter2.attach(foutprint2,1.0f);
-callMotor.attach(MeasureAndControl, 0.1f);
-         pc.baud(115200);
-    while (true) {             
-                } 
-}
-
+{
+   encoderTicker.attach(encoderTick, 1.0/100000.0); 
+    motor1MagnitudePin.period(1.0/100000.0);
+    callMotor.attach(MeasureAndControl,1);
+    pc.baud(115200);
+    //tick.attach(print,1);
+    while(true){}
+}
\ No newline at end of file