File content as of revision 22:ae8012b12890:

//****************************************************************************/
// Includes
#include "PID.h"
#include "QEI.h"
#include "HIDScope.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;
    leftMotor=0;
    rightMotor=0;
}
int main() {
    
    // initialize (defined in inits.h)
    initMotors();
    initPIDs();
    
    motorControlTicker.attach(&setGoFlag, RATE);
    T1.attach(&samplego, (float)1/Fs);
    
    cali_button.rise(&calibrate);
    startButton.rise(&systemStart);
    stopButton.rise(&systemStop);
    
    endTimer.start(); //Run for 100 seconds.
    while (true){
        if (goFlag==true && systemOn==true){
            // get 'emg' signal
            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;
            }
            leftController.setSetPoint(request);
            rightController.setSetPoint(0);
            
            // *******************
            // 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 = 0;
                rightMotor = -rightPwmDuty;
            }
            else {
                rightDirection = 1;
                rightMotor = rightPwmDuty;
            }
            
            // User feedback
            //scope.set(0, request);
            //scope.set(1, leftPwmDuty);
            //scope.set(2, leftVelocity);
            //scope.send();
            pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
        
            goFlag=false;
        }
        if(sample_go && systemOn==true)
        {
            sample_filter();
            scope.set(0, y1);
            scope.set(1, y2);
            scope.send();
            sample_go = 0;
        }
    }

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