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@14:102a2b4f5c86, 2015-10-06 (annotated)

Committer:
ewoud
Date:
Tue Oct 06 14:17:11 2015 +0000
Revision:
14:102a2b4f5c86
Parent:
13:40141b362092
Child:
16:e9945e3b4712
integrating xy to angle calculations, not working yet

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 14:102a2b4f5c86 4 #define RATE 0.1
ewoud 14:102a2b4f5c86 5 #define Kc 1.5
ewoud 14:102a2b4f5c86 6 #define Ti 0.8
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 14:102a2b4f5c86 13 HIDScope scope(5); // 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 14:102a2b4f5c86 42
ewoud 14:102a2b4f5c86 43 // Working variables: motors
ewoud 10:e807253d4879 44 volatile int leftPulses = 0;
ewoud 10:e807253d4879 45 volatile int leftPrevPulses = 0;
ewoud 10:e807253d4879 46 volatile float leftPwmDuty = 0.0;
ewoud 10:e807253d4879 47 volatile float leftPwmDutyPrev = 0.0;
ewoud 10:e807253d4879 48 volatile float leftVelocity = 0.0;
ewoud 10:e807253d4879 49
ewoud 10:e807253d4879 50 volatile int rightPulses = 0;
ewoud 10:e807253d4879 51 volatile int rightPrevPulses = 0;
ewoud 10:e807253d4879 52 volatile float rightPwmDuty = 0.0;
ewoud 10:e807253d4879 53 volatile float rightPwmDutyPrev = 0.0;
ewoud 10:e807253d4879 54 volatile float rightVelocity = 0.0;
ewoud 10:e807253d4879 55
ewoud 14:102a2b4f5c86 56 // request calculation variables
ewoud 10:e807253d4879 57 float request;
ewoud 10:e807253d4879 58 float request_pos;
ewoud 10:e807253d4879 59 float request_neg;
ewoud 14:102a2b4f5c86 60 float leftAngle;
ewoud 14:102a2b4f5c86 61 float rightAngle;
ewoud 14:102a2b4f5c86 62 float round=4200;
ewoud 14:102a2b4f5c86 63 float toX;
ewoud 14:102a2b4f5c86 64 float toY;
ewoud 14:102a2b4f5c86 65 float leftDeltaAngle;
ewoud 14:102a2b4f5c86 66 float rightDeltaAngle;
ewoud 14:102a2b4f5c86 67 float leftRequest;
ewoud 14:102a2b4f5c86 68 float rightRequest;
ewoud 14:102a2b4f5c86 69 float currentX;
ewoud 14:102a2b4f5c86 70 float currentY;
ewoud 14:102a2b4f5c86 71 float toLeftAngle;
ewoud 14:102a2b4f5c86 72 float toRightAngle;
ewoud 14:102a2b4f5c86 73 const double M_PI =3.141592653589793238463;
ewoud 14:102a2b4f5c86 74 const float l = 10; // distance between the motors
ewoud 14:102a2b4f5c86 75 const float armlength=15; // length of the arms from the motor
ewoud 10:e807253d4879 76
ewoud 10:e807253d4879 77 void initMotors(){
ewoud 10:e807253d4879 78 //Initialization of motor
ewoud 10:e807253d4879 79 leftMotor.period_us(50);
ewoud 10:e807253d4879 80 leftMotor = 0.0;
ewoud 10:e807253d4879 81 leftDirection = 1;
ewoud 10:e807253d4879 82
ewoud 10:e807253d4879 83 rightMotor.period_us(50);
ewoud 10:e807253d4879 84 rightMotor = 0.0;
ewoud 14:102a2b4f5c86 85 rightDirection = 0;
ewoud 10:e807253d4879 86
ewoud 10:e807253d4879 87 }
ewoud 10:e807253d4879 88
ewoud 10:e807253d4879 89 void initPIDs(){
ewoud 10:e807253d4879 90 //Initialization of PID controller
ewoud 10:e807253d4879 91 leftController.setInputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 92 leftController.setOutputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 93 leftController.setBias(0);
ewoud 10:e807253d4879 94 leftController.setDeadzone(-0.4, 0.4);
ewoud 10:e807253d4879 95 leftController.setMode(AUTO_MODE);
ewoud 10:e807253d4879 96
ewoud 10:e807253d4879 97 rightController.setInputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 98 rightController.setOutputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 99 rightController.setBias(0);
ewoud 10:e807253d4879 100 rightController.setDeadzone(-0.4, 0.4);
ewoud 10:e807253d4879 101 rightController.setMode(AUTO_MODE);
ewoud 10:e807253d4879 102 }