Diff: main.cpp

Revision:

8:b0971033dc41

Parent:

7:572e7f3e184a

Child:

9:3e19a344c025

--- a/main.cpp	Tue Oct 20 08:21:50 2015 +0000
+++ b/main.cpp	Mon Oct 26 11:15:31 2015 +0000
@@ -1,5 +1,6 @@
 //****************************************************************************/
 // Includes
+#include "TextLCD.h"
 #include "mbed.h"
 #include "PID.h"
 #include "QEI.h"
@@ -18,19 +19,32 @@
 void initpositiongo(){
     if(initposition_go){initposition_go=false;}
     else {initposition_go=true;}
-    
+    pc.printf("initposition: %d",initposition_go);
 }
 
 void systemStart(){
-    systemOn=true;
+    if (systemOn==true){
+        systemOn=false;
+        lcd.cls(); 
+        lcd.printf("stopped :(");
+    }
+    else {
+        systemOn=true;
+        lcd.cls(); 
+        lcd.printf("GO GO GO!!");
+    }
 }
 void systemStop(){
     systemOn=false;
     pc.printf("system stopped\n\r");
+    lcd.cls(); 
+    lcd.printf("stopped :(");
 }
 
+
 int main() {
-    
+    pc.printf("system start");
+    lcd.printf("A-MAZE-ING\nMARIO"); // print a test message to the display.
     // initialize (defined in inits.h)
     initMotors();
     initPIDs();
@@ -38,6 +52,8 @@
     outofboundsled=1;
     edgeleft.mode(PullUp);
     edgeright.mode(PullUp);
+    edgetop.mode(PullUp);
+    edgebottom.mode(PullUp);
     
     // interrupts
     motorControlTicker.attach(&setGoFlag, RATE);
@@ -48,8 +64,11 @@
     
     while (true){
         if (calibrate_go==true){
-            calibrate();         
-            
+            lcd.cls();
+            lcd.printf("calibrating...");
+            calibrate();    
+            lcd.cls();     
+            lcd.printf("ready to start!\nPress the Button");
             
         }
         if (goFlag==true && systemOn==true){
@@ -63,12 +82,12 @@
             // **************
             // get emg signal
             if(initposition_go){
-                leftRequest=pot1.read()*20-10;
-                rightRequest=pot2.read()*20-10;
+                leftRequest=pot1.read()*10-5;
+                rightRequest=pot2.read()*10-5;
+                //pc.printf("initposition_go, left:%f, leftAngle:%f, right:%f, rightAngle: %f",leftRequest, leftAngle, rightRequest, rightAngle);
             }
             else {
-                sample_filter();
-                
+                sample_filter();      
                 // determine if forward or backward signal is stronger
                 if (y1 > y2){
                     horrequest = y1; 
@@ -83,6 +102,7 @@
                     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;} 
@@ -100,25 +120,25 @@
                 // ***************       
                 // 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;
+                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 (edgeleft==0){pc.printf("hit left edge \n\r");
+                    if (horrequest < 0){horrequest=0; }
                 }
-                if (edgeright==0){
-                    if (horrequest > 0){horrequest=0; pc.printf("hit right edge \n\r");}
+                if (edgeright==0){pc.printf("hit right edge \n\r");
+                    if (horrequest > 0){horrequest=0; }
                 }
-                if (edgetop==0){
-                    if (verrequest > 0){verrequest=0; pc.printf("hit top edge \n\r");}
+                if (edgetop==0){pc.printf("hit top edge \n\r");
+                    if (verrequest > 0){verrequest=0; }
                 }
-                if (edgebottom==0){
-                    if (verrequest < 0){verrequest=0; pc.printf("hit bottom edge \n\r");}
+                if (edgebottom==0){pc.printf("hit bottom edge \n\r");
+                    if (verrequest < 0){verrequest=0; }
                 }
                 
                 // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
@@ -147,7 +167,7 @@
                 leftRequest=(toLeftAngle-leftAngle)/RATE;
                 rightRequest=(toRightAngle-rightAngle)/RATE;
             }
-            pc.printf("leftrequest: %f, rightrequest: %f \n\r",leftRequest, rightRequest);
+            //pc.printf("leftrequest: %f, rightrequest: %f \n\r",leftRequest, rightRequest);
             
             // set the setpoint to the pid controller
             leftController.setSetPoint(leftRequest);
@@ -174,11 +194,11 @@
             
             leftPwmDuty = leftController.compute();
             if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
-                leftDirection = 0;
+                leftDirection = 1;
                 leftMotor = -leftPwmDuty;
             }
             else {
-                leftDirection = 1;
+                leftDirection = 0;
                 leftMotor = leftPwmDuty;
             }
             
@@ -195,11 +215,12 @@
             
             // ***************
             // User feedback
-            scope.set(0, horrequest);
-            scope.set(1, leftPwmDuty);
-            scope.set(2, leftVelocity);
-            scope.set(3, currentX);
-            scope.set(4, currentY);
+            scope.set(0, y1);
+            scope.set(1, y2);
+            scope.set(2, y3);
+            scope.set(3, y4);
+            scope.set(4, horrequest);
+            scope.set(5, verrequest);
             scope.send();
                     
             goFlag=false;