motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp@0:9bca35ae9c6b, 2010-08-03 (annotated)
- Committer:
- aberk
- Date:
- Tue Aug 03 09:24:06 2010 +0000
- Revision:
- 0:9bca35ae9c6b
- Child:
- 1:ac598811dd00
Version 1.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aberk | 0:9bca35ae9c6b | 1 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 2 | // Includes |
aberk | 0:9bca35ae9c6b | 3 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 4 | #include "PID.h" |
aberk | 0:9bca35ae9c6b | 5 | #include "QEI.h" |
aberk | 0:9bca35ae9c6b | 6 | |
aberk | 0:9bca35ae9c6b | 7 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 8 | // Defines |
aberk | 0:9bca35ae9c6b | 9 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 10 | #define RATE 0.01 |
aberk | 0:9bca35ae9c6b | 11 | #define Kc -2.6 |
aberk | 0:9bca35ae9c6b | 12 | #define Ti 0.0 |
aberk | 0:9bca35ae9c6b | 13 | #define Td 0.0 |
aberk | 0:9bca35ae9c6b | 14 | |
aberk | 0:9bca35ae9c6b | 15 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 16 | // Globals |
aberk | 0:9bca35ae9c6b | 17 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 18 | //-------- |
aberk | 0:9bca35ae9c6b | 19 | // Motors |
aberk | 0:9bca35ae9c6b | 20 | //-------- |
aberk | 0:9bca35ae9c6b | 21 | //Left motor. |
aberk | 0:9bca35ae9c6b | 22 | PwmOut leftMotor(p23); |
aberk | 0:9bca35ae9c6b | 23 | DigitalOut leftBrake(p19); |
aberk | 0:9bca35ae9c6b | 24 | DigitalOut leftDirection(p28); |
aberk | 0:9bca35ae9c6b | 25 | QEI leftQei(p29, p30, NC, 624); |
aberk | 0:9bca35ae9c6b | 26 | PID leftController(Kc, Ti, Td, RATE); |
aberk | 0:9bca35ae9c6b | 27 | //------- |
aberk | 0:9bca35ae9c6b | 28 | // Files |
aberk | 0:9bca35ae9c6b | 29 | //------- |
aberk | 0:9bca35ae9c6b | 30 | LocalFileSystem local("local"); |
aberk | 0:9bca35ae9c6b | 31 | FILE* fp; |
aberk | 0:9bca35ae9c6b | 32 | //-------- |
aberk | 0:9bca35ae9c6b | 33 | // Timers |
aberk | 0:9bca35ae9c6b | 34 | //-------- |
aberk | 0:9bca35ae9c6b | 35 | Timer endTimer; |
aberk | 0:9bca35ae9c6b | 36 | //-------------------- |
aberk | 0:9bca35ae9c6b | 37 | // Working variables. |
aberk | 0:9bca35ae9c6b | 38 | //-------------------- |
aberk | 0:9bca35ae9c6b | 39 | volatile int leftPulses = 0; |
aberk | 0:9bca35ae9c6b | 40 | volatile int leftPrevPulses = 0; |
aberk | 0:9bca35ae9c6b | 41 | volatile float leftPwmDuty = 1.0; |
aberk | 0:9bca35ae9c6b | 42 | volatile float leftVelocity = 0.0; |
aberk | 0:9bca35ae9c6b | 43 | //Velocity to reach. |
aberk | 0:9bca35ae9c6b | 44 | int goal = 3000; |
aberk | 0:9bca35ae9c6b | 45 | |
aberk | 0:9bca35ae9c6b | 46 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 47 | // Prototypes |
aberk | 0:9bca35ae9c6b | 48 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 49 | //Set motors to go "forward", brake off, not moving. |
aberk | 0:9bca35ae9c6b | 50 | void initializeMotors(void); |
aberk | 0:9bca35ae9c6b | 51 | //Set up PID controllers with appropriate limits and biases. |
aberk | 0:9bca35ae9c6b | 52 | void initializePidControllers(void); |
aberk | 0:9bca35ae9c6b | 53 | |
aberk | 0:9bca35ae9c6b | 54 | void initializeMotors(void){ |
aberk | 0:9bca35ae9c6b | 55 | |
aberk | 0:9bca35ae9c6b | 56 | leftMotor.period_us(50); |
aberk | 0:9bca35ae9c6b | 57 | leftMotor = 1.0; |
aberk | 0:9bca35ae9c6b | 58 | leftBrake = 0.0; |
aberk | 0:9bca35ae9c6b | 59 | leftDirection = 0; |
aberk | 0:9bca35ae9c6b | 60 | |
aberk | 0:9bca35ae9c6b | 61 | } |
aberk | 0:9bca35ae9c6b | 62 | |
aberk | 0:9bca35ae9c6b | 63 | void initializePidControllers(void){ |
aberk | 0:9bca35ae9c6b | 64 | |
aberk | 0:9bca35ae9c6b | 65 | leftController.setInputLimits(0.0, 10500.0); |
aberk | 0:9bca35ae9c6b | 66 | leftController.setOutputLimits(0.0, 1.0); |
aberk | 0:9bca35ae9c6b | 67 | leftController.setBias(1.0); |
aberk | 0:9bca35ae9c6b | 68 | leftController.setMode(AUTO_MODE); |
aberk | 0:9bca35ae9c6b | 69 | |
aberk | 0:9bca35ae9c6b | 70 | } |
aberk | 0:9bca35ae9c6b | 71 | |
aberk | 0:9bca35ae9c6b | 72 | int main() { |
aberk | 0:9bca35ae9c6b | 73 | |
aberk | 0:9bca35ae9c6b | 74 | //Initialization. |
aberk | 0:9bca35ae9c6b | 75 | initializeMotors(); |
aberk | 0:9bca35ae9c6b | 76 | initializePidControllers(); |
aberk | 0:9bca35ae9c6b | 77 | |
aberk | 0:9bca35ae9c6b | 78 | //Open results file. |
aberk | 0:9bca35ae9c6b | 79 | fp = fopen("/local/pidtest.csv", "w"); |
aberk | 0:9bca35ae9c6b | 80 | |
aberk | 0:9bca35ae9c6b | 81 | endTimer.start(); |
aberk | 0:9bca35ae9c6b | 82 | |
aberk | 0:9bca35ae9c6b | 83 | //Set velocity set point. |
aberk | 0:9bca35ae9c6b | 84 | leftController.setSetPoint(goal); |
aberk | 0:9bca35ae9c6b | 85 | |
aberk | 0:9bca35ae9c6b | 86 | //Run for 3 seconds. |
aberk | 0:9bca35ae9c6b | 87 | while (endTimer.read() < 3.0){ |
aberk | 0:9bca35ae9c6b | 88 | leftPulses = leftQei.getPulses(); |
aberk | 0:9bca35ae9c6b | 89 | leftVelocity = (leftPulses - leftPrevPulses) / RATE; |
aberk | 0:9bca35ae9c6b | 90 | leftPrevPulses = leftPulses; |
aberk | 0:9bca35ae9c6b | 91 | leftController.setProcessValue(leftVelocity); |
aberk | 0:9bca35ae9c6b | 92 | leftPwmDuty = leftController.getRealOutput(); |
aberk | 0:9bca35ae9c6b | 93 | leftMotor = leftPwmDuty; |
aberk | 0:9bca35ae9c6b | 94 | fprintf(fp, "%f,%f\n", leftVelocity, goal); |
aberk | 0:9bca35ae9c6b | 95 | wait(RATE); |
aberk | 0:9bca35ae9c6b | 96 | } |
aberk | 0:9bca35ae9c6b | 97 | |
aberk | 0:9bca35ae9c6b | 98 | //Stop motors. |
aberk | 0:9bca35ae9c6b | 99 | leftMotor = 1.0; |
aberk | 0:9bca35ae9c6b | 100 | |
aberk | 0:9bca35ae9c6b | 101 | //Close results file. |
aberk | 0:9bca35ae9c6b | 102 | fclose(fp); |
aberk | 0:9bca35ae9c6b | 103 | |
aberk | 0:9bca35ae9c6b | 104 | } |