Diff: main.cpp

Revision:

16:e9945e3b4712

Parent:

14:102a2b4f5c86

Child:

17:034b50f49f46

--- a/main.cpp	Tue Oct 06 14:17:11 2015 +0000
+++ b/main.cpp	Wed Oct 07 11:45:27 2015 +0000
@@ -8,13 +8,18 @@
 #include "inits.h" // all globals, pin and variable initialization 
 
 void setGoFlag(){
-    if (goFlag==true && systemOn==true){
+    if (goFlag==true && systemOn==true && calcFlag==false){
         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;
 }
-
+void setCalcFlag(){
+    if (calcFlag==true && systemOn==true){
+        pc.printf("ERROR: INCORRECT TIMINGS, look at the setCalcFlag \n\r");   
+    }
+    calcFlag=true;     
+}
 void systemStart(){
     systemOn=true;
 }
@@ -29,14 +34,14 @@
     initMotors();
     initPIDs();
     
+    speedcalcTicker.attach(&setCalcFlag, calcRATE);
     motorControlTicker.attach(&setGoFlag, RATE);
     
     startButton.rise(&systemStart);
     stopButton.rise(&systemStop);
     
-    endTimer.start(); //Run for 100 seconds.
-    while (endTimer.read() < 100){
-        if (goFlag==true && systemOn==true){
+    while (true){
+        if (systemOn==true && calcFlag==true){
             // get 'emg' signal
             request_pos = pot1.read();
             request_neg = pot2.read();
@@ -51,29 +56,31 @@
             
             // 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 ){
+            // make them start at 45 degrees
+            leftAngle=(leftQei.getPulses()/round)*360+45;
+            rightAngle=(rightQei.getPulses()/round)*360+45;
+            
+            if (leftAngle < 0 or leftAngle > 90 or rightAngle < 0 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;}
+                //if (leftAngle < -45){leftRequest=0.1;}
+                //if (leftAngle > 45){leftRequest=-0.1;}
+                //if (rightAngle < -45){rightRequest=0.1;}
+                //if (rightAngle > 45){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
+                //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
+                //}
+                //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=currentX+request*calcRATE; // should be request*RATE
                 //toX=5;
                 //toY=pCurrent.posY+0*RATE;
                 toY=currentY;
@@ -86,15 +93,18 @@
                 rightDeltaAngle=toRightAngle-rightAngle;
                 
                 // calculate the neccesairy velocities to make these angles happen.
-                leftRequest=(leftDeltaAngle/RATE);
-                rightRequest=(rightDeltaAngle/RATE);
+                leftRequest=(leftDeltaAngle/calcRATE);
+                rightRequest=(rightDeltaAngle/calcRATE);
             }
-            
+            pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle);
+            calcFlag=false;
+        }
+        else if (systemOn == true && goFlag == true){
             // 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);
+            //pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle);
             
             // *******************
             // Velocity calculation