Diff: main.cpp

Revision:

5:a3848a66a4df

Parent:

4:983b50758735

Child:

6:1a410879a539

--- a/main.cpp	Fri Oct 06 09:05:35 2017 +0000
+++ b/main.cpp	Fri Oct 06 11:52:13 2017 +0000
@@ -24,6 +24,16 @@
 Ticker controller;
 
 
+
+float GetReferencePosition()
+{
+    int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position
+    
+    float referencePosition;
+    referencePosition = potMeterIn2 * potmultiplier;
+    return referencePosition;
+}
+
 float GetReferenceVelocity()
 {
     // Returns reference velocity in rad/s. 
@@ -48,24 +58,39 @@
     return referenceVelocity;
 }
 
-
-float FeedForwardControl(float &referenceVelocity)
+float FeedForwardControl(float referenceVelocity)
 {
     // very simple linear feed-forward control
     const float MotorGain=8.4; // unit: (rad/s) / PWM
-    float motorValue = referenceVelocity / MotorGain;
+    float motorValue; //= referenceVelocity / MotorGain;
+    
+   // float referenceVelocity = GetReferenceVelocity();
+    int referencePosition = GetReferencePosition();
+    int counts =  Encoder.getPulses();
+           
+        if(counts < referencePosition)
+        {
+            //float motorValue = FeedForwardControl(referenceVelocity);
+            //SetMotor1(motorValue);
+            motorValue = referenceVelocity / MotorGain;
+        }
+        else if (counts > referencePosition)
+        {
+            //float motorValue = b * FeedForwardControl(referenceVelocity);
+            //SetMotor1(motorValue);
+            
+            motorValue=  -1 * (referenceVelocity / MotorGain);
+        }  
+        else
+        {
+            motorValue = 0;
+        }
+        
+        pc.printf("motorvalue %f",motorValue);
+    
     return motorValue;
 }
 
-float GetReferencePosition()
-{
-    int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position
-    
-    float referencePosition;
-    referencePosition = potMeterIn2 * potmultiplier;
-    return referencePosition;
-}
-
 void SetMotor1(float motorValue)
 {
     // Given -1<=motorValue<=1, this sets the PWM and direction
@@ -82,7 +107,10 @@
 {
     float referenceVelocity = GetReferenceVelocity();
     int referencePosition = GetReferencePosition();
-    int counts =  Encoder.getPulses();
+    float motorValue = FeedForwardControl(referenceVelocity);
+    SetMotor1(motorValue);
+    
+    /*int counts =  Encoder.getPulses();
     
     
     
@@ -96,7 +124,7 @@
             int b = -1;
             float motorValue = b * FeedForwardControl(referenceVelocity);
             SetMotor1(motorValue);
-        }
+        } */
         
 }
 
@@ -120,7 +148,7 @@
         float mV = FeedForwardControl(rV);
         int rP = GetReferencePosition();
           
-        pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",mV,rP,rV,counts);
+        pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",rV,rP,mV,counts);
     }
    
 }