File content as of revision 5:e52055ff2bfe:

//****************************************************************************/
// Includes
#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 systemStart(){
    systemOn=true;
}
void systemStop(){
    systemOn=false;
    pc.printf("system stopped\n\r");
}

int main() {
    
    // initialize (defined in inits.h)
    initMotors();
    initPIDs();
    
    outofboundsled=1;
    edgeleft.mode(PullUp);
    edgeright.mode(PullUp);
    
    // interrupts
    motorControlTicker.attach(&setGoFlag, RATE);
    cali_button.rise(&calibratego);
    startButton.rise(&systemStart);
    stopButton.rise(&systemStop);
    
    while (true){
        if (calibrate_go==true){
            calibrate();         
            
            
        }
        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
            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){
                if (horrequest < 0){horrequest=0; pc.printf("hit left edge \n\r");}
            }
            if (edgeright==0){
                if (horrequest > 0){horrequest=0; pc.printf("hit right edge \n\r");}
            }
            if (edgetop==0){
                if (verrequest > 0){verrequest=0; pc.printf("hit top edge \n\r");}
            }
            if (edgebottom==0){
                if (verrequest < 0){verrequest=0; pc.printf("hit bottom edge \n\r");}
            }
            
            // 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;
            
            // 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 = 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 = 1;
                rightMotor = -rightPwmDuty;
            }
            else {
                rightDirection = 0;
                rightMotor = rightPwmDuty;
            }
            
            // ***************
            // User feedback
            scope.set(0, horrequest);
            scope.set(1, leftPwmDuty);
            scope.set(2, leftVelocity);
            scope.set(3, currentX);
            scope.set(4, currentY);
            scope.send();
                    
            goFlag=false;
        }
        if(systemOn==false)
        {
            leftMotor=0;
            rightMotor=0;
        }
    }
    
}