Diff: main.cpp

Revision:

11:c5042e19a096

Parent:

10:819fb5288aa0

--- a/main.cpp	Tue Oct 27 17:18:17 2015 +0000
+++ b/main.cpp	Mon Nov 02 11:12:46 2015 +0000
@@ -1,6 +1,9 @@
 //****************************************************************************/
 // Includes
 #include <string>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sstream>
 #include "TextLCD.h"
 #include "mbed.h"
 #include "PID.h"
@@ -9,7 +12,15 @@
 #include "biquadFilter.h"
 #include "inits.h" // all globals, pin and variable initialization 
 #include "EMG.h"
+string intToString(int i)
+{
+    std::stringstream ss;
+    std::string s;
+    ss << i;
+    s = ss.str();
 
+    return s;
+}
 void setLcdFlag(){
     lcdGoFlag=true;   
     
@@ -30,6 +41,19 @@
     }
 }
 
+void showFinishScreen(){
+    lcd.cls();
+    lcd.printf("FINISH!\n\rSCORE: %d",playTime, hitCount, playTime+5*hitCount);
+    pc.printf("enter name (3 characters): ");
+    for (int i=0; i<=3; i++){
+        char input= pc.getc();
+        pc.putc(input);
+    }
+    lcd.cls();
+    lcd.printf("press button to\n\rstart again");
+    
+}
+
 void systemStart(){
     //if (calibrated==false){
     //    calibrate_go=true;
@@ -40,6 +64,7 @@
         lcd.printf("stopped :(\n\r");
         pc.printf("stopped :(\n\r");
         statusled=1;
+        playTimer.reset();
     }
     else {
         systemOn=true;
@@ -80,8 +105,47 @@
     startButton.rise(&systemStart);
     stopButton.rise(&systemStop);
     
-    playTimer.start();
+    
+    
+    bool endchar=false;
+    string input;
+    float leftOffset;
+    float rightOffset;
+    pc.printf("read angle from left arm (end with d): ");
+    while(endchar==false){
+        char input= pc.getc();
+        
+        if(input=='d'){endchar=true;}
+        else {text+=input;}
+        pc.putc(input);
+    }
+    char chartext[1024];
+    strcpy(chartext, text.c_str());
+    
+    leftOffset=180-atof(chartext);
+    
     
+    pc.printf("\n\rread angle from right arm (end with d): ");
+    text="";
+    endchar=false;
+    while(endchar==false){
+        char input= pc.getc();
+        
+        if(input=='d'){endchar=true;}
+        else {text+=input;}
+        pc.putc(input);
+    }
+
+    strcpy(chartext, text.c_str());
+    
+    rightOffset=atof(chartext);
+    pc.printf("\n\rcontrol by pc? (y/n): ");
+    char userinput=pc.getc();
+    if(userinput == 'y'){controlbypc=true;}
+    else {controlbypc=false;}
+    
+    pc.printf("\n\rvalues set, leftangle:%f, rightangle:%f\n\r",leftOffset,rightOffset);
+        
     while (true){
         if (calibrate_go==true){
             lcd.cls();
@@ -112,9 +176,7 @@
                     else if(input=='k'){rightRequest=0;}
                     else if(input=='m'){rightRequest=-0.5;}
                     pc.putc(input);
-                    
-                    leftRequest=leftRequest*maxspeed;
-                    rightRequest=rightRequest*maxspeed;
+
                 }
 
             }
@@ -122,29 +184,29 @@
                 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;}
+                        if(input=='8'){verrequest=1;}
+                        else if(input=='2'){verrequest=-1;}
+                        else if(input=='4'){horrequest=-1;}
+                        else if(input=='6'){horrequest=1;}
                         else {horrequest=0; verrequest=0;}
                         pc.putc(input);
                     }
                     
                     
                 }
