inits.h

00001 
00002 #define RATE  0.005f 
00003 #define Kc    0.30
00004 #define Ti    0.00
00005 #define Td    0.0
00006 
00007 ///////////////////////
00008 /// pin layout ////////
00009 ///////////////////////
00010 
00011 Serial pc(USBTX, USBRX);
00012 TextLCD lcd(D9, D14, D15, PTB19, PTB18, A4); // rs, e, d4 to d7
00013 
00014 // Left motor. (motor connection 1)
00015 PwmOut leftMotor(D5); 
00016 DigitalOut leftDirection(D4);
00017 QEI leftQei(D12, D13, NC, 624);
00018 PID leftController(Kc, Ti, Td, RATE);
00019 
00020 // Right motor (motor connection 2)
00021 PwmOut rightMotor(D6);
00022 DigitalOut rightDirection(D7);
00023 QEI rightQei(D11, D10, NC, 624);
00024 PID rightController(Kc, Ti, Td, RATE);
00025 
00026 // edge leads
00027 DigitalIn edgeleft(PTC5);
00028 DigitalIn edgeright(PTC7);
00029 DigitalIn edgetop(PTC0);
00030 DigitalIn edgebottom(PTC9);
00031 DigitalIn edgeStart(PTC8);
00032 DigitalIn edgeFinish(PTC1);
00033 
00034 // user interface
00035 DigitalOut statusled(LED_GREEN);
00036 InterruptIn startButton(D3);
00037 bool calibrated=false; // bool to determine if button press has to start calibration or the game
00038 InterruptIn stopButton(D2);
00039 InterruptIn initpositionButton(PTC6); 
00040 HIDScope scope(6);
00041 bool controlbypc=false;
00042 DigitalOut musicHit(A5);
00043 
00044 
00045 ////////////////////////////
00046 //////// Timers ////////////
00047 ////////////////////////////
00048 Timer playTimer;
00049 int playTime;
00050 Ticker motorControlTicker;
00051 Ticker lcdTicker;
00052 bool lcdGoFlag=false;
00053 bool goFlag=false;
00054 bool systemOn=false;
00055 bool initposition_go=false;
00056 
00057 
00058 ////////////////////////////
00059 ///////// Variables ////////
00060 ////////////////////////////
00061 
00062 
00063 // Working variables for motors
00064 volatile float leftPulses     = 0;
00065 volatile float leftAngle=0;
00066 volatile float leftPrevPulses = 0;
00067 volatile float leftPwmDuty  = 0.0;
00068 volatile float leftVelocity = 0.0;
00069 float leftRequest=0;
00070 
00071 volatile float rightPulses     = 0;
00072 volatile float rightPrevPulses = 0;
00073 volatile float rightAngle =0;
00074 volatile float rightPwmDuty  = 0.0;
00075 volatile float rightVelocity = 0.0;
00076 float rightRequest=0;
00077 
00078 // working vars for calculation
00079 float horrequest;
00080 float verrequest;
00081 float grenslaag=0.3;
00082 float grenshoog=0.7;
00083 float round=4200.0*6.6*6.6;
00084 float maxspeed=1; //cm/sec
00085 float currentX;
00086 float currentY;
00087 float l=50;
00088 float toX;
00089 float toY;
00090 float toLeftAngle;
00091 float toRightAngle;
00092 float M_PI=3.14159265359;
00093 string text;
00094 int hitCount=0;
00095 float hitTime=0;
00096 bool started=false;
00097 
00098 //////////////////////////////////
00099 ////// initialize functions //////
00100 //////////////////////////////////
00101 
00102 void initMotors(){
00103     //Initialization of motor
00104     leftMotor.period_us(50);
00105     leftMotor = 0.0;
00106     leftDirection = 1;
00107     
00108     rightMotor.period_us(50);
00109     rightMotor = 0.0;
00110     rightDirection = 1;
00111     
00112 }
00113 
00114 void initPIDs(){
00115     //Initialization of PID controller
00116     leftController.setInputLimits(-90.0, 90.0);
00117     leftController.setOutputLimits(-1.0, 1.0);
00118     leftController.setBias(0);
00119     leftController.setDeadzone(-0.4, 0.4);
00120     leftController.setMode(AUTO_MODE);
00121     
00122     rightController.setInputLimits(-90.0, 90.0);
00123     rightController.setOutputLimits(-1.0, 1.0);
00124     rightController.setBias(0);
00125     rightController.setDeadzone(-0.4, 0.4);
00126     rightController.setMode(AUTO_MODE);
00127 }