File content as of revision 8:b0971033dc41:

//****************************************************************************/
// Includes
#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"

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;}
    else {initposition_go=true;}
    pc.printf("initposition: %d",initposition_go);
}

void systemStart(){
    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();
    
    outofboundsled=1;
    edgeleft.mode(PullUp);
    edgeright.mode(PullUp);
    edgetop.mode(PullUp);
    edgebottom.mode(PullUp);
    
    // interrupts
    motorControlTicker.attach(&setGoFlag, RATE);
    cali_button.rise(&calibratego);
    initpositionButton.rise(&initpositiongo);
    startButton.rise(&systemStart);
    stopButton.rise(&systemStop);
    
    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){
                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();      
                // determine if forward or backward signal is stronger
                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;}
            
            
                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;
                
                // 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){pc.printf("hit left edge \n\r");
                    if (horrequest < 0){horrequest=0; }
                }
                if (edgeright==0){pc.printf("hit right edge \n\r");
                    if (horrequest > 0){horrequest=0; }
                }
                if (edgetop==0){pc.printf("hit top edge \n\r");
                    if (verrequest > 0){verrequest=0; }
                }
                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)
                toX=currentX+horrequest*RATE; 
                toY=currentY+verrequest*RATE; // should be vertical request*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 = 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)
            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 = 1;
                leftMotor = -leftPwmDuty;
            }
            else {
                leftDirection = 0;
                leftMotor = leftPwmDuty;
            }
            
            // right
            rightPwmDuty = rightController.compute();
            if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
                rightDirection = 1;
                rightMotor = -rightPwmDuty;
            }
            else {
                rightDirection = 0;
                rightMotor = rightPwmDuty;
            }
            
            // ***************
            // User feedback
            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;
        }
        if(systemOn==false)
        {
            leftMotor=0;
            rightMotor=0;
        }
    }
    
}