inits.h

00001 //****************************************************************************/
00002 // Defines
00003 //****************************************************************************/
00004 #define RATE  0.01 
00005 #define Kc    0.30
00006 #define Ti    0.20
00007 #define Td    0.0
00008 
00009 //****************************************************************************/
00010 // Globals
00011 //****************************************************************************/
00012 Serial pc(USBTX, USBRX);
00013 HIDScope    scope(2);        // Instantize a 2-channel HIDScope object
00014 Ticker      scopeTimer;      // Instantize the timer for sending data to the PC 
00015 InterruptIn startButton(D3);
00016 InterruptIn stopButton(D2);
00017 //--------
00018 // Motors 
00019 //--------
00020 // Left motor.
00021 PwmOut leftMotor(D5);
00022 DigitalOut leftDirection(D4);
00023 QEI leftQei(D12, D13, NC, 624);
00024 PID leftController(Kc, Ti, Td, RATE);
00025 
00026 // Right motor
00027 PwmOut rightMotor(D6);
00028 DigitalOut rightDirection(D7);
00029 QEI rightQei(D11, D10, NC, 624);
00030 PID rightController(Kc, Ti, Td, RATE);
00031 
00032 // EMG input
00033 AnalogIn pot1(A0);
00034 AnalogIn pot2(A1);
00035 
00036 
00037 // Timers
00038 Timer endTimer;
00039 Ticker motorControlTicker;
00040 bool goFlag=false;
00041 bool systemOn=false;
00042 // Working variables.
00043 volatile int leftPulses     = 0;
00044 volatile int leftPrevPulses = 0;
00045 volatile float leftPwmDuty  = 0.0;
00046 volatile float leftPwmDutyPrev = 0.0;
00047 volatile float leftVelocity = 0.0;
00048 
00049 volatile int rightPulses     = 0;
00050 volatile int rightPrevPulses = 0;
00051 volatile float rightPwmDuty  = 0.0;
00052 volatile float rightPwmDutyPrev = 0.0;
00053 volatile float rightVelocity = 0.0;
00054 
00055 float request;
00056 float request_pos;
00057 float request_neg;
00058 
00059 void initMotors(){
00060     //Initialization of motor
00061     leftMotor.period_us(50);
00062     leftMotor = 0.0;
00063     leftDirection = 1;
00064     
00065     rightMotor.period_us(50);
00066     rightMotor = 0.0;
00067     rightDirection = 1;
00068     
00069 }
00070 
00071 void initPIDs(){
00072     //Initialization of PID controller
00073     leftController.setInputLimits(-1.0, 1.0);
00074     leftController.setOutputLimits(-1.0, 1.0);
00075     leftController.setBias(0);
00076     leftController.setDeadzone(-0.4, 0.4);
00077     leftController.setMode(AUTO_MODE);
00078     
00079     rightController.setInputLimits(-1.0, 1.0);
00080     rightController.setOutputLimits(-1.0, 1.0);
00081     rightController.setBias(0);
00082     rightController.setDeadzone(-0.4, 0.4);
00083     rightController.setMode(AUTO_MODE);
00084 }