main.cpp

00001 //****************************************************************************/
00002 // Includes
00003 #include <string>
00004 #include <stdio.h>
00005 #include <stdlib.h>
00006 #include <sstream>
00007 #include "TextLCD.h"
00008 #include "mbed.h"
00009 #include "PID.h"
00010 #include "QEI.h"
00011 #include "HIDScope.h"
00012 #include "biquadFilter.h"
00013 #include "inits.h" // all globals, pin and variable initialization 
00014 #include "EMG.h"
00015 string intToString(int i)
00016 {
00017     std::stringstream ss;
00018     std::string s;
00019     ss << i;
00020     s = ss.str();
00021 
00022     return s;
00023 }
00024 void setLcdFlag(){
00025     lcdGoFlag=true;   
00026     
00027 }
00028 void setGoFlag(){
00029     if (goFlag==true){
00030         //pc.printf("rate too high, error in setGoFlag \n\r");
00031         // flag is already set true: code too slow or frequency too high    
00032     }
00033     goFlag=true;
00034 }
00035 void initpositiongo(){
00036     if(initposition_go){initposition_go=false;
00037         pc.printf("ended init process\n\r");}
00038     else {initposition_go=true;
00039         pc.printf("started init process\n\rleft arm: QAZ right arm: OKM");
00040        
00041     }
00042 }
00043 
00044 void showFinishScreen(){
00045     lcd.cls();
00046     lcd.printf("FINISH!\n\rSCORE: %d",playTime, hitCount, playTime+5*hitCount);
00047     pc.printf("enter name (3 characters): ");
00048     for (int i=0; i<=3; i++){
00049         char input= pc.getc();
00050         pc.putc(input);
00051     }
00052     lcd.cls();
00053     lcd.printf("press button to\n\rstart again");
00054     
00055 }
00056 
00057 void systemStart(){
00058     //if (calibrated==false){
00059     //    calibrate_go=true;
00060    // }
00061     if (systemOn==true){
00062         systemOn=false;
00063         lcd.cls(); 
00064         lcd.printf("stopped :(\n\r");
00065         pc.printf("stopped :(\n\r");
00066         statusled=1;
00067         playTimer.reset();
00068     }
00069     else {
00070         systemOn=true;
00071         lcd.cls(); 
00072         lcd.printf("GO GO GO!!\n\r");
00073         pc.printf("started\n\r");
00074         statusled=0;
00075     }
00076 }
00077 void systemStop(){
00078     systemOn=false;
00079     pc.printf("system stopped\n\r");
00080     lcd.cls(); 
00081     lcd.printf("stopped :(\n\r");
00082     statusled=1;
00083 }
00084 
00085 int main() {
00086     pc.printf("system start\n\r");
00087     lcd.printf("A-MAZE-ING\nMARIO"); // print a test message to the display.
00088     // initialize (defined in inits.h)
00089     initMotors();
00090     initPIDs();
00091     
00092     edgeleft.mode(PullUp);
00093     edgeright.mode(PullUp);
00094     edgetop.mode(PullUp);
00095     edgebottom.mode(PullUp);
00096     edgeStart.mode(PullUp);
00097     edgeFinish.mode(PullUp);
00098     
00099     // interrupts
00100     motorControlTicker.attach(&setGoFlag, RATE);
00101     lcdTicker.attach(&setLcdFlag,0.2);
00102     cali_button.rise(&calibratego);
00103     initpositionButton.rise(&initpositiongo);
00104     //startButton.fall(&buttonpressTimer); could be used to calculate the button press time
00105     startButton.rise(&systemStart);
00106     stopButton.rise(&systemStop);
00107     
00108     
00109     
00110     bool endchar=false;
00111     string input;
00112     float leftOffset;
00113     float rightOffset;
00114     pc.printf("read angle from left arm (end with d): ");
00115     while(endchar==false){
00116         char input= pc.getc();
00117         
00118         if(input=='d'){endchar=true;}
00119         else {text+=input;}
00120         pc.putc(input);
00121     }
00122     char chartext[1024];
00123     strcpy(chartext, text.c_str());
00124     
00125     leftOffset=180-atof(chartext);
00126     
00127     
00128     pc.printf("\n\rread angle from right arm (end with d): ");
00129     text="";
00130     endchar=false;
00131     while(endchar==false){
00132         char input= pc.getc();
00133         
00134         if(input=='d'){endchar=true;}
00135         else {text+=input;}
00136         pc.putc(input);
00137     }
00138 
00139     strcpy(chartext, text.c_str());
00140     
00141     rightOffset=atof(chartext);
00142     pc.printf("\n\rcontrol by pc? (y/n): ");
00143     char userinput=pc.getc();
00144     if(userinput == 'y'){controlbypc=true;}
00145     else {controlbypc=false;}
00146     
00147     pc.printf("\n\rvalues set, leftangle:%f, rightangle:%f\n\r",leftOffset,rightOffset);
00148         
00149     while (true){
00150         if (calibrate_go==true){
00151             lcd.cls();
00152             lcd.printf("calibrating...");
00153             calibrate();
00154             lcd.cls();     
00155             lcd.printf("ready to start!\nPress the Button");
00156             
00157         }
00158         if (goFlag==true && systemOn==true){
00159             /*********** Contents *************/
00160             // 1) get emg signal
00161             // 2) calculate desired angle velocities
00162             // 3) calculate current angle velocities
00163             // 4) pid controller output
00164             // 5) user output
00165                 
00166             // **************
00167             // get emg signal
00168             if(initposition_go){
00169                 if (pc.readable()){
00170                     char input= pc.getc();
00171                     if(input=='q'){leftRequest=0.5;}
00172                     else if(input=='a'){leftRequest=0;}
00173                     else if(input=='z'){leftRequest=-0.5;}
00174 
00175                     if(input=='o'){rightRequest=0.5;}
00176                     else if(input=='k'){rightRequest=0;}
00177                     else if(input=='m'){rightRequest=-0.5;}
00178                     pc.putc(input);
00179 
00180                 }
00181 
00182             }
00183             else {
00184                 if(controlbypc){
00185                     if (pc.readable()){
00186                         char input= pc.getc();
00187                         if(input=='8'){verrequest=1;}
00188                         else if(input=='2'){verrequest=-1;}
00189                         else if(input=='4'){horrequest=-1;}
00190                         else if(input=='6'){horrequest=1;}
00191                         else {horrequest=0; verrequest=0;}
00192                         pc.putc(input);
00193                     }
00194                     
00195                     
00196                 }
00197                 else { // control by emg
00198                     sample_filter();    
00199                     if (y3 > y2){
00200                         horrequest = y3; 
00201                     } 
00202                     else {
00203                         horrequest = -y2;
00204                     }
00205                     if (y4 > y1){
00206                         verrequest = y4; 
00207                     } 
00208                     else {
00209                         verrequest = -y1;
00210                     }
00211                      // perform stepping between boundries
00212                     if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){horrequest=-0.5;}
00213                     else if(horrequest >  grenshoog){horrequest=1; } else if(horrequest>= grenslaag and horrequest<grenshoog){horrequest=0.5;} 
00214                     else {horrequest=0;}
00215                     
00216                     if(verrequest < -grenshoog){verrequest=-1;} else if(verrequest>=-grenshoog and verrequest<-grenslaag){verrequest=-0.5;}
00217                     else if(verrequest >  grenshoog){verrequest=1; } else if(verrequest>= grenslaag and verrequest<grenshoog){verrequest=0.5;} 
00218                     else {verrequest=0;}
00219             
00220                 }
00221                    
00222                 
00223                 // ***************       
00224                 // calculate required rotational velocity from the requested horizontal velocity
00225                 // first get the current position from the motor encoders and make them start at 45 degree. 
00226                 leftAngle=-(leftQei.getPulses()/round)*360+leftOffset; 
00227                 rightAngle=-(rightQei.getPulses()/round)*360+rightOffset;
00228                 
00229                 // trigonometry to get xy position from angles (cm)
00230                 currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
00231                 currentY = tan(leftAngle*M_PI/180)*currentX;
00232                 
00233                 // restrict motion if edges are touched
00234                 if (edgeleft==0 or edgeStart==0){
00235                     if (horrequest < 0){horrequest=0; }
00236                 }
00237                 if (edgeright==0 or edgeFinish==0){
00238                     if (horrequest > 0){horrequest=0; }
00239                 }
00240                 if (edgetop==0){
00241                     if (verrequest > 0){verrequest=0; }
00242                 }
00243                 if (edgebottom==0){
00244                     if (verrequest < 0){verrequest=0; }
00245                 }
00246                 
00247                 if(edgebottom==0 or edgetop==0 or edgeleft==0 or edgeright==0) {
00248                     musicHit=1;
00249                     if (playTimer.read() > hitTime+3){
00250                         hitTime=playTimer.read();
00251                         hitCount+=1;
00252                     }
00253                 }
00254                 else {musicHit=0;}
00255                 
00256                 if(edgeStart==0){
00257                     playTimer.reset();
00258                     playTimer.start();
00259                     hitCount=0;
00260                     started=true;
00261                 }
00262                 
00263                 if(edgeFinish==0 and started==true){
00264                     started=false;
00265                     playTimer.stop();
00266                     systemOn=false;
00267                     showFinishScreen(); 
00268                     hitCount=0;  
00269                 }
00270                 
00271                 
00272                 // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
00273                 toX=currentX+horrequest*maxspeed*RATE; 
00274                 toY=currentY+verrequest*maxspeed*RATE;
00275                 
00276                 // trigonometry to get angles from xy new position (degree)
00277                 toLeftAngle = atan(toY/toX)*180/M_PI;
00278                 toRightAngle = atan(toY/(l-toX))*180/M_PI;
00279                 
00280                 // restrict motion if angles out-of-bound
00281                 //if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
00282                 //if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
00283                 //if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
00284                 //if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
00285                 
00286                 // restrict motion if position is out of reach of the arms
00287                 //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
00288                     //return 0;
00289                 //}
00290                 //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
00291                     //return 0;
00292                 //}
00293                 
00294                 // calculate the neccesairy velocities to make these angles happen (degree/sec)
00295                 leftRequest=(toLeftAngle-leftAngle)/RATE;
00296                 rightRequest=(toRightAngle-rightAngle)/RATE;
00297             }
00298             //pc.printf("leftrequest: %f, rightrequest: %f \n\r",leftRequest, rightRequest);
00299                 
00300             // set the setpoint to the pid controller
00301             leftController.setSetPoint(leftRequest);
00302             rightController.setSetPoint(rightRequest);
00303             
00304             // **************
00305             // Velocity calculation
00306             // left, differentiate from encoders
00307             leftPulses = -(float)leftQei.getPulses()/round*360; // (degree)
00308             leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; // (degree/sec)
00309             leftPrevPulses = leftPulses;
00310             leftController.setProcessValue(leftVelocity); // set PID process value
00311             
00312             // right
00313             rightPulses = -(float)rightQei.getPulses()/round*360; // (degree)
00314             rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
00315             rightPrevPulses = rightPulses;
00316             rightController.setProcessValue(rightVelocity); // set PID process value
00317             
00318             
00319             // **************
00320             // PID control output
00321             // left
00322             
00323             leftPwmDuty = leftController.compute();
00324             if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
00325                 leftDirection = 0;
00326                 leftMotor = -leftPwmDuty;
00327             }
00328             else {
00329                 leftDirection = 1;
00330                 leftMotor = leftPwmDuty;
00331             }
00332             
00333             // right
00334             rightPwmDuty = rightController.compute();
00335             if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
00336                 rightDirection = 0;
00337                 rightMotor = -rightPwmDuty;
00338             }
00339             else {
00340                 rightDirection = 1;
00341                 rightMotor = rightPwmDuty;
00342             }
00343             
00344             // ***************
00345             // User feedback
00346             scope.set(0, y1);
00347             scope.set(1, y2);
00348             scope.set(2, y3);
00349             scope.set(3, y4);
00350             scope.set(4, currentX);
00351             scope.set(5, currentY);
00352             scope.send();
00353                     
00354             goFlag=false;
00355         }
00356         if(systemOn==false)
00357         {
00358             leftMotor=0;
00359             rightMotor=0;
00360         }
00361         
00362         if(lcdGoFlag==true && systemOn==true){
00363             text="";
00364             text+="hor: ";
00365             if(horrequest==-1){text+="<<";}
00366             if(horrequest==-0.5){text+=" <";}
00367             if(horrequest>=0){text+="  ";}
00368             text+="-";
00369             if(horrequest<=0){text+="  ";}
00370             if(horrequest==0.5){text+="> ";}
00371             if(horrequest==1){text+=">>";}
00372             
00373             playTime=(int)playTimer.read();
00374 
00375             if(playTime < 10){text+=" T:  ";}
00376             else if(playTime < 100){text+=" T: ";}
00377             else if(playTime < 1000){text+=" T:";}
00378             else {text+=" T:MAX";}
00379             
00380             text+=intToString(playTime);
00381             text+="ver: ";
00382             
00383             if(verrequest==-1){text+="vv";}
00384             else if(verrequest==-0.5){text+=" v";}
00385             else {text+="  ";}
00386             text+="|";
00387             
00388             if(verrequest==1){text+="^^";}
00389             else if(verrequest==0.5){text+="^ ";}
00390             if(verrequest<=0){text+="  ";}
00391             
00392             if(hitCount < 10){text+=" H:  ";}
00393             else if(hitCount < 100){text+=" H: ";}
00394             else if(hitCount < 1000){text+=" H:";}
00395             else {text+=" H:MAX";}
00396             
00397             text+=intToString(hitCount);
00398            
00399             
00400             char chartext[1024];
00401             strcpy(chartext, text.c_str());
00402              
00403             lcd.cls();
00404             lcd.printf(chartext);
00405             
00406             lcdGoFlag=false;
00407             
00408         }
00409     }
00410     
00411 }