File content as of revision 0:ca94aa9bf823:

//****************************************************************************/
// 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");
}
void stoptop(){
    pc.printf("stop top\n\r");
}
void gotop(){
    pc.printf("go top\n\r");
    
}
bool skipvelocity=false;
int main() {
    
    // initialize (defined in inits.h)
    initMotors();
    initPIDs();
    
    outofboundsled=1;
    edgeleft.mode(PullUp);
    edgeright.mode(PullUp);
    motorControlTicker.attach(&setGoFlag, RATE);
    //T1.attach(&samplego, (float)1/Fs);
    
    cali_button.rise(&calibrate);
    startButton.rise(&systemStart);
    stopButton.rise(&systemStop);
    //edgetop.rise(&stoptop);
    //edgetop.fall(&gotop);
    
    
    endTimer.start(); //Run for 100 seconds.
    while (true){
        
        if (goFlag==true && systemOn==true){
            /*********** Contents *************/
            // 1) get emg signal
            // 2) calculate desired angle velocities
            // 3) calculate current angle velocities
            // 4) pid controller
            // 5) user output
                
            
            // get emg signal
            sample_filter();
                       
            request_pos=y1;
            request_neg=y2;
            
            // determine if forward or backward signal is stronger and set reference
            if (request_pos > request_neg){
                request = request_pos; 
            } 
            else {
                request = -request_neg;
            }
            request=request*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
                     
            // calculate required rotational velocity from the requested horizontal velocity
            // first get the current position from the motor encoders
            // make them start at 45 degree.
            leftAngle=(leftQei.getPulses()/round)*360+45;
            rightAngle=(rightQei.getPulses()/round)*360+45;
            
            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 (request < 0){request=0; pc.printf("hit left edge \n\r");}
            }
            if (edgeright==0){
                if (request > 0){request=0; pc.printf("hit right edge \n\r");}
            }
            
            // calculate the position to go to according the the current position + the distance that should be covered in this timestep
            toX=currentX+request*RATE; 
            toY=currentY+0*RATE; // should be vertical request*RATE
            
            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");}
            
            //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 how much the angles should change in this timestep
            leftDeltaAngle=(toLeftAngle-leftAngle);
            rightDeltaAngle=(toRightAngle-rightAngle);
            
            // calculate the neccesairy velocities to make these angles happen.
            leftRequest=(leftDeltaAngle/RATE); // degrees/sec
            rightRequest=(rightDeltaAngle/RATE); // degrees/sec
            
            // set the setpoint to the pid controller
            leftController.setSetPoint(leftRequest);
            rightController.setSetPoint(rightRequest);
            
            // *******************
            // Velocity calculation
            // left
            leftPulses = leftQei.getPulses()/round*360; // in degree
            leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; //degree /sec
            leftVelocity = leftVelocity; // scale to 0 - 1, max velocity = 20 degrees /sec
            //leftVelocity = leftVelocityfilter.step(leftVelocity);
            leftPrevPulses = leftPulses;
            leftController.setProcessValue(leftVelocity);
            
            // right
            rightPulses = rightQei.getPulses()/round*360;
            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
            rightVelocity = rightVelocity; // scale to 0 - 1, max velocity = 4200
            //rightVelocity = rightVelocityfilter.step(rightVelocity);
            rightPrevPulses = rightPulses;
            rightController.setProcessValue(rightVelocity);
            
            
            // ***********
            // PID control output
            // left
            
            leftPwmDuty = leftController.compute();
            if (leftPwmDuty < 0){
                leftDirection = 0;
                leftMotor = -leftPwmDuty;
            }
            else {
                leftDirection = 1;
                leftMotor = leftPwmDuty;
            }
            
            // right
            rightPwmDuty = rightController.compute();
            if (rightPwmDuty < 0){
                rightDirection = 1;
                rightMotor = -rightPwmDuty;
            }
            else {
                rightDirection = 0;
                rightMotor = rightPwmDuty;
            }
            
            // User feedback
            scope.set(0, leftRequest);
            scope.set(1, leftPwmDuty);
            scope.set(2, leftVelocity);
            scope.set(3, currentX);
            scope.set(4, currentY);
            scope.send();
            //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
        
            goFlag=false;
        }
        if(systemOn==false)
        {
            leftMotor=0;
            rightMotor=0;
        }
    }
    
}