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@19:3ca10fe26131, 2015-10-12 (annotated)

Committer:
ewoud
Date:
Mon Oct 12 08:13:29 2015 +0000
Revision:
19:3ca10fe26131
Parent:
18:4ee32b922251 
last version

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 18:4ee32b922251 4 #define RATE 0.01
ewoud 19:3ca10fe26131 5 #define calcRATE 0.2
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 19:3ca10fe26131 36 float maxspeed=10; // xy in cm/s
ewoud 10:e807253d4879 37
ewoud 10:e807253d4879 38 // Timers
ewoud 10:e807253d4879 39 Ticker motorControlTicker;
ewoud 16:e9945e3b4712 40 Ticker speedcalcTicker;
ewoud 17:034b50f49f46 41 Timer velocityTimer;
ewoud 17:034b50f49f46 42 float looptime;
ewoud 17:034b50f49f46 43 float lasttime=0;
ewoud 17:034b50f49f46 44 float thistime;
ewoud 10:e807253d4879 45 bool goFlag=false;
ewoud 16:e9945e3b4712 46 bool calcFlag=false;
ewoud 13:40141b362092 47 bool systemOn=false;
ewoud 14:102a2b4f5c86 48
ewoud 14:102a2b4f5c86 49 // Working variables: motors
ewoud 10:e807253d4879 50 volatile int leftPulses = 0;
ewoud 10:e807253d4879 51 volatile int leftPrevPulses = 0;
ewoud 10:e807253d4879 52 volatile float leftPwmDuty = 0.0;
ewoud 10:e807253d4879 53 volatile float leftPwmDutyPrev = 0.0;
ewoud 10:e807253d4879 54 volatile float leftVelocity = 0.0;
ewoud 10:e807253d4879 55
ewoud 10:e807253d4879 56 volatile int rightPulses = 0;
ewoud 10:e807253d4879 57 volatile int rightPrevPulses = 0;
ewoud 10:e807253d4879 58 volatile float rightPwmDuty = 0.0;
ewoud 10:e807253d4879 59 volatile float rightPwmDutyPrev = 0.0;
ewoud 10:e807253d4879 60 volatile float rightVelocity = 0.0;
ewoud 10:e807253d4879 61
ewoud 14:102a2b4f5c86 62 // request calculation variables
ewoud 10:e807253d4879 63 float request;
ewoud 10:e807253d4879 64 float request_pos;
ewoud 10:e807253d4879 65 float request_neg;
ewoud 14:102a2b4f5c86 66 float leftAngle;
ewoud 14:102a2b4f5c86 67 float rightAngle;
ewoud 16:e9945e3b4712 68 //float round=4200;
ewoud 16:e9945e3b4712 69 float round=28000; // including extra gear.
ewoud 14:102a2b4f5c86 70 float toX;
ewoud 14:102a2b4f5c86 71 float toY;
ewoud 14:102a2b4f5c86 72 float leftDeltaAngle;
ewoud 14:102a2b4f5c86 73 float rightDeltaAngle;
ewoud 14:102a2b4f5c86 74 float leftRequest;
ewoud 14:102a2b4f5c86 75 float rightRequest;
ewoud 14:102a2b4f5c86 76 float currentX;
ewoud 14:102a2b4f5c86 77 float currentY;
ewoud 14:102a2b4f5c86 78 float toLeftAngle;
ewoud 14:102a2b4f5c86 79 float toRightAngle;
ewoud 14:102a2b4f5c86 80 const double M_PI =3.141592653589793238463;
ewoud 16:e9945e3b4712 81 const float l = 100; // distance between the motors
ewoud 18:4ee32b922251 82 const float armlength=50; // length of the arms from the motor
ewoud 10:e807253d4879 83
ewoud 10:e807253d4879 84 void initMotors(){
ewoud 10:e807253d4879 85 //Initialization of motor
ewoud 10:e807253d4879 86 leftMotor.period_us(50);
ewoud 10:e807253d4879 87 leftMotor = 0.0;
ewoud 10:e807253d4879 88 leftDirection = 1;
ewoud 10:e807253d4879 89
ewoud 10:e807253d4879 90 rightMotor.period_us(50);
ewoud 10:e807253d4879 91 rightMotor = 0.0;
ewoud 14:102a2b4f5c86 92 rightDirection = 0;
ewoud 10:e807253d4879 93
ewoud 10:e807253d4879 94 }
ewoud 10:e807253d4879 95
ewoud 10:e807253d4879 96 void initPIDs(){
ewoud 10:e807253d4879 97 //Initialization of PID controller
ewoud 10:e807253d4879 98 leftController.setInputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 99 leftController.setOutputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 100 leftController.setBias(0);
ewoud 10:e807253d4879 101 leftController.setDeadzone(-0.4, 0.4);
ewoud 10:e807253d4879 102 leftController.setMode(AUTO_MODE);
ewoud 10:e807253d4879 103
ewoud 10:e807253d4879 104 rightController.setInputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 105 rightController.setOutputLimits(-1.0, 1.0);
ewoud 10:e807253d4879 106 rightController.setBias(0);
ewoud 10:e807253d4879 107 rightController.setDeadzone(-0.4, 0.4);
ewoud 10:e807253d4879 108 rightController.setMode(AUTO_MODE);
ewoud 10:e807253d4879 109 }