Numero Uno / Mbed 2 deprecated PID_VelocityExample

motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

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

Committer:
ewoud
Date:
Tue Oct 06 10:19:15 2015 +0000
Revision:
10:e807253d4879
Child:
12:d7bb475bb82d
added second motor and deadzone support

Who changed what in which revision?

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