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@22:ae8012b12890, 2015-10-12 (annotated)

Committer:
ewoud
Date:
Mon Oct 12 09:33:34 2015 +0000
Revision:
22:ae8012b12890
Parent:
13:40141b362092 
integration issues

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