using this library
Fork of QEI by
Diff: inits.h
- Revision:
- 1:a0b7b5fba2a4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/inits.h Tue Oct 06 10:16:58 2015 +0000 @@ -0,0 +1,82 @@ +//****************************************************************************/ +// Defines +//****************************************************************************/ +#define RATE 0.01 +#define Kc 0.30 +#define Ti 0.15 +#define Td 0.0 + +//****************************************************************************/ +// Globals +//****************************************************************************/ +Serial pc(USBTX, USBRX); +HIDScope scope(3); // Instantize a 2-channel HIDScope object +Ticker scopeTimer; // Instantize the timer for sending data to the PC +//-------- +// 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(D10, D11, NC, 624); +PID rightController(Kc, Ti, Td, RATE); + +// EMG input +AnalogIn pot1(A0); +AnalogIn pot2(A1); + + +// Timers +Timer endTimer; +Ticker motorControlTicker; +bool goFlag=false; + +// Working variables. +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; + +float request; +float request_pos; +float request_neg; + +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(-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); +} \ No newline at end of file