File content as of revision 16:e9945e3b4712:

//****************************************************************************/
// Includes
#include "PID.h"
#include "QEI.h"
#include "HIDScope.h"
#include <math.h>
//#include "Point.cpp"
#include "inits.h" // all globals, pin and variable initialization 

void setGoFlag(){
    if (goFlag==true && systemOn==true && calcFlag==false){
        pc.printf("ERROR: INCORRECT TIMINGS, look at the setGoFlag \n\r");
        // flag is already set true: code too slow or frequency too high    
    }
    goFlag=true;
}
void setCalcFlag(){
    if (calcFlag==true && systemOn==true){
        pc.printf("ERROR: INCORRECT TIMINGS, look at the setCalcFlag \n\r");   
    }
    calcFlag=true;     
}
void systemStart(){
    systemOn=true;
}
void systemStop(){
    systemOn=false;
    leftMotor=0;
    rightMotor=0;
}
int main() {
    
    // initialize (defined in inits.h)
    initMotors();
    initPIDs();
    
    speedcalcTicker.attach(&setCalcFlag, calcRATE);
    motorControlTicker.attach(&setGoFlag, RATE);
    
    startButton.rise(&systemStart);
    stopButton.rise(&systemStop);
    
    while (true){
        if (systemOn==true && calcFlag==true){
            // get 'emg' signal
            request_pos = pot1.read();
            request_neg = pot2.read();
            
            // determine if forward or backward signal is stronger
            if (request_pos > request_neg){
                request = request_pos; 
            } 
            else {
                request = -request_neg;
            }
            
            // calculate required rotational velocity from the requested horizontal velocity
            // first get the current position from the motor encoders
            // make them start at 45 degrees
            leftAngle=(leftQei.getPulses()/round)*360+45;
            rightAngle=(rightQei.getPulses()/round)*360+45;
            
            if (leftAngle < 0 or leftAngle > 90 or rightAngle < 0 or rightAngle > 90 ){
                pc.printf("out of bounds \n\r");
                leftRequest=0;
                rightRequest=0;
                //if (leftAngle < -45){leftRequest=0.1;}
                //if (leftAngle > 45){leftRequest=-0.1;}
                //if (rightAngle < -45){rightRequest=0.1;}
                //if (rightAngle > 45){rightRequest=-0.1;}
            }
            else {
                currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
                currentY = tan(leftAngle*M_PI/180)*currentX;
                
                //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 position to go to according the the current position + the distance that should be covered in this timestep
                toX=currentX+request*calcRATE; // should be request*RATE
                //toX=5;
                //toY=pCurrent.posY+0*RATE;
                toY=currentY;
            
                toLeftAngle = atan(toY/toX)*180/M_PI;
                toRightAngle = atan(toY/(l-toX))*180/M_PI;
                
                // 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/calcRATE);
                rightRequest=(rightDeltaAngle/calcRATE);
            }
            pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle);
            calcFlag=false;
        }
        else if (systemOn == true && goFlag == true){
            // set the PID controller to go with that speed.
            leftController.setSetPoint(leftRequest);
            rightController.setSetPoint(rightRequest);
            
            //pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle);
            
            // *******************
            // Velocity calculation
            // left
            leftPulses = leftQei.getPulses();
            leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
            leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
            leftPrevPulses = leftPulses;
            
            // right
            rightPulses = rightQei.getPulses();
            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
            rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
            rightPrevPulses = rightPulses;
            
            // ***********
            // PID control
            // left
            leftController.setProcessValue(leftVelocity);
            leftPwmDuty = leftController.compute();
            if (leftPwmDuty < 0){
                leftDirection = 0;
                leftMotor = -leftPwmDuty;
            }
            else {
                leftDirection = 1;
                leftMotor = leftPwmDuty;
            }
            
            // right
            rightController.setProcessValue(rightVelocity);
            rightPwmDuty = rightController.compute();
            if (rightPwmDuty < 0){
                rightDirection = 1;
                rightMotor = -rightPwmDuty;
            }
            else {
                rightDirection = 0;
                rightMotor = rightPwmDuty;
            }
            
            // User feedback
            scope.set(0, rightRequest);
            scope.set(1, rightPwmDuty);
            scope.set(2, rightVelocity);
            scope.set(3, leftAngle);
            scope.set(4, rightAngle);
            scope.send();
            //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
        
            goFlag=false;
        }
    }

    //Stop motors.
    leftMotor  = 0;
    rightMotor = 0;
    
}