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@16:e9945e3b4712, 2015-10-07 (annotated)

Committer:
ewoud
Date:
Wed Oct 07 11:45:27 2015 +0000
Revision:
16:e9945e3b4712
Parent:
14:102a2b4f5c86
Child:
17:034b50f49f46
different timings for calculation and PID control; motor work with very tiny pot meter movements

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 16:e9945e3b4712 4 #define RATE 0.05
ewoud 16:e9945e3b4712 5 #define calcRATE 0.5
ewoud 14:102a2b4f5c86 6 #define Kc 1.5
ewoud 14:102a2b4f5c86 7 #define Ti 0.8
ewoud 10:e807253d4879 8 #define Td 0.0
ewoud 10:e807253d4879 9
ewoud 10:e807253d4879 10 //****************************************************************************/
ewoud 10:e807253d4879 11 // Globals
ewoud 10:e807253d4879 12 //****************************************************************************/
ewoud 10:e807253d4879 13 Serial pc(USBTX, USBRX);
ewoud 14:102a2b4f5c86 14 HIDScope scope(5); // Instantize a 2-channel HIDScope object
ewoud 10:e807253d4879 15 Ticker scopeTimer; // Instantize the timer for sending data to the PC
ewoud 13:40141b362092 16 InterruptIn startButton(D3);
ewoud 13:40141b362092 17 InterruptIn stopButton(D2);
ewoud 10:e807253d4879 18 //--------
ewoud 10:e807253d4879 19 // Motors
ewoud 10:e807253d4879 20 //--------
ewoud 10:e807253d4879 21 // Left motor.
ewoud 10:e807253d4879 22 PwmOut leftMotor(D5);
ewoud 10:e807253d4879 23 DigitalOut leftDirection(D4);
ewoud 10:e807253d4879 24 QEI leftQei(D12, D13, NC, 624);
ewoud 10:e807253d4879 25 PID leftController(Kc, Ti, Td, RATE);
ewoud 10:e807253d4879 26
ewoud 10:e807253d4879 27 // Right motor
ewoud 10:e807253d4879 28 PwmOut rightMotor(D6);
ewoud 10:e807253d4879 29 DigitalOut rightDirection(D7);
ewoud 12:d7bb475bb82d 30 QEI rightQei(D11, D10, NC, 624);
ewoud 10:e807253d4879 31 PID rightController(Kc, Ti, Td, RATE);
ewoud 10:e807253d4879 32
ewoud 10:e807253d4879 33 // EMG input
ewoud 10:e807253d4879 34 AnalogIn pot1(A0);
ewoud 10:e807253d4879 35 AnalogIn pot2(A1);
ewoud 10:e807253d4879 36
ewoud 10:e807253d4879 37
ewoud 10:e807253d4879 38 // Timers
ewoud 10:e807253d4879 39 Ticker motorControlTicker;
ewoud 16:e9945e3b4712 40 Ticker speedcalcTicker;
ewoud 10:e807253d4879 41 bool goFlag=false;
ewoud 16:e9945e3b4712 42 bool calcFlag=false;
ewoud 13:40141b362092 43 bool systemOn=false;
ewoud 14:102a2b4f5c86 44
ewoud 14:102a2b4f5c86 45 // Working variables: motors
ewoud 10:e807253d4879 46 volatile int leftPulses = 0;
ewoud 10:e807253d4879 47 volatile int leftPrevPulses = 0;
ewoud 10:e807253d4879 48 volatile float leftPwmDuty = 0.0;
ewoud 10:e807253d4879 49 volatile float leftPwmDutyPrev = 0.0;
ewoud 10:e807253d4879 50 volatile float leftVelocity = 0.0;
ewoud 10:e807253d4879 51
ewoud 10:e807253d4879 52 volatile int rightPulses = 0;
ewoud 10:e807253d4879 53 volatile int rightPrevPulses = 0;
ewoud 10:e807253d4879 54 volatile float rightPwmDuty = 0.0;
ewoud 10:e807253d4879 55 volatile float rightPwmDutyPrev = 0.0;
ewoud 10:e807253d4879 56 volatile float rightVelocity = 0.0;
ewoud 10:e807253d4879 57
ewoud 14:102a2b4f5c86 58 // request calculation variables
ewoud 10:e807253d4879 59 float request;
ewoud 10:e807253d4879 60 float request_pos;
ewoud 10:e807253d4879 61 float request_neg;
ewoud 14:102a2b4f5c86 62 float leftAngle;
ewoud 14:102a2b4f5c86 63 float rightAngle;
ewoud 16:e9945e3b4712 64 //float round=4200;
ewoud 16:e9945e3b4712 65 float round=28000; // including extra gear.
ewoud 14:102a2b4f5c86 66 float toX;
ewoud 14:102a2b4f5c86 67 float toY;
ewoud 14:102a2b4f5c86 68 float leftDeltaAngle;
ewoud 14:102a2b4f5c86 69 float rightDeltaAngle;
ewoud 14:102a2b4f5c86 70 float leftRequest;
ewoud 14:102a2b4f5c86 71 float rightRequest;
ewoud 14:102a2b4f5c86 72 float currentX;
ewoud 14:102a2b4f5c86 73 float currentY;
ewoud 14:102a2b4f5c86 74 float toLeftAngle;
ewoud 14:102a2b4f5c86 75 float toRightAngle;
ewoud 14:102a2b4f5c86 76 const double M_PI =3.141592653589793238463;
ewoud 16:e9945e3b4712 77 const float l = 100; // distance between the motors
ewoud 16:e9945e3b4712 78 const float armlength=150; // length of the arms from the motor
ewoud 10:e807253d4879 79
ewoud 10:e807253d4879 80 void initMotors(){
ewoud 10:e807253d4879 81 //Initialization of motor
ewoud 10:e807253d4879 82 leftMotor.period_us(50);
ewoud 10:e807253d4879 83 leftMotor = 0.0;
ewoud 10:e807253d4879 84 leftDirection = 1;
ewoud 10:e807253d4879 85
ewoud 10:e807253d4879 86 rightMotor.period_us(50);
ewoud 10:e807253d4879 87 rightMotor = 0.0;
ewoud 14:102a2b4f5c86 88 rightDirection = 0;
ewoud 10:e807253d4879 89
ewoud 10:e807253d4879 90 }
ewoud 10:e807253d4879 91
ewoud 10:e807253d4879 92 void initPIDs(){
ewoud 10:e807253d4879 93 //Initialization of PID controller
ewoud 10:e807253d4879 94 leftController.setInputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 95 leftController.setOutputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 96 leftController.setBias(0);
ewoud 10:e807253d4879 97 leftController.setDeadzone(-0.4, 0.4);
ewoud 10:e807253d4879 98 leftController.setMode(AUTO_MODE);
ewoud 10:e807253d4879 99
ewoud 10:e807253d4879 100 rightController.setInputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 101 rightController.setOutputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 102 rightController.setBias(0);
ewoud 10:e807253d4879 103 rightController.setDeadzone(-0.4, 0.4);
ewoud 10:e807253d4879 104 rightController.setMode(AUTO_MODE);
ewoud 10:e807253d4879 105 }