Betago / Mbed 2 deprecated PID_VelocityExample

velocity control

Dependencies:   PID QEI mbed

Fork of PID_VelocityExample by Aaron Berk

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?

UserRevisionLine numberNew 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 }