File content as of revision 1:dafb63606b66:

//****************************************************************************/
// 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(&calibrate);
    startButton.rise(&systemStart);
    stopButton.rise(&systemStop);
    
    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 output
            // 5) user output
                
            // **************
            // get emg signal
            sample_filter();
                       
            request_pos=y1;
            request_neg=y2;
            
            // determine if forward or backward signal is stronger
            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 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 (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 (cm)
            toX=currentX+request*RATE; 
            toY=currentY+0*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)
            //leftVelocity = leftVelocityfilter.step(leftVelocity);
            leftPrevPulses = leftPulses;
            leftController.setProcessValue(leftVelocity); // set PID process value
            
            // right
            rightPulses = rightQei.getPulses()/round*360; // (degree)
            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
            //rightVelocity = rightVelocityfilter.step(rightVelocity);
            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, leftRequest);
            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;
        }
    }
    
}