Diff: main.cpp

Revision:

3:70f78cfc0f25

Parent:

1:dafb63606b66

Child:

4:680f775a3703

--- a/main.cpp	Tue Oct 13 18:54:38 2015 +0000
+++ b/main.cpp	Wed Oct 14 13:16:35 2015 +0000
@@ -36,12 +36,16 @@
     
     // interrupts
     motorControlTicker.attach(&setGoFlag, RATE);
-    cali_button.rise(&calibrate);
+    cali_button.rise(&calibratego);
     startButton.rise(&systemStart);
     stopButton.rise(&systemStop);
     
     while (true){
-        
+        if (calibrate_go==true){
+            calibrate();         
+            
+            
+        }
         if (goFlag==true && systemOn==true){
             /*********** Contents *************/
             // 1) get emg signal
@@ -53,18 +57,33 @@
             // **************
             // get emg signal
             sample_filter();
-                       
-            request_pos=y1;
-            request_neg=y2;
             
             // determine if forward or backward signal is stronger
-            if (request_pos > request_neg){
-                request = request_pos; 
+            if (y1 > y2){
+                horrequest = y1; 
+            } 
+            else {
+                horrequest = -y2;
+            }
+            if (y3 > y4){
+                verrequest = y3; 
             } 
             else {
-                request = -request_neg;
+                verrequest = -y4;
             }
-            request=request*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
+            
+            // perform stepping between boundries
+            if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){y1=-0.5;}
+            if(horrequest >  grenshoog){horrequest=1; } else if(horrequest>= grenslaag and horrequest<grenshoog){y1=0.5;} 
+            else {horrequest=0;}
+            
+            if(verrequest < -grenshoog){verrequest=-1;} else if(verrequest>=-grenshoog and verrequest<-grenslaag){y1=-0.5;}
+            if(verrequest >  grenshoog){verrequest=1; } else if(verrequest>= grenslaag and verrequest<grenshoog){y1=0.5;} 
+            else {verrequest=0;}
+            
+            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
@@ -78,15 +97,21 @@
             
             // restrict motion if edges are touched
             if (edgeleft==0){
-                if (request < 0){request=0; pc.printf("hit left edge \n\r");}
+                if (horrequest < 0){horrequest=0; pc.printf("hit left edge \n\r");}
             }
             if (edgeright==0){
-                if (request > 0){request=0; pc.printf("hit right edge \n\r");}
+                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+request*RATE; 
-            toY=currentY+0*RATE; // should be vertical request*RATE
+            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;
@@ -119,14 +144,12 @@
             // left, differentiate from encoders
             leftPulses = leftQei.getPulses()/round*360; // (degree)
             leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; // (degree/sec)
-            //leftVelocity = leftVelocityfilter.step(leftVelocity);
             leftPrevPulses = leftPulses;
             leftController.setProcessValue(leftVelocity); // set PID process value
             
             // right
             rightPulses = rightQei.getPulses()/round*360; // (degree)
             rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
-            //rightVelocity = rightVelocityfilter.step(rightVelocity);
             rightPrevPulses = rightPulses;
             rightController.setProcessValue(rightVelocity); // set PID process value
             
@@ -158,7 +181,7 @@
             
             // ***************
             // User feedback
-            scope.set(0, leftRequest);
+            scope.set(0, horrequest);
             scope.set(1, leftPwmDuty);
             scope.set(2, leftVelocity);
             scope.set(3, currentX);