Diff: Controller.cpp

Revision:

8:82e38f4aa690

Parent:

6:a4440eec3652

--- a/Controller.cpp	Wed Nov 02 15:40:15 2016 +0000
+++ b/Controller.cpp	Mon Nov 07 11:29:28 2016 +0000
@@ -16,7 +16,6 @@
 
 void run() 
 {
-    //pc.printf("ik ga nu runnen G \n\r");
     motorControl.attach(&setUpdate_flag, RATE); // rate =0.001
     while(button) // button not presseed
     {
@@ -24,47 +23,42 @@
         { 
             // returns filtered value
             r = signal.readValue(1); 
-            l = signal.readValue(2);
-            //pc.printf("rechts is : %f\n\r", r);
-            //pc.printf("links is  : %f\n\r", l);
+            l = signal.readValue(2);         
+            u = signal.readValue(3);
+            d = signal.readValue(4);
             
-            //u = signal.readValue(3);
-           // d = signal.readValue(4);
+            pc.printf("r : %f \n\r", r);
+            pc.printf("l : %f \n\r", l);           
             // calculate r,l,u,d relative to the max [0,1]
             percentageR = r/rMax;
             percentageL = l/lMax;
             percentageU = u/uMax;
             percentageD = d/dMax;
-            //scope.set(0,percentageR);
-            //scope.set(1,percentageL);
-            //scope.send();
-            //pc.printf("wat is percentageL : %f \n\r", percentageL);
+            
             // ignore the weaker signal,motorValue calculation, threshold set
             if ( percentageR > percentageL)
             { // horizontal speed request
                 motorValueX = percentageR;
-                //pc.printf("rechts > links : %f\n\r", motorValueX);
             }   
             else 
             {
                 motorValueX = -percentageL;
-                //pc.printf("rechts < links : %f\n\r", motorValueX);
             }
+            
             if (percentageU > percentageD) // vertical speed request
             { 
                 motorValueY = percentageU;
-            }   
+            }
+               
             else 
             {
                 motorValueY = -percentageD;
             }
             
-            //pc.printf("motorValueX : %f\n\r", motorValueX);
             // current pulses
             xPulses = X_Motor.getPulses();
-           // pc.printf("curentpulses : %f \n\r", xPulses);
             yPulses = Y_Motor.getPulses();
-            
+           
             //edge detection
             if ((xPulses > (x_max-margin)) && (motorValueX > sensitivityFactor)) // (x_max-margin
             { // right should correspond to positive motorvalue
@@ -78,27 +72,31 @@
             { // up should correspond to positive motorvalue
                 motorValueY = 0;
             }
-            if ((yPulses < margin) && (motorValueY < -sensitivityFactor)) 
+            if ((yPulses < margin) && (motorValueY < ((-1) * sensitivityFactor))) 
             {
-                motorValueY =0;
+                motorValueY = 0;
             } 
             // Quantize the linear speed request
-            // motorValue is a value [-1,1], currently only 0 or 1/-1 
-            if (motorValueX >= maxXValue){motorValueX = maxXValue;} // safety
-                else if (motorValueX <= -maxXValue){motorValueX = -maxXValue;}
-                    else if ((motorValueX >= sensitivityFactor) && (motorValueX < maxXValue)){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here
-                        else if ((motorValueX <= -sensitivityFactor) && (motorValueX > -maxXValue)){motorValueX = -maxXValue;}
-                            else {motorValueX = 0;}
+            // motorValue is a value [-1,1], currently only 0 or 1/-1
+            
+             //pc.printf("X_MotorValue : %f \n\r", motorValueX);
+            if (motorValueX >= maxXValue){motorValueX = maxXValue;} // boundary for the motor value
+            else if (motorValueX <= -maxXValue){motorValueX = -maxXValue;}
+            else if ((motorValueX >= sensitivityFactor) && (motorValueX < maxXValue)){motorValueX = maxXValue;} // if threshold is met go a fixed speed%, we could quantize here
+            else if ((motorValueX <= -sensitivityFactor) && (motorValueX > -maxXValue)){motorValueX = -maxXValue;}
+            else {motorValueX = 0;}
+            
             if (motorValueY >= maxYValue){motorValueY = maxYValue;}
-                else if (motorValueY <= -maxYValue){motorValueY = -maxYValue;}
-                    else if ((motorValueY >= sensitivityFactor) && (motorValueY <= maxYValue)){motorValueY = maxYValue;}
-                        else if ((motorValueY <= -sensitivityFactor) && (motorValueY >= -maxYValue)){motorValueY = -maxYValue;}
-                            else {motorValueX = 0;}
-            //pc.printf("dit moet nu value hebben als je aanspant : %f \n\r", motorValueX);        
+            else if (motorValueY <= -maxYValue){motorValueY = -maxYValue;}
+            else if ((motorValueY >= sensitivityFactor) && (motorValueY < maxYValue)){motorValueY = maxYValue;}
+            else if ((motorValueY <= -sensitivityFactor) && (motorValueY > -maxYValue)){motorValueY = -maxYValue;}
+            else {motorValueY = 0;}
+            
+            
+            
             // current position of pen in Meters
             float currentX = (xPulses/(8400.0f))*(2*PI)*RmX;
             float currentY = (yPulses/(8400.0f))*(2*PI)*Rpulley; 
-            //pc.printf("currentX : %f \n\r", currentX);
             
             // upcoming position of pen in Meters
             //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s
@@ -107,11 +105,11 @@
             // distance it should travel for 1 interval in Meters 
             float deltaX = motorValueX*maxSpeed*RATE;
             float deltaY = motorValueY*maxSpeed*RATE;
-            //pc.printf("deltaX : %f \n\r", deltaX);
+            
             // desired velocity in m/s for this interval
-            float toVelX = deltaX/RATE; 
+            float toVelX = deltaX/RATE;
             float toVelY = deltaY/RATE;
-            //pc.printf("toVelX : %f \n\r", toVelX);
+            
             // set setpoint of PID controller, this is the thing you want
             X_Controller.setSetPoint(toVelX);
             Y_Controller.setSetPoint(toVelY);
@@ -120,51 +118,78 @@
             // horizontal motor
             float currVelX = (currentX - prevX)/RATE;
             prevX = currentX;
-            //pc.printf("currVe;X : %f \n\r", currVelX);
             X_Controller.setProcessValue(currVelX);
             
             //vertical motor
             float currVelY = (currentY - prevY)/RATE;
             prevY = currentY;
             Y_Controller.setProcessValue(currVelY);
+           
+           
+           
             
             // compute the output
-            float X_MotorOutput = 10*(X_Controller.compute());
-            pc.printf("X_MotorOutput : %f \n\r", X_MotorOutput);
-            
-            if (X_MotorOutput >= X_frictionTrheshold) // hardcoded threshold internal friction
+            float X_MotorOutput = 9 * X_Controller.compute();
+           //pc.printf("X_MotorOutput : %f \n\r", X_MotorOutput);    
+           
+            if (X_MotorOutput > maxSpeed) // 
             { // right request
                 X_Direction.write(CW);
-                X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1
+                X_Magnitude.write(maxSpeed); // 
             }   
-            else if (X_MotorOutput <= -X_frictionTrheshold)
+            else if (X_MotorOutput <= -maxSpeed)
             {
                 X_Direction.write(!CW); // left request
-                X_Magnitude.write(fabs(X_MotorOutput)); //motor output will be negative and you need to write a pos value
+                X_Magnitude.write(maxSpeed); //motor output will be negative and you need to write a pos value
+            }
+            else if ((X_MotorOutput < maxSpeed) && (X_MotorOutput > sensitivityFactor))
+            {
+                X_Magnitude.write(X_MotorOutput);
+                X_Direction.write(CW);
             }
-            else // else the internal friction is not compensated
+                        
+            else if ((X_MotorOutput > -maxSpeed) && (X_MotorOutput < -sensitivityFactor))
+            {
+                X_Magnitude.write(X_MotorOutput);
+                X_Direction.write(!CW);
+            }
+            else 
             {
                 X_Magnitude.write(0);
-                X_Direction.write(CW);
+                }
+            
+
+            float Y_MotorOutput = 9 * Y_Controller.compute();
+            if (Y_MotorOutput > maxSpeed) // 
+            { // right request
+                Y_Direction.write(CW);
+                Y_Magnitude.write(maxSpeed); // 
+            }   
+            else if (Y_MotorOutput <= -maxSpeed)
+            {
+                Y_Direction.write(!CW); // left request
+                Y_Magnitude.write(maxSpeed); //motor output will be negative and you need to write a pos value
+            }
+            else if ((Y_MotorOutput < maxSpeed) && (Y_MotorOutput > sensitivityFactor))
+            {
+                Y_Magnitude.write(Y_MotorOutput);
+                Y_Direction.write(CW);
             }
             
-            float Y_MotorOutput = Y_Controller.compute();
-            if (Y_MotorOutput >= Y_frictionTrheshold)
-            { // up request
-                Y_Direction.write(CW);
+               
+            else if ((Y_MotorOutput > -maxSpeed) && (Y_MotorOutput < -sensitivityFactor))
+            {
                 Y_Magnitude.write(Y_MotorOutput);
-            }   
-            else if (Y_MotorOutput <= -Y_frictionTrheshold) 
-            {
-                Y_Direction.write(!CW); // down request
-                Y_Magnitude.write(fabs(Y_MotorOutput));
+                Y_Direction.write(!CW);
             }
-            else
+            
+            else 
             {
                 Y_Magnitude.write(0);
-                Y_Direction.write(CW);
             }
+            
             update_flag = false;
+            
         }
     }
     motorControl.detach();
@@ -180,11 +205,12 @@
     while ((X_Motor.getPulses() > margin) || (Y_Motor.getPulses() > margin)){
         float pulses = X_Motor.getPulses();
         printf("pulsesX : %f\n\r", pulses);
+        
         if (X_Motor.getPulses() > margin)
         {
         // go direction LEFT
             X_Direction.write(!CW);
-            X_Magnitude.write(0.4); // 8 RPM with no load, maybe compensate for internal friction
+            X_Magnitude.write(calibrationSpeed); // 8 RPM with no load, maybe compensate for internal friction
         }   
         else 
         { //motormagX 0 
@@ -195,7 +221,7 @@
         {
         // go direction DOWN
             Y_Direction.write(!CW);
-            Y_Magnitude.write(0.4);
+            Y_Magnitude.write(calibrationSpeed);
         }   
         else 
         { //motormagY 0  
@@ -225,7 +251,7 @@
             {
                 lMax = lCal;
             }
-            /*
+            
             float uCal = signal.readValue(3);
             if ( uCal > uMax)
             {
@@ -236,10 +262,7 @@
             {
                 dMax = dCal;
             }
-            */
-            counter++;
-            //pc.printf("rCal: %f \n\r", rCal);
-            
+            counter++;         
             update_flag = false;
         }  
     }
@@ -248,6 +271,4 @@
     pc.printf("lMax : %f\n\r", lMax);
     motorControl.detach();
     led = true;
-    //pc.printf("ik moet hier led weer uitzetten \n\r");
-
 }