File content as of revision 17:034b50f49f46:

//****************************************************************************/
// Defines
//****************************************************************************/
#define RATE  0.05 
#define calcRATE 0.5
#define Kc    1.5
#define Ti    0.8
#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);

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


// Timers
Ticker motorControlTicker;
Ticker speedcalcTicker;
Timer velocityTimer;
float looptime;
float lasttime=0;
float thistime;
bool goFlag=false;
bool calcFlag=false;
bool didCalc=false;
bool systemOn=false;

// Working variables: motors
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;

// request calculation variables
float request;
float request_pos;
float request_neg;
float leftAngle;
float rightAngle;
//float round=4200;
float round=28000; // including extra gear.
float toX;
float toY;
float leftDeltaAngle;
float rightDeltaAngle;
float leftRequest;
float rightRequest;
float currentX;
float currentY;
float toLeftAngle;
float toRightAngle;
const double M_PI =3.141592653589793238463;
const float l = 100; // distance between the motors
const float armlength=150; // length of the arms from the motor

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

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);
}