File content as of revision 11:c5042e19a096:

//****************************************************************************/
// Includes
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <sstream>
#include "TextLCD.h"
#include "mbed.h"
#include "PID.h"
#include "QEI.h"
#include "HIDScope.h"
#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;   
    
}
void setGoFlag(){
    if (goFlag==true){
        //pc.printf("rate too high, error in setGoFlag \n\r");
        // flag is already set true: code too slow or frequency too high    
    }
    goFlag=true;
}
void initpositiongo(){
    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 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;
   // }
    if (systemOn==true){
        systemOn=false;
        lcd.cls(); 
        lcd.printf("stopped :(\n\r");
        pc.printf("stopped :(\n\r");
        statusled=1;
        playTimer.reset();
    }
    else {
        systemOn=true;
        lcd.cls(); 
        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 :(\n\r");
    statusled=1;
}

int main() {
    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();
    
    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);
    
    
    
    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();
            lcd.printf("calibrating...");
            calibrate();
            lcd.cls();     
            lcd.printf("ready to start!\nPress the Button");
            
        }
        if (goFlag==true && systemOn==true){
            /*********** Contents *************/
            // 1) get emg signal
            // 2) calculate desired angle velocities
            // 3) calculate current angle velocities
            // 4) pid controller output
            // 5) user output
                
            // **************
            // get emg signal
            if(initposition_go){
                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);

                }

            }
            else {
                if(controlbypc){
                    if (pc.readable()){
                        char input= pc.getc();
                        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 { // control by emg
                    sample_filter();    
                    if (y3 > y2){
                        horrequest = y3; 
                    } 
                    else {
                        horrequest = -y2;
                    }
                    if (y4 > y1){
                        verrequest = y4; 
                    } 
                    else {
                        verrequest = -y1;
                    }
                     // 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;}
            
                }
                   
                
                // ***************       
                // 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+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 edgeStart==0){
                    if (horrequest < 0){horrequest=0; }
                }
                if (edgeright==0 or edgeFinish==0){
                    if (horrequest > 0){horrequest=0; }
                }
                if (edgetop==0){
                    if (verrequest > 0){verrequest=0; }
                }
                if (edgebottom==0){
                    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*maxspeed*RATE; 
                toY=currentY+verrequest*maxspeed*RATE;
                
                // trigonometry to get angles from xy new position (degree)
                toLeftAngle = atan(toY/toX)*180/M_PI;
                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");}
                
                // 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
                    //return 0;
                //}
                //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
                    //return 0;
                //}
                
                // calculate the neccesairy velocities to make these angles happen (degree/sec)
                leftRequest=(toLeftAngle-leftAngle)/RATE;
                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);
            
            // **************
            // Velocity calculation
            // left, differentiate from encoders
            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 = -(float)rightQei.getPulses()/round*360; // (degree)
            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
            rightPrevPulses = rightPulses;
            rightController.setProcessValue(rightVelocity); // set PID process value
            
            
            // **************
            // PID control output
            // left
            
            leftPwmDuty = leftController.compute();
            if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
                leftDirection = 0;
                leftMotor = -leftPwmDuty;
            }
            else {
                leftDirection = 1;
                leftMotor = leftPwmDuty;
            }
            
            // right
            rightPwmDuty = rightController.compute();
            if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
                rightDirection = 0;
                rightMotor = -rightPwmDuty;
            }
            else {
                rightDirection = 1;
                rightMotor = rightPwmDuty;
            }
            
            // ***************
            // User feedback
            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;
        }
        if(systemOn==false)
        {
            leftMotor=0;
            rightMotor=0;
        }
        
        if(lcdGoFlag==true && systemOn==true){
            text="";
            text+="hor: ";
            if(horrequest==-1){text+="<<";}
            if(horrequest==-0.5){text+=" <";}
            if(horrequest>=0){text+="  ";}
            text+="-";
            if(horrequest<=0){text+="  ";}
            if(horrequest==0.5){text+="> ";}
            if(horrequest==1){text+=">>";}
            
            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+="|";
            
            if(verrequest==1){text+="^^";}
            else if(verrequest==0.5){text+="^ ";}
            if(verrequest<=0){text+="  ";}
            
            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];
            strcpy(chartext, text.c_str());
             
            lcd.cls();
            lcd.printf(chartext);
            
            lcdGoFlag=false;
            
        }
    }
    
}