Diff: main.cpp

Revision:

10:819fb5288aa0

Parent:

9:3e19a344c025

Child:

11:c5042e19a096

diff -r 3e19a344c025 -r 819fb5288aa0 main.cpp
--- a/main.cpp	Mon Oct 26 14:26:13 2015 +0000
+++ b/main.cpp	Tue Oct 27 17:18:17 2015 +0000
@@ -22,49 +22,61 @@
     goFlag=true;
 }
 void initpositiongo(){
-    if(initposition_go){initposition_go=false;}
-    else {initposition_go=true;}
-    pc.printf("initposition: %d \n\r",initposition_go);
+    if(initposition_go){initposition_go=false;
+        pc.printf("ended init process\n\r");}
+    else {initposition_go=true;
+        pc.printf("started init process\n\rleft arm: QAZ right arm: OKM");
+       
+    }
 }
 
 void systemStart(){
+    //if (calibrated==false){
+    //    calibrate_go=true;
+   // }
     if (systemOn==true){
         systemOn=false;
         lcd.cls(); 
-        lcd.printf("stopped :(");
+        lcd.printf("stopped :(\n\r");
+        pc.printf("stopped :(\n\r");
+        statusled=1;
     }
     else {
         systemOn=true;
         lcd.cls(); 
-        lcd.printf("GO GO GO!!");
+        lcd.printf("GO GO GO!!\n\r");
+        pc.printf("started\n\r");
+        statusled=0;
     }
 }
 void systemStop(){
     systemOn=false;
     pc.printf("system stopped\n\r");
     lcd.cls(); 
-    lcd.printf("stopped :(");
+    lcd.printf("stopped :(\n\r");
+    statusled=1;
 }
 
-
 int main() {
-    pc.printf("system start");
+    pc.printf("system start\n\r");
     lcd.printf("A-MAZE-ING\nMARIO"); // print a test message to the display.
     // initialize (defined in inits.h)
     initMotors();
     initPIDs();
     
-    outofboundsled=1;
     edgeleft.mode(PullUp);
     edgeright.mode(PullUp);
     edgetop.mode(PullUp);
     edgebottom.mode(PullUp);
+    edgeStart.mode(PullUp);
+    edgeFinish.mode(PullUp);
     
     // interrupts
     motorControlTicker.attach(&setGoFlag, RATE);
     lcdTicker.attach(&setLcdFlag,0.2);
     cali_button.rise(&calibratego);
     initpositionButton.rise(&initpositiongo);
+    //startButton.fall(&buttonpressTimer); could be used to calculate the button press time
     startButton.rise(&systemStart);
     stopButton.rise(&systemStop);
     
@@ -74,7 +86,7 @@
         if (calibrate_go==true){
             lcd.cls();
             lcd.printf("calibrating...");
-            calibrate();    
+            calibrate();
             lcd.cls();     
             lcd.printf("ready to start!\nPress the Button");
             
@@ -90,37 +102,63 @@
             // **************
             // get emg signal
             if(initposition_go){
-                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);
+                if (pc.readable()){
+                    char input= pc.getc();
+                    if(input=='q'){leftRequest=0.5;}
+                    else if(input=='a'){leftRequest=0;}
+                    else if(input=='z'){leftRequest=-0.5;}
+
+                    if(input=='o'){rightRequest=0.5;}
+                    else if(input=='k'){rightRequest=0;}
+                    else if(input=='m'){rightRequest=-0.5;}
+                    pc.putc(input);
+                    
+                    leftRequest=leftRequest*maxspeed;
+                    rightRequest=rightRequest*maxspeed;
+                }
+
             }
             else {
-                sample_filter();   
-                y2=0;   
-                // determine if forward or backward signal is stronger
-                if (y1 > y2){
-                    horrequest = y1; 
-                } 
+                if(controlbypc){
+                    if (pc.readable()){
+                        char input= pc.getc();
+                        if(input=='8'){verrequest=0.5;}
+                        else if(input=='2'){verrequest=-0.5;}
+                        else if(input=='4'){horrequest=-0.5;}
+                        else if(input=='6'){horrequest=0.5;}
+                        else {horrequest=0; verrequest=0;}
+                        pc.putc(input);
+                    }
+                    
+                    
+                }
                 else {
-                    horrequest = -y2;
-                }
-                if (y3 > y4){
-                    verrequest = y3; 
-                } 
-                else {
-                    verrequest = -y4;
+                    sample_filter();    
+                    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;}
+            
                 }
                 
                 
-                // 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;
@@ -129,18 +167,18 @@
                 // ***************       
                 // 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 (edgeleft==0 or edgeFinish==0){
                     if (horrequest < 0){horrequest=0; }
                 }
