Diff: main.cpp

Revision:

6:1a410879a539

Parent:

5:a3848a66a4df

Child:

7:080805fc3cf0

--- a/main.cpp	Fri Oct 06 11:52:13 2017 +0000
+++ b/main.cpp	Fri Oct 27 09:38:09 2017 +0000
@@ -27,7 +27,7 @@
 
 float GetReferencePosition()
 {
-    int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position
+    int potmultiplier = 8000; // constant to multiply the pot 2 value with to get a reference position
     
     float referencePosition;
     referencePosition = potMeterIn2 * potmultiplier;
@@ -39,36 +39,45 @@
     // Returns reference velocity in rad/s. 
     // Positive value means clockwise rotation.
     
+    int referencePosition = GetReferencePosition();
+    int counts =  Encoder.getPulses();
+    
     const float maxVelocity=8.4; // in rad/s of course!
     
     float referenceVelocity;  // in rad/s
     
-    if (button1)   {
+    if (counts > referencePosition)   {
         // Clockwise rotation
-        referenceVelocity = potMeterIn1 * maxVelocity;
+        referenceVelocity = -1*potMeterIn1 * maxVelocity;
         ledr = 1;
+        ledg = 1;
         ledb = 0;
         } 
-        else {
+        else if ( counts < referencePosition){
         // Counterclockwise rotation
-        referenceVelocity = -1*potMeterIn1 * maxVelocity;
+        referenceVelocity = potMeterIn1 * maxVelocity;
         ledb = 1;
+        ledg = 1;
         ledr = 0;
         }
+        else{
+        referenceVelocity = 0;
+        ledb = 1;
+        ledr = 1;
+        ledg = 0;
+        }
     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)
+               
+        /* if(counts < referencePosition)
         {
             //float motorValue = FeedForwardControl(referenceVelocity);
             //SetMotor1(motorValue);
@@ -84,9 +93,7 @@
         else
         {
             motorValue = 0;
-        }
-        
-        pc.printf("motorvalue %f",motorValue);
+        }*/
     
     return motorValue;
 }
@@ -139,6 +146,8 @@
     ledb = 1;
     ledg = 1;
     
+    
+    
     controller.attach(&MeasureAndControl, 0.1);
        
     while(1)