Diff: main.cpp

Revision:

14:102a2b4f5c86

Parent:

13:40141b362092

Child:

16:e9945e3b4712

--- a/main.cpp	Tue Oct 06 11:30:38 2015 +0000
+++ b/main.cpp	Tue Oct 06 14:17:11 2015 +0000
@@ -3,10 +3,13 @@
 #include "PID.h"
 #include "QEI.h"
 #include "HIDScope.h"
+#include <math.h>
+//#include "Point.cpp"
 #include "inits.h" // all globals, pin and variable initialization 
 
 void setGoFlag(){
-    if (goFlag==true){
+    if (goFlag==true && systemOn==true){
+        pc.printf("ERROR: INCORRECT TIMINGS, look at the setGoFlag \n\r");
         // flag is already set true: code too slow or frequency too high    
     }
     goFlag=true;
@@ -38,15 +41,60 @@
             request_pos = pot1.read();
             request_neg = pot2.read();
             
-            // determine if forward or backward signal is stronger and set reference
+            // determine if forward or backward signal is stronger
             if (request_pos > request_neg){
                 request = request_pos; 
             } 
             else {
                 request = -request_neg;
             }
-            leftController.setSetPoint(request);
-            rightController.setSetPoint(request);
+            
+            // calculate required rotational velocity from the requested horizontal velocity
+            // first get the current position from the motor encoders
+            leftAngle=(leftQei.getPulses()/round)*360;
+            rightAngle=(rightQei.getPulses()/round)*360;
+            if (leftAngle < -90 or leftAngle > 90 or rightAngle < -90 or rightAngle > 90 ){
+                pc.printf("out of bounds \n\r");
+                leftRequest=0;
+                rightRequest=0;
+                if (leftAngle < -90){leftRequest=0.1;}
+                if (leftAngle > 90){leftRequest=-0.1;}
+                if (rightAngle < -90){rightRequest=0.1;}
+                if (rightAngle > 90){rightRequest=-0.1;}
+            }
+            else {
+                currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
+                currentY = tan(leftAngle*M_PI/180)*currentX;
+                
+                if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
+                    //return 0;
+                }
+                if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
+                    //return 0;
+                }
+                // calculate the position to go to according the the current position + the distance that should be covered in this timestep
+                toX=currentX+request*RATE*10; // should be request*RATE
+                //toX=5;
+                //toY=pCurrent.posY+0*RATE;
+                toY=currentY;
+            
+                toLeftAngle = atan(toY/toX)*180/M_PI;
+                toRightAngle = atan(toY/(l-toX))*180/M_PI;
+                
+                // calculate how much the angles should change in this timestep
+                leftDeltaAngle=toLeftAngle-leftAngle;
+                rightDeltaAngle=toRightAngle-rightAngle;
+                
+                // calculate the neccesairy velocities to make these angles happen.
+                leftRequest=(leftDeltaAngle/RATE);
+                rightRequest=(rightDeltaAngle/RATE);
+            }
+            
+            // set the PID controller to go with that speed.
+            leftController.setSetPoint(leftRequest);
+            rightController.setSetPoint(rightRequest);
+            
+            pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle);
             
             // *******************
             // Velocity calculation
@@ -80,20 +128,22 @@
             rightController.setProcessValue(rightVelocity);
             rightPwmDuty = rightController.compute();
             if (rightPwmDuty < 0){
-                rightDirection = 0;
+                rightDirection = 1;
                 rightMotor = -rightPwmDuty;
             }
             else {
-                rightDirection = 1;
+                rightDirection = 0;
                 rightMotor = rightPwmDuty;
             }
             
             // User feedback
-            scope.set(0, request);
+            scope.set(0, rightRequest);
             scope.set(1, rightPwmDuty);
             scope.set(2, rightVelocity);
+            scope.set(3, leftAngle);
+            scope.set(4, rightAngle);
             scope.send();
-            pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
+            //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
         
             goFlag=false;
         }