-                if (edgeright==0){
+                if (edgeright==0 or edgeStart==0){
                     if (horrequest > 0){horrequest=0; }
                 }
                 if (edgetop==0){
@@ -159,10 +197,10 @@
                 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");}
+                //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
@@ -185,13 +223,13 @@
             // **************
             // Velocity calculation
             // left, differentiate from encoders
-            leftPulses = leftQei.getPulses()/round*360; // (degree)
+            leftPulses = -(float)leftQei.getPulses()/round*360; // (degree)
             leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; // (degree/sec)
             leftPrevPulses = leftPulses;
             leftController.setProcessValue(leftVelocity); // set PID process value
             
             // right
-            rightPulses = rightQei.getPulses()/round*360; // (degree)
+            rightPulses = -(float)rightQei.getPulses()/round*360; // (degree)
             rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
             rightPrevPulses = rightPulses;
             rightController.setProcessValue(rightVelocity); // set PID process value
@@ -203,31 +241,31 @@
             
             leftPwmDuty = leftController.compute();
             if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
-                leftDirection = 1;
+                leftDirection = 0;
                 leftMotor = -leftPwmDuty;
             }
             else {
-                leftDirection = 0;
+                leftDirection = 1;
                 leftMotor = leftPwmDuty;
             }
             
             // right
             rightPwmDuty = rightController.compute();
             if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
-                rightDirection = 1;
+                rightDirection = 0;
                 rightMotor = -rightPwmDuty;
             }
             else {
-                rightDirection = 0;
+                rightDirection = 1;
                 rightMotor = rightPwmDuty;
             }
             
             // ***************
             // User feedback
-            scope.set(0, y1);
-            scope.set(1, y2);
-            scope.set(2, y3);
-            scope.set(3, y4);
+            scope.set(0, leftAngle);
+            scope.set(1, toLeftAngle);
+            scope.set(2, rightAngle);
+            scope.set(3, toRightAngle);
             scope.set(4, horrequest);
             scope.set(5, verrequest);
             scope.send();
@@ -239,7 +277,8 @@
             leftMotor=0;
             rightMotor=0;
         }
-        if(lcdGoFlag==true){
+        
+        if(lcdGoFlag==true && systemOn==true){
             text="";
             text+="hor: ";
             if(horrequest==-1){text+="<<";}
@@ -251,24 +290,24 @@
             if(horrequest==1){text+=">>";}
         
         
-            text+=" T:%d\n",playTimer.read();
+            text+=" T:XXX";
+            text+="ver: ";
             
-            if(verrequest==-1){text+="<<";}
-            if(verrequest==-0.5){text+=" <";}
-            if(verrequest>=0){text+="  ";}
+            if(verrequest==-1){text+="vv";}
+            else if(verrequest==-0.5){text+=" v";}
+            else {text+="  ";}
             text+="0";
+            
+            if(verrequest==1){text+="TT";}
+            else if(verrequest==0.5){text+="T ";}
             if(verrequest<=0){text+="  ";}
-            if(verrequest==0.5){text+="> ";}
-            if(verrequest==1){text+=">>";}
             
-            text+" HIT:%d",0;
+            text+=" HIT:X";
             
            
             
             char chartext[1024];
             strcpy(chartext, text.c_str());
-            
-            pc.printf(chartext);
              
             lcd.cls();
             lcd.printf(chartext);