Diff: main.cpp

Revision:

6:ae2ce89dd695

Parent:

5:e52055ff2bfe

Child:

7:572e7f3e184a

--- a/main.cpp	Mon Oct 19 11:46:57 2015 +0000
+++ b/main.cpp	Tue Oct 20 07:52:45 2015 +0000
@@ -15,6 +15,11 @@
     }
     goFlag=true;
 }
+void initpositiongo(){
+    if(initposition_go){initposition_go=false;}
+    else {initposition_go=true;}
+    
+}
 
 void systemStart(){
     systemOn=true;
@@ -37,6 +42,7 @@
     // interrupts
     motorControlTicker.attach(&setGoFlag, RATE);
     cali_button.rise(&calibratego);
+    initpositionButton.rise(&initpositiongo);
     startButton.rise(&systemStart);
     stopButton.rise(&systemStop);
     
@@ -56,85 +62,91 @@
                 
             // **************
             // get emg signal
-            sample_filter();
-            
-            // determine if forward or backward signal is stronger
-            if (y1 > y2){
-                horrequest = y1; 
-            } 
-            else {
-                horrequest = -y2;
+            if(initposition_go){
+                leftRequest=pot1.read()*10-5;
+                rightRequest=pot2.read()*10-5;
             }
-            if (y3 > y4){
-                verrequest = y3; 
-            } 
             else {
-                verrequest = -y4;
-            }
-            
-            // perform stepping between boundries
-            if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){horrequest=-0.5;}
-            else if(horrequest >  grenshoog){horrequest=1; } else if(horrequest>= grenslaag and horrequest<grenshoog){horrequest=0.5;} 
-            else {horrequest=0;}
-            
-            if(verrequest < -grenshoog){verrequest=-1;} else if(verrequest>=-grenshoog and verrequest<-grenslaag){verrequest=-0.5;}
-            else if(verrequest >  grenshoog){verrequest=1; } else if(verrequest>= grenslaag and verrequest<grenshoog){verrequest=0.5;} 
-            else {verrequest=0;}
-            
-            horrequest=horrequest*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
-            verrequest=verrequest*maxspeed;
+                sample_filter();
+                
+                // determine if forward or backward signal is stronger
+                if (y1 > y2){
+                    horrequest = y1; 
+                } 
+                else {
+                    horrequest = -y2;
+                }
+                if (y3 > y4){
+                    verrequest = y3; 
+                } 
+                else {
+                    verrequest = -y4;
+                }
+                
+                // perform stepping between boundries
+                if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){horrequest=-0.5;}
+                else if(horrequest >  grenshoog){horrequest=1; } else if(horrequest>= grenslaag and horrequest<grenshoog){horrequest=0.5;} 
+                else {horrequest=0;}
+                
+                if(verrequest < -grenshoog){verrequest=-1;} else if(verrequest>=-grenshoog and verrequest<-grenslaag){verrequest=-0.5;}
+                else if(verrequest >  grenshoog){verrequest=1; } else if(verrequest>= grenslaag and verrequest<grenshoog){verrequest=0.5;} 
+                else {verrequest=0;}
             
             
-            // ***************       
-            // calculate required rotational velocity from the requested horizontal velocity
-            // first get the current position from the motor encoders and make them start at 45 degree. 
-            leftAngle=(leftQei.getPulses()/round)*360+45; 
-            rightAngle=(rightQei.getPulses()/round)*360+45;
-            
-            // trigonometry to get xy position from angles (cm)
-            currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
-            currentY = tan(leftAngle*M_PI/180)*currentX;
-            
-            // restrict motion if edges are touched
-            if (edgeleft==0){
-                if (horrequest < 0){horrequest=0; pc.printf("hit left edge \n\r");}
-            }
-            if (edgeright==0){
-                if (horrequest > 0){horrequest=0; pc.printf("hit right edge \n\r");}
-            }
-            if (edgetop==0){
-                if (verrequest > 0){verrequest=0; pc.printf("hit top edge \n\r");}
-            }
-            if (edgebottom==0){
-                if (verrequest < 0){verrequest=0; pc.printf("hit bottom edge \n\r");}
+                horrequest=horrequest*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
+                verrequest=verrequest*maxspeed;
+                
+                
+                // ***************       
+                // calculate required rotational velocity from the requested horizontal velocity
+                // first get the current position from the motor encoders and make them start at 45 degree. 
+                leftAngle=(leftQei.getPulses()/round)*360+45; 
+                rightAngle=(rightQei.getPulses()/round)*360+45;
+                
+                // trigonometry to get xy position from angles (cm)
+                currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
+                currentY = tan(leftAngle*M_PI/180)*currentX;
+                
+                // restrict motion if edges are touched
+                if (edgeleft==0){
+                    if (horrequest < 0){horrequest=0; pc.printf("hit left edge \n\r");}
+                }
+                if (edgeright==0){
+                    if (horrequest > 0){horrequest=0; pc.printf("hit right edge \n\r");}
+                }
+                if (edgetop==0){
+                    if (verrequest > 0){verrequest=0; pc.printf("hit top edge \n\r");}
+                }
+                if (edgebottom==0){
+                    if (verrequest < 0){verrequest=0; pc.printf("hit bottom edge \n\r");}
+                }
+                
+                // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
+                toX=currentX+horrequest*RATE; 
+                toY=currentY+verrequest*RATE; // should be vertical request*RATE
+                
+                // trigonometry to get angles from xy new position (degree)
+                toLeftAngle = atan(toY/toX)*180/M_PI;
+                toRightAngle = atan(toY/(l-toX))*180/M_PI;
+                
+                // restrict motion if angles out-of-bound
+                if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
+                if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
+                if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
+                if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
+                
+                // restrict motion if position is out of reach of the arms
+                //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 neccesairy velocities to make these angles happen (degree/sec)
+                leftRequest=(toLeftAngle-leftAngle)/RATE;
+                rightRequest=(toRightAngle-rightAngle)/RATE;
             }
-            
-            // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
-            toX=currentX+horrequest*RATE; 
-            toY=currentY+verrequest*RATE; // should be vertical request*RATE
-            
-            // trigonometry to get angles from xy new position (degree)
-            toLeftAngle = atan(toY/toX)*180/M_PI;
-            toRightAngle = atan(toY/(l-toX))*180/M_PI;
-            
-            // restrict motion if angles out-of-bound
-            if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
-            if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
-            if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
-            if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
-            
-            // restrict motion if position is out of reach of the arms
-            //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 neccesairy velocities to make these angles happen (degree/sec)
-            leftRequest=(toLeftAngle-leftAngle)/RATE;
-            rightRequest=(toRightAngle-rightAngle)/RATE;
-            
             // set the setpoint to the pid controller
             leftController.setSetPoint(leftRequest);
             rightController.setSetPoint(rightRequest);