File content as of revision 1:a0b7b5fba2a4:

//****************************************************************************/
// Defines
//****************************************************************************/
#define RATE  0.01 
#define Kc    0.30
#define Ti    0.15
#define Td    0.0

//****************************************************************************/
// Globals
//****************************************************************************/
Serial pc(USBTX, USBRX);
HIDScope    scope(3);        // Instantize a 2-channel HIDScope object
Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
//--------
// Motors 
//--------
// Left motor.
PwmOut leftMotor(D5);
DigitalOut leftDirection(D4);
QEI leftQei(D12, D13, NC, 624);
PID leftController(Kc, Ti, Td, RATE);

// Right motor
PwmOut rightMotor(D6);
DigitalOut rightDirection(D7);
QEI rightQei(D10, D11, NC, 624);
PID rightController(Kc, Ti, Td, RATE);

// EMG input
AnalogIn pot1(A0);
AnalogIn pot2(A1);


// Timers
Timer endTimer;
Ticker motorControlTicker;
bool goFlag=false;

// Working variables.
volatile int leftPulses     = 0;
volatile int leftPrevPulses = 0;
volatile float leftPwmDuty  = 0.0;
volatile float leftPwmDutyPrev = 0.0;
volatile float leftVelocity = 0.0;

volatile int rightPulses     = 0;
volatile int rightPrevPulses = 0;
volatile float rightPwmDuty  = 0.0;
volatile float rightPwmDutyPrev = 0.0;
volatile float rightVelocity = 0.0;

float request;
float request_pos;
float request_neg;

void initMotors(){
    //Initialization of motor
    leftMotor.period_us(50);
    leftMotor = 0.0;
    leftDirection = 1;
    
    rightMotor.period_us(50);
    rightMotor = 0.0;
    rightDirection = 1;
    
}

void initPIDs(){
    //Initialization of PID controller
    leftController.setInputLimits(-1.0, 1.0);
    leftController.setOutputLimits(-1.0, 1.0);
    leftController.setBias(0);
    leftController.setDeadzone(-0.4, 0.4);
    leftController.setMode(AUTO_MODE);
    
    rightController.setInputLimits(-1.0, 1.0);
    rightController.setOutputLimits(-1.0, 1.0);
    rightController.setBias(0);
    rightController.setDeadzone(-0.4, 0.4);
    rightController.setMode(AUTO_MODE);
}