File content as of revision 10:819fb5288aa0:

#define RATE  0.005f 
#define Kc    0.30
#define Ti    0.00
#define Td    0.0

///////////////////////
/// pin layout ////////
///////////////////////

Serial pc(USBTX, USBRX);
TextLCD lcd(D9, D14, D15, PTB19, PTB18, A4); // rs, e, d4 to d7

// Left motor. (motor connection 1)
PwmOut leftMotor(D5); 
DigitalOut leftDirection(D4);
QEI leftQei(D12, D13, NC, 624);
PID leftController(Kc, Ti, Td, RATE);

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

// edge leads
DigitalIn edgeleft(PTC5);
DigitalIn edgeright(PTC7);
DigitalIn edgetop(PTC0);
DigitalIn edgebottom(PTC9);
DigitalIn edgeStart(PTC8);
DigitalIn edgeFinish(PTC1);

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

// user interface
DigitalOut statusled(LED_GREEN);
InterruptIn startButton(D3);
bool calibrated=false; // bool to determine if button press has to start calibration or the game
InterruptIn stopButton(D2);
InterruptIn initpositionButton(PTC6); 
HIDScope scope(5);
bool controlbypc=false;

////////////////////////////
//////// Timers ////////////
////////////////////////////
Ticker scopeTimer;
Timer playTimer;
Ticker motorControlTicker;
Ticker lcdTicker;
bool lcdGoFlag=false;
bool goFlag=false;
bool systemOn=false;
bool initposition_go=false;


////////////////////////////
///////// Variables ////////
////////////////////////////


// Working variables for motors
volatile float leftPulses     = 0;
volatile float leftAngle=0;
volatile float leftPrevPulses = 0;
volatile float leftPwmDuty  = 0.0;
volatile float leftVelocity = 0.0;
float leftRequest=0;

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

// working vars for calculation
float horrequest;
float verrequest;
float grenslaag=0.3;
float grenshoog=0.7;
float round=4200.0*6.6*6.6;
float maxspeed=1; //cm/sec
float currentX;
float currentY;
float l=57;
float toX;
float toY;
float toLeftAngle;
float toRightAngle;
float M_PI=3.14159265359;
string text;

//////////////////////////////////
////// initialize functions //////
//////////////////////////////////

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