File content as of revision 2:8f9b6c1e89cf:

//****************************************************************************/
// Defines
//****************************************************************************/
#define RATE  0.005f 
#define Kc    0.30
#define Ti    0.00
#define Td    0.0

//****************************************************************************/
// Globals
//****************************************************************************/
Serial pc(USBTX, USBRX);
HIDScope    scope(5);        // Instantize a 2-channel HIDScope object
Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
InterruptIn startButton(D3);
InterruptIn stopButton(D2);
//--------
// 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(D11, D10, NC, 624);
PID rightController(Kc, Ti, Td, RATE);

// edge leads
DigitalOut outofboundsled(LED1);
DigitalIn edgeleft(D0);
DigitalIn edgeright(D1);


// EMG input
AnalogIn pot1(A0);
AnalogIn pot2(A1);
biquadFilter leftVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
biquadFilter rightVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
// Timers
Timer endTimer;
Ticker motorControlTicker;
bool goFlag=false;
bool systemOn=false;
// Working variables.
volatile int leftPulses     = 0;
volatile float leftAngle=0;
volatile int leftPrevPulses = 0;
volatile float leftPwmDuty  = 0.0;
volatile float leftPwmDutyPrev = 0.0;
volatile float leftVelocity = 0.0;
float leftRequest=0;

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

float request;
float request_pos;
float request_neg;
float round=27720;
float maxspeed=10; //cm/sec
float currentX;
float currentY;
float l=10;
float toX;
float toY;
float toLeftAngle;
float toRightAngle;
float leftDeltaAngle;
float rightDeltaAngle;
float M_PI=3.14159265359;


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(-180.0, 180.0);
    leftController.setOutputLimits(-1.0, 1.0);
    leftController.setBias(0);
    leftController.setDeadzone(-0.4, 0.4);
    leftController.setMode(AUTO_MODE);
    
    rightController.setInputLimits(-180.0, 180.0);
    rightController.setOutputLimits(-1.0, 1.0);
    rightController.setBias(0);
    rightController.setDeadzone(-0.4, 0.4);
    rightController.setMode(AUTO_MODE);
}