motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

Committer:
ewoud
Date:
Wed Oct 07 12:19:30 2015 +0000
Revision:
17:034b50f49f46
Parent:
16:e9945e3b4712
Child:
18:4ee32b922251
fixed timing issue by introducing a clock to get the loop time

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