Betago
/
PID_VelocityExample
velocity control
Fork of PID_VelocityExample by
main.cpp@2:646e3bca12a8, 2016-06-27 (annotated)
- Committer:
- soulx
- Date:
- Mon Jun 27 23:12:28 2016 +0000
- Revision:
- 2:646e3bca12a8
- Parent:
- 1:ac598811dd00
edit 2 motor
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" |
soulx | 2:646e3bca12a8 | 6 | #include "mbed.h" |
aberk | 0:9bca35ae9c6b | 7 | |
aberk | 0:9bca35ae9c6b | 8 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 9 | // Defines |
aberk | 0:9bca35ae9c6b | 10 | //****************************************************************************/ |
soulx | 2:646e3bca12a8 | 11 | #define RATE 0.1 |
soulx | 2:646e3bca12a8 | 12 | #define Kc 1.0 |
aberk | 0:9bca35ae9c6b | 13 | #define Ti 0.0 |
aberk | 0:9bca35ae9c6b | 14 | #define Td 0.0 |
aberk | 0:9bca35ae9c6b | 15 | |
aberk | 0:9bca35ae9c6b | 16 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 17 | // Globals |
aberk | 0:9bca35ae9c6b | 18 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 19 | //-------- |
soulx | 2:646e3bca12a8 | 20 | // Motors |
aberk | 0:9bca35ae9c6b | 21 | //-------- |
aberk | 0:9bca35ae9c6b | 22 | //Left motor. |
soulx | 2:646e3bca12a8 | 23 | PwmOut leftMotor(PB_4); |
soulx | 2:646e3bca12a8 | 24 | DigitalOut leftBrake(PA_15); |
soulx | 2:646e3bca12a8 | 25 | DigitalOut leftDirection(PB_7); |
soulx | 2:646e3bca12a8 | 26 | QEI leftQei(PC_10, PC_12, NC, 624); |
aberk | 0:9bca35ae9c6b | 27 | PID leftController(Kc, Ti, Td, RATE); |
soulx | 2:646e3bca12a8 | 28 | |
soulx | 2:646e3bca12a8 | 29 | //Right motor. |
soulx | 2:646e3bca12a8 | 30 | PwmOut rightMotor(PB_10); |
soulx | 2:646e3bca12a8 | 31 | DigitalOut rightBrake(PA_13); |
soulx | 2:646e3bca12a8 | 32 | DigitalOut rightDirection(PA_14); |
soulx | 2:646e3bca12a8 | 33 | QEI rightQei(PA_5, PA_6, NC, 624); |
soulx | 2:646e3bca12a8 | 34 | PID rightController(Kc, Ti, Td, RATE); |
soulx | 2:646e3bca12a8 | 35 | |
soulx | 2:646e3bca12a8 | 36 | //-------- |
soulx | 2:646e3bca12a8 | 37 | // Serial |
soulx | 2:646e3bca12a8 | 38 | //-------- |
soulx | 2:646e3bca12a8 | 39 | Serial pc(SERIAL_TX,SERIAL_RX); |
soulx | 2:646e3bca12a8 | 40 | |
aberk | 0:9bca35ae9c6b | 41 | //-------- |
aberk | 0:9bca35ae9c6b | 42 | // Timers |
aberk | 0:9bca35ae9c6b | 43 | //-------- |
aberk | 0:9bca35ae9c6b | 44 | Timer endTimer; |
aberk | 0:9bca35ae9c6b | 45 | //-------------------- |
aberk | 0:9bca35ae9c6b | 46 | // Working variables. |
aberk | 0:9bca35ae9c6b | 47 | //-------------------- |
aberk | 0:9bca35ae9c6b | 48 | volatile int leftPulses = 0; |
aberk | 0:9bca35ae9c6b | 49 | volatile int leftPrevPulses = 0; |
aberk | 0:9bca35ae9c6b | 50 | volatile float leftPwmDuty = 1.0; |
aberk | 0:9bca35ae9c6b | 51 | volatile float leftVelocity = 0.0; |
soulx | 2:646e3bca12a8 | 52 | |
soulx | 2:646e3bca12a8 | 53 | volatile int rightPulses = 0; |
soulx | 2:646e3bca12a8 | 54 | volatile int rightPrevPulses = 0; |
soulx | 2:646e3bca12a8 | 55 | volatile float rightPwmDuty = 1.0; |
soulx | 2:646e3bca12a8 | 56 | volatile float rightVelocity = 0.0; |
soulx | 2:646e3bca12a8 | 57 | |
aberk | 0:9bca35ae9c6b | 58 | //Velocity to reach. |
soulx | 2:646e3bca12a8 | 59 | int goal = 50; |
aberk | 0:9bca35ae9c6b | 60 | |
aberk | 0:9bca35ae9c6b | 61 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 62 | // Prototypes |
aberk | 0:9bca35ae9c6b | 63 | //****************************************************************************/ |
aberk | 0:9bca35ae9c6b | 64 | //Set motors to go "forward", brake off, not moving. |
aberk | 0:9bca35ae9c6b | 65 | void initializeMotors(void); |
aberk | 0:9bca35ae9c6b | 66 | //Set up PID controllers with appropriate limits and biases. |
aberk | 0:9bca35ae9c6b | 67 | void initializePidControllers(void); |
aberk | 0:9bca35ae9c6b | 68 | |
soulx | 2:646e3bca12a8 | 69 | void initializeMotors(void) |
soulx | 2:646e3bca12a8 | 70 | { |
aberk | 0:9bca35ae9c6b | 71 | |
aberk | 0:9bca35ae9c6b | 72 | leftMotor.period_us(50); |
aberk | 0:9bca35ae9c6b | 73 | leftMotor = 1.0; |
aberk | 0:9bca35ae9c6b | 74 | leftBrake = 0.0; |
aberk | 0:9bca35ae9c6b | 75 | leftDirection = 0; |
aberk | 0:9bca35ae9c6b | 76 | |
soulx | 2:646e3bca12a8 | 77 | rightMotor.period_us(50); |
soulx | 2:646e3bca12a8 | 78 | rightMotor = 1.0; |
soulx | 2:646e3bca12a8 | 79 | rightBrake = 0.0; |
soulx | 2:646e3bca12a8 | 80 | rightDirection = 0; |
soulx | 2:646e3bca12a8 | 81 | |
aberk | 0:9bca35ae9c6b | 82 | } |
aberk | 0:9bca35ae9c6b | 83 | |
soulx | 2:646e3bca12a8 | 84 | void initializePidControllers(void) |
soulx | 2:646e3bca12a8 | 85 | { |
aberk | 0:9bca35ae9c6b | 86 | |
aberk | 0:9bca35ae9c6b | 87 | leftController.setInputLimits(0.0, 10500.0); |
aberk | 0:9bca35ae9c6b | 88 | leftController.setOutputLimits(0.0, 1.0); |
aberk | 0:9bca35ae9c6b | 89 | leftController.setBias(1.0); |
aberk | 0:9bca35ae9c6b | 90 | leftController.setMode(AUTO_MODE); |
aberk | 0:9bca35ae9c6b | 91 | |
soulx | 2:646e3bca12a8 | 92 | rightController.setInputLimits(0.0, 10500.0); |
soulx | 2:646e3bca12a8 | 93 | rightController.setOutputLimits(0.0, 1.0); |
soulx | 2:646e3bca12a8 | 94 | rightController.setBias(1.0); |
soulx | 2:646e3bca12a8 | 95 | rightController.setMode(AUTO_MODE); |
aberk | 0:9bca35ae9c6b | 96 | } |
aberk | 0:9bca35ae9c6b | 97 | |
soulx | 2:646e3bca12a8 | 98 | int main() |
soulx | 2:646e3bca12a8 | 99 | { |
aberk | 0:9bca35ae9c6b | 100 | |
soulx | 2:646e3bca12a8 | 101 | int updateTime=0; |
soulx | 2:646e3bca12a8 | 102 | |
soulx | 2:646e3bca12a8 | 103 | pc.baud(115200); |
soulx | 2:646e3bca12a8 | 104 | pc.printf("start\n\r"); |
aberk | 0:9bca35ae9c6b | 105 | //Initialization. |
aberk | 0:9bca35ae9c6b | 106 | initializeMotors(); |
soulx | 2:646e3bca12a8 | 107 | |
soulx | 2:646e3bca12a8 | 108 | wait(1); |
soulx | 2:646e3bca12a8 | 109 | /* |
soulx | 2:646e3bca12a8 | 110 | while(1){ |
soulx | 2:646e3bca12a8 | 111 | leftPulses = leftQei.getPulses(); |
soulx | 2:646e3bca12a8 | 112 | rightPulses = rightQei.getPulses(); |
soulx | 2:646e3bca12a8 | 113 | pc.printf("leftPulses = %d\n\r",leftPulses); |
soulx | 2:646e3bca12a8 | 114 | pc.printf("rightPulses = %d\n\r",rightPulses); |
soulx | 2:646e3bca12a8 | 115 | wait(0.5); |
soulx | 2:646e3bca12a8 | 116 | } |
soulx | 2:646e3bca12a8 | 117 | */ |
aberk | 0:9bca35ae9c6b | 118 | initializePidControllers(); |
aberk | 0:9bca35ae9c6b | 119 | |
soulx | 2:646e3bca12a8 | 120 | |
aberk | 0:9bca35ae9c6b | 121 | endTimer.start(); |
aberk | 0:9bca35ae9c6b | 122 | |
aberk | 0:9bca35ae9c6b | 123 | //Set velocity set point. |
aberk | 0:9bca35ae9c6b | 124 | leftController.setSetPoint(goal); |
soulx | 2:646e3bca12a8 | 125 | rightController.setSetPoint(goal); |
aberk | 0:9bca35ae9c6b | 126 | |
aberk | 0:9bca35ae9c6b | 127 | //Run for 3 seconds. |
soulx | 2:646e3bca12a8 | 128 | while (endTimer.read() < 15.0f) { |
soulx | 2:646e3bca12a8 | 129 | |
soulx | 2:646e3bca12a8 | 130 | leftDirection = 1; |
soulx | 2:646e3bca12a8 | 131 | rightDirection = 1; |
soulx | 2:646e3bca12a8 | 132 | |
soulx | 2:646e3bca12a8 | 133 | if( (endTimer.read_ms()-updateTime) > (RATE*1000)) { |
soulx | 2:646e3bca12a8 | 134 | leftPulses = leftQei.getPulses(); |
soulx | 2:646e3bca12a8 | 135 | leftVelocity = (leftPulses - leftPrevPulses) / RATE; |
soulx | 2:646e3bca12a8 | 136 | leftPrevPulses = leftPulses; |
soulx | 2:646e3bca12a8 | 137 | leftController.setProcessValue(leftVelocity); |
soulx | 2:646e3bca12a8 | 138 | leftPwmDuty = leftController.compute(); |
soulx | 2:646e3bca12a8 | 139 | leftMotor = leftPwmDuty; |
soulx | 2:646e3bca12a8 | 140 | |
soulx | 2:646e3bca12a8 | 141 | rightPulses = rightQei.getPulses(); |
soulx | 2:646e3bca12a8 | 142 | rightVelocity = (rightPulses - rightPrevPulses) / RATE; |
soulx | 2:646e3bca12a8 | 143 | rightPrevPulses = rightPulses; |
soulx | 2:646e3bca12a8 | 144 | rightController.setProcessValue(rightVelocity); |
soulx | 2:646e3bca12a8 | 145 | rightPwmDuty = rightController.compute(); |
soulx | 2:646e3bca12a8 | 146 | rightMotor = rightPwmDuty; |
soulx | 2:646e3bca12a8 | 147 | |
soulx | 2:646e3bca12a8 | 148 | updateTime = endTimer.read_ms(); |
soulx | 2:646e3bca12a8 | 149 | |
soulx | 2:646e3bca12a8 | 150 | pc.printf("leftPulses = %d\n\r",leftPulses); |
soulx | 2:646e3bca12a8 | 151 | pc.printf("rightPulses = %d\n\r",rightPulses); |
soulx | 2:646e3bca12a8 | 152 | pc.printf("leftPwm = %f\n\r",leftPwmDuty); |
soulx | 2:646e3bca12a8 | 153 | pc.printf("rightPwm = %f\n\r",rightPwmDuty); |
soulx | 2:646e3bca12a8 | 154 | } |
soulx | 2:646e3bca12a8 | 155 | //wait(RATE); |
aberk | 0:9bca35ae9c6b | 156 | } |
aberk | 0:9bca35ae9c6b | 157 | |
aberk | 0:9bca35ae9c6b | 158 | //Stop motors. |
aberk | 0:9bca35ae9c6b | 159 | leftMotor = 1.0; |
soulx | 2:646e3bca12a8 | 160 | rightMotor =1.0; |
aberk | 0:9bca35ae9c6b | 161 | |
soulx | 2:646e3bca12a8 | 162 | rightDirection = 0; |
soulx | 2:646e3bca12a8 | 163 | leftDirection = 0; |
soulx | 2:646e3bca12a8 | 164 | endTimer.reset(); |
soulx | 2:646e3bca12a8 | 165 | wait(1.0f); |
soulx | 2:646e3bca12a8 | 166 | |
soulx | 2:646e3bca12a8 | 167 | pc.printf("stop\n]r"); |
soulx | 2:646e3bca12a8 | 168 | |
soulx | 2:646e3bca12a8 | 169 | //Set velocity set point. |
soulx | 2:646e3bca12a8 | 170 | leftController.setSetPoint(goal+1000); |
soulx | 2:646e3bca12a8 | 171 | rightController.setSetPoint(goal); |
soulx | 2:646e3bca12a8 | 172 | |
soulx | 2:646e3bca12a8 | 173 | leftDirection = 1; |
soulx | 2:646e3bca12a8 | 174 | rightDirection = 1; |
soulx | 2:646e3bca12a8 | 175 | |
soulx | 2:646e3bca12a8 | 176 | //Run for 2 seconds. |
soulx | 2:646e3bca12a8 | 177 | while (endTimer.read() < 2.0f) { |
aberk | 0:9bca35ae9c6b | 178 | |
soulx | 2:646e3bca12a8 | 179 | if(endTimer.read_ms()-updateTime > RATE*1000) { |
soulx | 2:646e3bca12a8 | 180 | leftPulses = leftQei.getPulses(); |
soulx | 2:646e3bca12a8 | 181 | leftVelocity = (leftPulses - leftPrevPulses) / RATE; |
soulx | 2:646e3bca12a8 | 182 | leftPrevPulses = leftPulses; |
soulx | 2:646e3bca12a8 | 183 | leftController.setProcessValue(leftVelocity); |
soulx | 2:646e3bca12a8 | 184 | leftPwmDuty = leftController.compute(); |
soulx | 2:646e3bca12a8 | 185 | leftMotor = leftPwmDuty; |
soulx | 2:646e3bca12a8 | 186 | |
soulx | 2:646e3bca12a8 | 187 | rightPulses = rightQei.getPulses(); |
soulx | 2:646e3bca12a8 | 188 | rightVelocity = (rightPulses - rightPrevPulses) / RATE; |
soulx | 2:646e3bca12a8 | 189 | rightPrevPulses = rightPulses; |
soulx | 2:646e3bca12a8 | 190 | rightController.setProcessValue(rightVelocity); |
soulx | 2:646e3bca12a8 | 191 | rightPwmDuty = rightController.compute(); |
soulx | 2:646e3bca12a8 | 192 | rightMotor = rightPwmDuty; |
soulx | 2:646e3bca12a8 | 193 | |
soulx | 2:646e3bca12a8 | 194 | updateTime = endTimer.read_ms(); |
soulx | 2:646e3bca12a8 | 195 | |
soulx | 2:646e3bca12a8 | 196 | pc.printf("leftPulses = %d\n\r",leftPulses); |
soulx | 2:646e3bca12a8 | 197 | pc.printf("rightPulses = %d\n\r",rightPulses); |
soulx | 2:646e3bca12a8 | 198 | } |
soulx | 2:646e3bca12a8 | 199 | //wait(RATE); |
soulx | 2:646e3bca12a8 | 200 | } |
soulx | 2:646e3bca12a8 | 201 | |
soulx | 2:646e3bca12a8 | 202 | rightDirection = 0; |
soulx | 2:646e3bca12a8 | 203 | leftDirection = 0; |
soulx | 2:646e3bca12a8 | 204 | |
aberk | 0:9bca35ae9c6b | 205 | } |