Diff: main.cpp

Revision:

4:983b50758735

Parent:

3:cc3766838777

Child:

5:a3848a66a4df

--- a/main.cpp	Thu Oct 05 13:10:39 2017 +0000
+++ b/main.cpp	Fri Oct 06 09:05:35 2017 +0000
@@ -15,11 +15,12 @@
 PwmOut motor2PWM(D5);
 
 AnalogIn   potMeterIn1(A0);
-AnalogIn   PotMeterIn2(A1);
+AnalogIn   potMeterIn2(A1);
 
 DigitalIn   button1(D11);
 
 MODSERIAL pc(USBTX,USBRX);
+QEI Encoder(D12,D13,NC,32);
 Ticker controller;
 
 
@@ -56,6 +57,15 @@
     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
@@ -71,8 +81,23 @@
 void MeasureAndControl(void)
 {
     float referenceVelocity = GetReferenceVelocity();
-    float motorValue = FeedForwardControl(referenceVelocity);
-    SetMotor1(motorValue);
+    int referencePosition = GetReferencePosition();
+    int counts =  Encoder.getPulses();
+    
+    
+    
+        if(counts < referencePosition)
+        {
+            float motorValue = FeedForwardControl(referenceVelocity);
+            SetMotor1(motorValue);
+        }
+        else
+        {
+            int b = -1;
+            float motorValue = b * FeedForwardControl(referenceVelocity);
+            SetMotor1(motorValue);
+        }
+        
 }
 
 
@@ -93,8 +118,9 @@
         int counts = Encoder.getPulses(); 
         float rV = GetReferenceVelocity();
         float mV = FeedForwardControl(rV);
+        int rP = GetReferencePosition();
           
-        pc.printf("\r reference velocity: %f. Motor Value is: %f number of counts: %i\n",mV,rV,counts);
+        pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",mV,rP,rV,counts);
     }
    
 }