Numero Uno / QEI

using this library

Fork of QEI by Aaron Berk

inits.h@1:a0b7b5fba2a4, 2015-10-06 (annotated)

Committer:
ewoud
Date:
Tue Oct 06 10:16:58 2015 +0000
Revision:
1:a0b7b5fba2a4
nothing much

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 1:a0b7b5fba2a4 1 //****************************************************************************/
ewoud 1:a0b7b5fba2a4 2 // Defines
ewoud 1:a0b7b5fba2a4 3 //****************************************************************************/
ewoud 1:a0b7b5fba2a4 4 #define RATE 0.01
ewoud 1:a0b7b5fba2a4 5 #define Kc 0.30
ewoud 1:a0b7b5fba2a4 6 #define Ti 0.15
ewoud 1:a0b7b5fba2a4 7 #define Td 0.0
ewoud 1:a0b7b5fba2a4 8
ewoud 1:a0b7b5fba2a4 9 //****************************************************************************/
ewoud 1:a0b7b5fba2a4 10 // Globals
ewoud 1:a0b7b5fba2a4 11 //****************************************************************************/
ewoud 1:a0b7b5fba2a4 12 Serial pc(USBTX, USBRX);
ewoud 1:a0b7b5fba2a4 13 HIDScope scope(3); // Instantize a 2-channel HIDScope object
ewoud 1:a0b7b5fba2a4 14 Ticker scopeTimer; // Instantize the timer for sending data to the PC
ewoud 1:a0b7b5fba2a4 15 //--------
ewoud 1:a0b7b5fba2a4 16 // Motors
ewoud 1:a0b7b5fba2a4 17 //--------
ewoud 1:a0b7b5fba2a4 18 // Left motor.
ewoud 1:a0b7b5fba2a4 19 PwmOut leftMotor(D5);
ewoud 1:a0b7b5fba2a4 20 DigitalOut leftDirection(D4);
ewoud 1:a0b7b5fba2a4 21 QEI leftQei(D12, D13, NC, 624);
ewoud 1:a0b7b5fba2a4 22 PID leftController(Kc, Ti, Td, RATE);
ewoud 1:a0b7b5fba2a4 23
ewoud 1:a0b7b5fba2a4 24 // Right motor
ewoud 1:a0b7b5fba2a4 25 PwmOut rightMotor(D6);
ewoud 1:a0b7b5fba2a4 26 DigitalOut rightDirection(D7);
ewoud 1:a0b7b5fba2a4 27 QEI rightQei(D10, D11, NC, 624);
ewoud 1:a0b7b5fba2a4 28 PID rightController(Kc, Ti, Td, RATE);
ewoud 1:a0b7b5fba2a4 29
ewoud 1:a0b7b5fba2a4 30 // EMG input
ewoud 1:a0b7b5fba2a4 31 AnalogIn pot1(A0);
ewoud 1:a0b7b5fba2a4 32 AnalogIn pot2(A1);
ewoud 1:a0b7b5fba2a4 33
ewoud 1:a0b7b5fba2a4 34
ewoud 1:a0b7b5fba2a4 35 // Timers
ewoud 1:a0b7b5fba2a4 36 Timer endTimer;
ewoud 1:a0b7b5fba2a4 37 Ticker motorControlTicker;
ewoud 1:a0b7b5fba2a4 38 bool goFlag=false;
ewoud 1:a0b7b5fba2a4 39
ewoud 1:a0b7b5fba2a4 40 // Working variables.
ewoud 1:a0b7b5fba2a4 41 volatile int leftPulses = 0;
ewoud 1:a0b7b5fba2a4 42 volatile int leftPrevPulses = 0;
ewoud 1:a0b7b5fba2a4 43 volatile float leftPwmDuty = 0.0;
ewoud 1:a0b7b5fba2a4 44 volatile float leftPwmDutyPrev = 0.0;
ewoud 1:a0b7b5fba2a4 45 volatile float leftVelocity = 0.0;
ewoud 1:a0b7b5fba2a4 46
ewoud 1:a0b7b5fba2a4 47 volatile int rightPulses = 0;
ewoud 1:a0b7b5fba2a4 48 volatile int rightPrevPulses = 0;
ewoud 1:a0b7b5fba2a4 49 volatile float rightPwmDuty = 0.0;
ewoud 1:a0b7b5fba2a4 50 volatile float rightPwmDutyPrev = 0.0;
ewoud 1:a0b7b5fba2a4 51 volatile float rightVelocity = 0.0;
ewoud 1:a0b7b5fba2a4 52
ewoud 1:a0b7b5fba2a4 53 float request;
ewoud 1:a0b7b5fba2a4 54 float request_pos;
ewoud 1:a0b7b5fba2a4 55 float request_neg;
ewoud 1:a0b7b5fba2a4 56
ewoud 1:a0b7b5fba2a4 57 void initMotors(){
ewoud 1:a0b7b5fba2a4 58 //Initialization of motor
ewoud 1:a0b7b5fba2a4 59 leftMotor.period_us(50);
ewoud 1:a0b7b5fba2a4 60 leftMotor = 0.0;
ewoud 1:a0b7b5fba2a4 61 leftDirection = 1;
ewoud 1:a0b7b5fba2a4 62
ewoud 1:a0b7b5fba2a4 63 rightMotor.period_us(50);
ewoud 1:a0b7b5fba2a4 64 rightMotor = 0.0;
ewoud 1:a0b7b5fba2a4 65 rightDirection = 1;
ewoud 1:a0b7b5fba2a4 66
ewoud 1:a0b7b5fba2a4 67 }
ewoud 1:a0b7b5fba2a4 68
ewoud 1:a0b7b5fba2a4 69 void initPIDs(){
ewoud 1:a0b7b5fba2a4 70 //Initialization of PID controller
ewoud 1:a0b7b5fba2a4 71 leftController.setInputLimits(-1.0, 1.0);
ewoud 1:a0b7b5fba2a4 72 leftController.setOutputLimits(-1.0, 1.0);
ewoud 1:a0b7b5fba2a4 73 leftController.setBias(0);
ewoud 1:a0b7b5fba2a4 74 leftController.setDeadzone(-0.4, 0.4);
ewoud 1:a0b7b5fba2a4 75 leftController.setMode(AUTO_MODE);
ewoud 1:a0b7b5fba2a4 76
ewoud 1:a0b7b5fba2a4 77 rightController.setInputLimits(-1.0, 1.0);
ewoud 1:a0b7b5fba2a4 78 rightController.setOutputLimits(-1.0, 1.0);
ewoud 1:a0b7b5fba2a4 79 rightController.setBias(0);
ewoud 1:a0b7b5fba2a4 80 rightController.setDeadzone(-0.4, 0.4);
ewoud 1:a0b7b5fba2a4 81 rightController.setMode(AUTO_MODE);
ewoud 1:a0b7b5fba2a4 82 }