-                else {
+                else { // control by emg
                     sample_filter();    
-                    if (y1 > y2){
-                        horrequest = y1; 
+                    if (y3 > y2){
+                        horrequest = y3; 
                     } 
                     else {
                         horrequest = -y2;
                     }
-                    if (y3 > y4){
-                        verrequest = y3; 
+                    if (y4 > y1){
+                        verrequest = y4; 
                     } 
                     else {
-                        verrequest = -y4;
+                        verrequest = -y1;
                     }
                      // perform stepping between boundries
                     if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){horrequest=-0.5;}
@@ -156,29 +218,23 @@
                     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
                 // 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+leftOffset; 
+                rightAngle=-(rightQei.getPulses()/round)*360+rightOffset;
                 
                 // 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 or edgeFinish==0){
+                if (edgeleft==0 or edgeStart==0){
                     if (horrequest < 0){horrequest=0; }
                 }
-                if (edgeright==0 or edgeStart==0){
+                if (edgeright==0 or edgeFinish==0){
                     if (horrequest > 0){horrequest=0; }
                 }
                 if (edgetop==0){
@@ -188,9 +244,34 @@
                     if (verrequest < 0){verrequest=0; }
                 }
                 
+                if(edgebottom==0 or edgetop==0 or edgeleft==0 or edgeright==0) {
+                    musicHit=1;
+                    if (playTimer.read() > hitTime+3){
+                        hitTime=playTimer.read();
+                        hitCount+=1;
+                    }
+                }
+                else {musicHit=0;}
+                
+                if(edgeStart==0){
+                    playTimer.reset();
+                    playTimer.start();
+                    hitCount=0;
+                    started=true;
+                }
+                
+                if(edgeFinish==0 and started==true){
+                    started=false;
+                    playTimer.stop();
+                    systemOn=false;
+                    showFinishScreen(); 
+                    hitCount=0;  
+                }
+                
+                
                 // 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
+                toX=currentX+horrequest*maxspeed*RATE; 
+                toY=currentY+verrequest*maxspeed*RATE;
                 
                 // trigonometry to get angles from xy new position (degree)
                 toLeftAngle = atan(toY/toX)*180/M_PI;
@@ -198,7 +279,7 @@
                 
                 // 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 (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");}
                 
@@ -215,7 +296,7 @@
                 rightRequest=(toRightAngle-rightAngle)/RATE;
             }
             //pc.printf("leftrequest: %f, rightrequest: %f \n\r",leftRequest, rightRequest);
-            
+                
             // set the setpoint to the pid controller
             leftController.setSetPoint(leftRequest);
             rightController.setSetPoint(rightRequest);
@@ -262,12 +343,12 @@
             
             // ***************
             // User feedback
-            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.set(0, y1);
+            scope.set(1, y2);
+            scope.set(2, y3);
+            scope.set(3, y4);
+            scope.set(4, currentX);
+            scope.set(5, currentY);
             scope.send();
                     
             goFlag=false;
@@ -284,26 +365,36 @@
             if(horrequest==-1){text+="<<";}
             if(horrequest==-0.5){text+=" <";}
             if(horrequest>=0){text+="  ";}
-            text+="0";
+            text+="-";
             if(horrequest<=0){text+="  ";}
             if(horrequest==0.5){text+="> ";}
             if(horrequest==1){text+=">>";}
-        
-        
-            text+=" T:XXX";
+            
+            playTime=(int)playTimer.read();
+
+            if(playTime < 10){text+=" T:  ";}
+            else if(playTime < 100){text+=" T: ";}
+            else if(playTime < 1000){text+=" T:";}
+            else {text+=" T:MAX";}
+            
+            text+=intToString(playTime);
             text+="ver: ";
             
             if(verrequest==-1){text+="vv";}
             else if(verrequest==-0.5){text+=" v";}
             else {text+="  ";}
-            text+="0";
+            text+="|";
             
-            if(verrequest==1){text+="TT";}
-            else if(verrequest==0.5){text+="T ";}
+            if(verrequest==1){text+="^^";}
+            else if(verrequest==0.5){text+="^ ";}
             if(verrequest<=0){text+="  ";}
             
-            text+=" HIT:X";
+            if(hitCount < 10){text+=" H:  ";}
+            else if(hitCount < 100){text+=" H: ";}
+            else if(hitCount < 1000){text+=" H:";}
+            else {text+=" H:MAX";}
             
+            text+=intToString(hitCount);
            
             
             char chartext[1024];