Aaron Berk / Mbed 2 deprecated RS-EDP-RDS-Rover

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

Rover.cpp@1:ffef6386027b, 2010-08-26 (annotated)

Committer:
aberk
Date:
Thu Aug 26 14:41:08 2010 +0000
Revision:
1:ffef6386027b
Parent:
0:8d15dc761944 
Added additional comments and documentation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:8d15dc761944 1 /**
aberk 0:8d15dc761944 2 * @author Aaron Berk
aberk 0:8d15dc761944 3 *
aberk 0:8d15dc761944 4 * @section LICENSE
aberk 0:8d15dc761944 5 *
aberk 0:8d15dc761944 6 * Copyright (c) 2010 ARM Limited
aberk 0:8d15dc761944 7 *
aberk 0:8d15dc761944 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
aberk 0:8d15dc761944 9 * of this software and associated documentation files (the "Software"), to deal
aberk 0:8d15dc761944 10 * in the Software without restriction, including without limitation the rights
aberk 0:8d15dc761944 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
aberk 0:8d15dc761944 12 * copies of the Software, and to permit persons to whom the Software is
aberk 0:8d15dc761944 13 * furnished to do so, subject to the following conditions:
aberk 0:8d15dc761944 14 *
aberk 0:8d15dc761944 15 * The above copyright notice and this permission notice shall be included in
aberk 0:8d15dc761944 16 * all copies or substantial portions of the Software.
aberk 0:8d15dc761944 17 *
aberk 0:8d15dc761944 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
aberk 0:8d15dc761944 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
aberk 0:8d15dc761944 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
aberk 0:8d15dc761944 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
aberk 0:8d15dc761944 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
aberk 0:8d15dc761944 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
aberk 0:8d15dc761944 24 * THE SOFTWARE.
aberk 0:8d15dc761944 25 *
aberk 0:8d15dc761944 26 * @section DESCRIPTION
aberk 0:8d15dc761944 27 *
aberk 0:8d15dc761944 28 * RS-EDP Rover application class.
aberk 0:8d15dc761944 29 *
aberk 0:8d15dc761944 30 * Demonstrates four action states: moving {forward, backward},
aberk 0:8d15dc761944 31 * rotating {clockwise, counter-clockwise}.
aberk 0:8d15dc761944 32 *
aberk 0:8d15dc761944 33 * Logs heading and left and right motor position and velocity data.
aberk 0:8d15dc761944 34 *
aberk 0:8d15dc761944 35 * Performs PID velocity control on the motors.
aberk 0:8d15dc761944 36 *
aberk 0:8d15dc761944 37 * ---------------
aberk 0:8d15dc761944 38 * CONFIGURATION
aberk 0:8d15dc761944 39 * ---------------
aberk 0:8d15dc761944 40 *
aberk 0:8d15dc761944 41 * The set up assumes the H-bridge being used has pins for:
aberk 0:8d15dc761944 42 *
aberk 0:8d15dc761944 43 * - PWM input
aberk 0:8d15dc761944 44 * - Brake
aberk 0:8d15dc761944 45 * - Direction
aberk 0:8d15dc761944 46 *
aberk 0:8d15dc761944 47 * The constructor arguments will need to be changed if a different type of
aberk 0:8d15dc761944 48 * H-bridge is used.
aberk 0:8d15dc761944 49 *
aberk 0:8d15dc761944 50 * The PID controllers are configured using the #defines below.
aberk 0:8d15dc761944 51 *
aberk 0:8d15dc761944 52 * The set up also assumes two quadrature encoders are used, each using the
aberk 0:8d15dc761944 53 * default X2 encoding.
aberk 0:8d15dc761944 54 *
aberk 0:8d15dc761944 55 * The constructor arguments will need to be changed if a different number
aberk 0:8d15dc761944 56 * of encoders are used or if a different encoding is required.
aberk 0:8d15dc761944 57 */
aberk 0:8d15dc761944 58
aberk 0:8d15dc761944 59 /**
aberk 0:8d15dc761944 60 * Includes
aberk 0:8d15dc761944 61 */
aberk 0:8d15dc761944 62 #include "Rover.h"
aberk 0:8d15dc761944 63
aberk 0:8d15dc761944 64 Rover::Rover(PinName leftMotorPwm,
aberk 0:8d15dc761944 65 PinName leftMotorBrake,
aberk 0:8d15dc761944 66 PinName leftMotorDirection,
aberk 0:8d15dc761944 67 PinName rightMotorPwm,
aberk 0:8d15dc761944 68 PinName rightMotorBrake,
aberk 0:8d15dc761944 69 PinName rightMotorDirection,
aberk 0:8d15dc761944 70 PinName leftQeiChannelA,
aberk 0:8d15dc761944 71 PinName leftQeiChannelB,
aberk 0:8d15dc761944 72 PinName leftQeiIndex,
aberk 0:8d15dc761944 73 int leftPulsesPerRev,
aberk 0:8d15dc761944 74 PinName rightQeiChannelA,
aberk 0:8d15dc761944 75 PinName rightQeiChannelB,
aberk 0:8d15dc761944 76 PinName rightQeiIndex,
aberk 0:8d15dc761944 77 int rightPulsesPerRev) :
aberk 0:8d15dc761944 78 leftMotors(),
aberk 0:8d15dc761944 79 rightMotors(),
aberk 0:8d15dc761944 80 leftQei(leftQeiChannelA,
aberk 0:8d15dc761944 81 leftQeiChannelB,
aberk 0:8d15dc761944 82 leftQeiIndex,
aberk 0:8d15dc761944 83 leftPulsesPerRev),
aberk 0:8d15dc761944 84 rightQei(rightQeiChannelA,
aberk 0:8d15dc761944 85 rightQeiChannelB,
aberk 0:8d15dc761944 86 rightQeiIndex,
aberk 0:8d15dc761944 87 rightPulsesPerRev),
aberk 0:8d15dc761944 88 leftController(Kc, Ti, Td, PID_RATE),
aberk 0:8d15dc761944 89 rightController(Kc, Ti, Td, PID_RATE),
aberk 0:8d15dc761944 90 stateTicker(),
aberk 0:8d15dc761944 91 logTicker(),
aberk 1:ffef6386027b 92 imu(IMU_RATE_,
aberk 0:8d15dc761944 93 GYRO_MEAS_ERROR,
aberk 0:8d15dc761944 94 ACCELEROMETER_RATE,
aberk 0:8d15dc761944 95 GYROSCOPE_RATE) {
aberk 0:8d15dc761944 96
aberk 0:8d15dc761944 97 //---------------------------------
aberk 0:8d15dc761944 98 // Left motors and PID controller.
aberk 0:8d15dc761944 99 //---------------------------------
aberk 0:8d15dc761944 100
aberk 0:8d15dc761944 101 //Motors.
aberk 0:8d15dc761944 102 leftMotors.setPwmPin(leftMotorPwm);
aberk 0:8d15dc761944 103 leftMotors.setBrakePin(leftMotorBrake);
aberk 0:8d15dc761944 104 leftMotors.setDirectionPin(leftMotorDirection);
aberk 0:8d15dc761944 105 leftMotors.initialize();
aberk 0:8d15dc761944 106 leftMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 107 leftMotors.setBrake(BRAKE_OFF);
aberk 0:8d15dc761944 108
aberk 0:8d15dc761944 109 //PID.
aberk 0:8d15dc761944 110 leftController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
aberk 0:8d15dc761944 111 leftController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
aberk 0:8d15dc761944 112 leftController.setBias(PID_BIAS);
aberk 0:8d15dc761944 113 leftController.setMode(AUTO_MODE);
aberk 0:8d15dc761944 114 leftPulses_ = 0;
aberk 0:8d15dc761944 115 leftPrevPulses_ = 0;
aberk 0:8d15dc761944 116 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 117 leftVelocity_ = 0.0;
aberk 0:8d15dc761944 118
aberk 0:8d15dc761944 119 //----------------------------------
aberk 0:8d15dc761944 120 // Right motors and PID controller.
aberk 0:8d15dc761944 121 //----------------------------------
aberk 0:8d15dc761944 122
aberk 0:8d15dc761944 123 //Motors.
aberk 0:8d15dc761944 124 rightMotors.setPwmPin(rightMotorPwm);
aberk 0:8d15dc761944 125 rightMotors.setBrakePin(rightMotorBrake);
aberk 0:8d15dc761944 126 rightMotors.setDirectionPin(rightMotorDirection);
aberk 0:8d15dc761944 127 rightMotors.initialize();
aberk 0:8d15dc761944 128 rightMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 129 rightMotors.setBrake(BRAKE_OFF);
aberk 0:8d15dc761944 130
aberk 0:8d15dc761944 131 //PID.
aberk 0:8d15dc761944 132 rightController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
aberk 0:8d15dc761944 133 rightController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
aberk 0:8d15dc761944 134 rightController.setBias(PID_BIAS);
aberk 0:8d15dc761944 135 rightController.setMode(AUTO_MODE);
aberk 0:8d15dc761944 136 rightPulses_ = 0;
aberk 0:8d15dc761944 137 rightPrevPulses_ = 0;
aberk 0:8d15dc761944 138 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 139 rightVelocity_ = 0.0;
aberk 0:8d15dc761944 140
aberk 0:8d15dc761944 141 //--------------------
aberk 0:8d15dc761944 142 // Working Variables.
aberk 0:8d15dc761944 143 //--------------------
aberk 0:8d15dc761944 144 positionSetPoint_ = 0.0;
aberk 0:8d15dc761944 145 headingSetPoint_ = 0.0;
aberk 0:8d15dc761944 146 heading_ = 0.0;
aberk 0:8d15dc761944 147 prevHeading_ = 0.0;
aberk 0:8d15dc761944 148 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 149 leftStopFlag_ = 0;
aberk 0:8d15dc761944 150 rightStopFlag_ = 0;
aberk 1:ffef6386027b 151 logIndex = 0;
aberk 0:8d15dc761944 152
aberk 0:8d15dc761944 153 //--------
aberk 0:8d15dc761944 154 // BEGIN!
aberk 0:8d15dc761944 155 //--------
aberk 0:8d15dc761944 156 state_ = STATE_STATIONARY;
aberk 0:8d15dc761944 157 stateTicker.attach(this, &Rover::doState, PID_RATE);
aberk 0:8d15dc761944 158 startLogging();
aberk 0:8d15dc761944 159
aberk 0:8d15dc761944 160 }
aberk 0:8d15dc761944 161
aberk 1:ffef6386027b 162 void Rover::move(float distance) {
aberk 1:ffef6386027b 163
aberk 1:ffef6386027b 164 //Convert from metres into millimetres.
aberk 1:ffef6386027b 165 distance *= 1000;
aberk 1:ffef6386027b 166 //Work out how many pulses are required to go that many millimetres.
aberk 1:ffef6386027b 167 distance *= PULSES_PER_MM;
aberk 1:ffef6386027b 168 //Make sure we scale the number of pulses according to our encoding method.
aberk 1:ffef6386027b 169 distance /= ENCODING;
aberk 0:8d15dc761944 170
aberk 0:8d15dc761944 171 positionSetPoint_ = distance;
aberk 0:8d15dc761944 172
aberk 0:8d15dc761944 173 //Moving forward.
aberk 0:8d15dc761944 174 if (distance > 0) {
aberk 0:8d15dc761944 175
aberk 0:8d15dc761944 176 enterState(STATE_MOVING_FORWARD);
aberk 0:8d15dc761944 177
aberk 0:8d15dc761944 178 }
aberk 0:8d15dc761944 179 //Moving backward.
aberk 0:8d15dc761944 180 else if (distance < 0) {
aberk 0:8d15dc761944 181
aberk 0:8d15dc761944 182 enterState(STATE_MOVING_BACKWARD);
aberk 0:8d15dc761944 183
aberk 0:8d15dc761944 184 }
aberk 0:8d15dc761944 185
aberk 0:8d15dc761944 186 //If distance == 0, then do nothing, i.e. stay stationary.
aberk 0:8d15dc761944 187
aberk 0:8d15dc761944 188 }
aberk 0:8d15dc761944 189
aberk 0:8d15dc761944 190 void Rover::turn(int degrees) {
aberk 0:8d15dc761944 191
aberk 1:ffef6386027b 192 //Correct the amount to turn based on deviation during last segment.
aberk 1:ffef6386027b 193 headingSetPoint_ = abs(degrees) + (endHeading_ - startHeading_);
aberk 1:ffef6386027b 194
aberk 1:ffef6386027b 195 //In case the rover tries to [pointlessly] turn >360 degrees.
aberk 1:ffef6386027b 196 if (headingSetPoint_ > 359.8){
aberk 1:ffef6386027b 197
aberk 1:ffef6386027b 198 headingSetPoint_ -= 359.8;
aberk 1:ffef6386027b 199
aberk 1:ffef6386027b 200 }
aberk 0:8d15dc761944 201
aberk 0:8d15dc761944 202 //Rotating clockwise.
aberk 0:8d15dc761944 203 if (degrees > 0) {
aberk 0:8d15dc761944 204
aberk 0:8d15dc761944 205 enterState(STATE_ROTATING_CLOCKWISE);
aberk 0:8d15dc761944 206
aberk 0:8d15dc761944 207 }
aberk 0:8d15dc761944 208 //Rotating counter-clockwise.
aberk 0:8d15dc761944 209 else if (degrees < 0) {
aberk 0:8d15dc761944 210
aberk 0:8d15dc761944 211 enterState(STATE_ROTATING_COUNTER_CLOCKWISE);
aberk 0:8d15dc761944 212
aberk 0:8d15dc761944 213 }
aberk 0:8d15dc761944 214
aberk 0:8d15dc761944 215 //If degrees == 0, then do nothing, i.e. stay stationary.
aberk 0:8d15dc761944 216
aberk 0:8d15dc761944 217 }
aberk 0:8d15dc761944 218
aberk 0:8d15dc761944 219 Rover::State Rover::getState(void) {
aberk 0:8d15dc761944 220
aberk 0:8d15dc761944 221 return state_;
aberk 0:8d15dc761944 222
aberk 0:8d15dc761944 223 }
aberk 0:8d15dc761944 224
aberk 0:8d15dc761944 225 void Rover::startLogging(void) {
aberk 0:8d15dc761944 226
aberk 1:ffef6386027b 227 logFile = fopen("/local/roverlog.csv", "w");
aberk 1:ffef6386027b 228 fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading\n");
aberk 1:ffef6386027b 229 //logTicker.attach(this, &Rover::log, LOG_RATE);
aberk 0:8d15dc761944 230
aberk 0:8d15dc761944 231 }
aberk 0:8d15dc761944 232
aberk 0:8d15dc761944 233 void Rover::stopLogging(void) {
aberk 0:8d15dc761944 234
aberk 1:ffef6386027b 235 //logFile = fopen("/local/roverlog.csv", "w");
aberk 1:ffef6386027b 236 //fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
aberk 1:ffef6386027b 237 //fprintf(logFile, "leftVelocity, rightVelocity\n");
aberk 1:ffef6386027b 238 //for(int i = 0; i < 1024; i++){
aberk 1:ffef6386027b 239 // fprintf(logFile, "%f, %f\n", leftVelocityBuffer[i], rightVelocityBuffer[i]);
aberk 1:ffef6386027b 240 //}
aberk 0:8d15dc761944 241 fclose(logFile);
aberk 0:8d15dc761944 242
aberk 0:8d15dc761944 243 }
aberk 0:8d15dc761944 244
aberk 0:8d15dc761944 245 void Rover::log(void) {
aberk 0:8d15dc761944 246
aberk 1:ffef6386027b 247 //fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
aberk 1:ffef6386027b 248 // leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
aberk 0:8d15dc761944 249
aberk 0:8d15dc761944 250 }
aberk 0:8d15dc761944 251
aberk 0:8d15dc761944 252 void Rover::doState(void) {
aberk 0:8d15dc761944 253
aberk 0:8d15dc761944 254 switch (state_) {
aberk 0:8d15dc761944 255
aberk 0:8d15dc761944 256 //We're not moving so don't do anything!
aberk 0:8d15dc761944 257 case (STATE_STATIONARY):
aberk 1:ffef6386027b 258
aberk 0:8d15dc761944 259 break;
aberk 0:8d15dc761944 260
aberk 0:8d15dc761944 261 case (STATE_MOVING_FORWARD):
aberk 0:8d15dc761944 262
aberk 0:8d15dc761944 263 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 264 //perform velocity control on the motors.
aberk 0:8d15dc761944 265 if (leftPulses_ < positionSetPoint_) {
aberk 0:8d15dc761944 266
aberk 0:8d15dc761944 267 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 268 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 269 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 270 leftController.setProcessValue(leftVelocity_);
aberk 1:ffef6386027b 271 leftPwmDuty_ = leftController.compute();
aberk 0:8d15dc761944 272
aberk 0:8d15dc761944 273 } else {
aberk 0:8d15dc761944 274 leftStopFlag_ = 1;
aberk 0:8d15dc761944 275 }
aberk 0:8d15dc761944 276
aberk 0:8d15dc761944 277 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 278
aberk 0:8d15dc761944 279 if (rightPulses_ < positionSetPoint_) {
aberk 0:8d15dc761944 280
aberk 0:8d15dc761944 281 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 282 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 283 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 284 rightController.setProcessValue(rightVelocity_);
aberk 1:ffef6386027b 285 rightPwmDuty_ = rightController.compute();
aberk 1:ffef6386027b 286
aberk 0:8d15dc761944 287 } else {
aberk 0:8d15dc761944 288 rightStopFlag_ = 1;
aberk 0:8d15dc761944 289 }
aberk 0:8d15dc761944 290
aberk 0:8d15dc761944 291 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 292
aberk 0:8d15dc761944 293 //We've hit the end position set point.
aberk 0:8d15dc761944 294 if (leftStopFlag_ == 1 && rightStopFlag_ == 1) {
aberk 0:8d15dc761944 295 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 296 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 297 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 298 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 1:ffef6386027b 299 endHeading_ = imu.getYaw();
aberk 0:8d15dc761944 300 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 301 }
aberk 0:8d15dc761944 302
aberk 0:8d15dc761944 303 break;
aberk 0:8d15dc761944 304
aberk 0:8d15dc761944 305 case (STATE_MOVING_BACKWARD):
aberk 0:8d15dc761944 306
aberk 0:8d15dc761944 307 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 308 //perform velocity control on the motors.
aberk 0:8d15dc761944 309 if (leftPulses_ > positionSetPoint_) {
aberk 0:8d15dc761944 310 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 311 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 312 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 313 leftController.setProcessValue(fabs(leftVelocity_));
aberk 1:ffef6386027b 314 leftPwmDuty_ = leftController.compute();
aberk 0:8d15dc761944 315 } else {
aberk 0:8d15dc761944 316 leftStopFlag_ = 1;
aberk 0:8d15dc761944 317 }
aberk 0:8d15dc761944 318
aberk 0:8d15dc761944 319 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 320
aberk 0:8d15dc761944 321 if (rightPulses_ > positionSetPoint_) {
aberk 0:8d15dc761944 322 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 323 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 324 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 325 rightController.setProcessValue(fabs(rightVelocity_));
aberk 1:ffef6386027b 326 rightPwmDuty_ = rightController.compute();
aberk 0:8d15dc761944 327
aberk 0:8d15dc761944 328 } else {
aberk 0:8d15dc761944 329 rightStopFlag_ = 1;
aberk 0:8d15dc761944 330 }
aberk 0:8d15dc761944 331
aberk 0:8d15dc761944 332 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 333
aberk 0:8d15dc761944 334 //We've hit the end position set point.
aberk 0:8d15dc761944 335 if (leftStopFlag_ == 1.0 && rightStopFlag_ == 1.0) {
aberk 0:8d15dc761944 336 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 337 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 338 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 339 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 340 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 341 }
aberk 0:8d15dc761944 342
aberk 0:8d15dc761944 343 break;
aberk 0:8d15dc761944 344
aberk 0:8d15dc761944 345 case (STATE_ROTATING_CLOCKWISE):
aberk 0:8d15dc761944 346
aberk 0:8d15dc761944 347 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 348 //perform velocity control on the motors.
aberk 0:8d15dc761944 349 if (degreesTurned_ < headingSetPoint_) {
aberk 0:8d15dc761944 350
aberk 0:8d15dc761944 351 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 352 degreesTurned_ += fabs(heading_ - prevHeading_);
aberk 0:8d15dc761944 353 prevHeading_ = heading_;
aberk 0:8d15dc761944 354
aberk 0:8d15dc761944 355 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 356 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 357 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 358 leftController.setProcessValue(leftVelocity_);
aberk 1:ffef6386027b 359 leftPwmDuty_ = leftController.compute();
aberk 0:8d15dc761944 360
aberk 0:8d15dc761944 361 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 362 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 363 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 364 rightController.setProcessValue(fabs(rightVelocity_));
aberk 1:ffef6386027b 365 rightPwmDuty_ = rightController.compute();
aberk 0:8d15dc761944 366
aberk 0:8d15dc761944 367 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 368 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 369
aberk 0:8d15dc761944 370 } else {
aberk 0:8d15dc761944 371
aberk 0:8d15dc761944 372 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 373 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 374 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 375 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 376 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 377
aberk 0:8d15dc761944 378 }
aberk 0:8d15dc761944 379
aberk 0:8d15dc761944 380 break;
aberk 0:8d15dc761944 381
aberk 0:8d15dc761944 382 case (STATE_ROTATING_COUNTER_CLOCKWISE):
aberk 0:8d15dc761944 383
aberk 0:8d15dc761944 384 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 385 //perform velocity control on the motors.
aberk 0:8d15dc761944 386 if (degreesTurned_ < headingSetPoint_) {
aberk 0:8d15dc761944 387
aberk 0:8d15dc761944 388 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 389 degreesTurned_ += fabs(heading_ - prevHeading_);
aberk 0:8d15dc761944 390 prevHeading_ = heading_;
aberk 0:8d15dc761944 391
aberk 0:8d15dc761944 392 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 393 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 394 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 395 leftController.setProcessValue(fabs(leftVelocity_));
aberk 1:ffef6386027b 396 leftPwmDuty_ = leftController.compute();
aberk 0:8d15dc761944 397
aberk 0:8d15dc761944 398 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 399 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 400 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 401 rightController.setProcessValue(rightVelocity_);
aberk 1:ffef6386027b 402 rightPwmDuty_ = rightController.compute();
aberk 0:8d15dc761944 403
aberk 0:8d15dc761944 404 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 405 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 406
aberk 0:8d15dc761944 407 } else {
aberk 0:8d15dc761944 408
aberk 0:8d15dc761944 409 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 410 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 411 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 412 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 413 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 414
aberk 0:8d15dc761944 415 }
aberk 0:8d15dc761944 416
aberk 0:8d15dc761944 417 break;
aberk 0:8d15dc761944 418
aberk 0:8d15dc761944 419 //If we've fallen into a black hole, the least we can do is turn
aberk 0:8d15dc761944 420 //the motors off.
aberk 0:8d15dc761944 421 default:
aberk 0:8d15dc761944 422
aberk 0:8d15dc761944 423 leftMotors.setPwmDuty(1.0);
aberk 0:8d15dc761944 424 rightMotors.setPwmDuty(1.0);
aberk 0:8d15dc761944 425
aberk 0:8d15dc761944 426 break;
aberk 0:8d15dc761944 427
aberk 0:8d15dc761944 428 }
aberk 0:8d15dc761944 429
aberk 1:ffef6386027b 430 if (logIndex < 1024) {
aberk 1:ffef6386027b 431 headingBuffer[logIndex] = imu.getYaw();
aberk 1:ffef6386027b 432 leftVelocityBuffer[logIndex] = leftVelocity_;
aberk 1:ffef6386027b 433 rightVelocityBuffer[logIndex] = rightVelocity_;
aberk 1:ffef6386027b 434 leftPositionBuffer[logIndex] = leftPulses_;
aberk 1:ffef6386027b 435 rightPositionBuffer[logIndex] = rightPulses_;
aberk 1:ffef6386027b 436 logIndex++;
aberk 1:ffef6386027b 437 }
aberk 1:ffef6386027b 438
aberk 0:8d15dc761944 439 }
aberk 0:8d15dc761944 440
aberk 0:8d15dc761944 441 void Rover::enterState(State state) {
aberk 0:8d15dc761944 442
aberk 0:8d15dc761944 443 switch (state) {
aberk 0:8d15dc761944 444
aberk 0:8d15dc761944 445 //Entering stationary state.
aberk 0:8d15dc761944 446 //1. Turn motors off.
aberk 0:8d15dc761944 447 //2. Reset QEIs.
aberk 0:8d15dc761944 448 //3. Reset PID working variables.
aberk 0:8d15dc761944 449 //4. Reset PIDs.
aberk 0:8d15dc761944 450 //5. Reset set points and working variables.
aberk 0:8d15dc761944 451 //7. Set state variable.
aberk 0:8d15dc761944 452 case (STATE_STATIONARY):
aberk 0:8d15dc761944 453
aberk 0:8d15dc761944 454 leftMotors.setPwmDuty(1.0);
aberk 0:8d15dc761944 455 rightMotors.setPwmDuty(1.0);
aberk 0:8d15dc761944 456
aberk 0:8d15dc761944 457 leftQei.reset();
aberk 0:8d15dc761944 458 rightQei.reset();
aberk 0:8d15dc761944 459
aberk 0:8d15dc761944 460 leftPulses_ = 0;
aberk 0:8d15dc761944 461 leftPrevPulses_ = 0;
aberk 0:8d15dc761944 462 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 463 leftVelocity_ = 0.0;
aberk 0:8d15dc761944 464
aberk 0:8d15dc761944 465 rightPulses_ = 0;
aberk 0:8d15dc761944 466 rightPrevPulses_ = 0;
aberk 0:8d15dc761944 467 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 468 rightVelocity_ = 0.0;
aberk 0:8d15dc761944 469
aberk 0:8d15dc761944 470 leftController.setSetPoint(0.0);
aberk 0:8d15dc761944 471 leftController.setProcessValue(0.0);
aberk 0:8d15dc761944 472 rightController.setSetPoint(0.0);
aberk 0:8d15dc761944 473 rightController.setProcessValue(0.0);
aberk 0:8d15dc761944 474
aberk 0:8d15dc761944 475 positionSetPoint_ = 0.0;
aberk 0:8d15dc761944 476 headingSetPoint_ = 0.0;
aberk 0:8d15dc761944 477 heading_ = 0.0;
aberk 0:8d15dc761944 478 prevHeading_ = 0.0;
aberk 0:8d15dc761944 479 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 480 leftStopFlag_ = 0;
aberk 0:8d15dc761944 481 rightStopFlag_ = 0;
aberk 0:8d15dc761944 482
aberk 1:ffef6386027b 483 for (int i = 0; i < logIndex; i++) {
aberk 1:ffef6386027b 484 fprintf(logFile, "%i, %i, %f, %f, %f\n", leftPositionBuffer[i],
aberk 1:ffef6386027b 485 rightPositionBuffer[i],
aberk 1:ffef6386027b 486 leftVelocityBuffer[i],
aberk 1:ffef6386027b 487 rightVelocityBuffer[i],
aberk 1:ffef6386027b 488 headingBuffer[i]);
aberk 1:ffef6386027b 489 }
aberk 1:ffef6386027b 490
aberk 1:ffef6386027b 491 logIndex = 0;
aberk 1:ffef6386027b 492
aberk 1:ffef6386027b 493 imu.reset();
aberk 1:ffef6386027b 494
aberk 0:8d15dc761944 495 state_ = STATE_STATIONARY;
aberk 0:8d15dc761944 496
aberk 0:8d15dc761944 497 break;
aberk 0:8d15dc761944 498
aberk 0:8d15dc761944 499 //Entering moving forward state.
aberk 0:8d15dc761944 500 //1. Set correct direction for motors.
aberk 0:8d15dc761944 501 //2. Set velocity set point.
aberk 0:8d15dc761944 502 //3. Set state variable.
aberk 0:8d15dc761944 503 case (STATE_MOVING_FORWARD):
aberk 0:8d15dc761944 504
aberk 0:8d15dc761944 505 leftMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 506 rightMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 507
aberk 0:8d15dc761944 508 //Velocity control.
aberk 0:8d15dc761944 509 leftController.setSetPoint(1000);
aberk 0:8d15dc761944 510 rightController.setSetPoint(1000);
aberk 0:8d15dc761944 511
aberk 1:ffef6386027b 512 logIndex = 0;
aberk 1:ffef6386027b 513
aberk 1:ffef6386027b 514 startHeading_ = imu.getYaw();
aberk 1:ffef6386027b 515
aberk 0:8d15dc761944 516 state_ = STATE_MOVING_FORWARD;
aberk 0:8d15dc761944 517
aberk 0:8d15dc761944 518 break;
aberk 0:8d15dc761944 519
aberk 0:8d15dc761944 520 //Entering moving backward state.
aberk 0:8d15dc761944 521 //1. Set correct direction for motors.
aberk 0:8d15dc761944 522 //2. Set velocity set point.
aberk 0:8d15dc761944 523 //3. Set state variable.
aberk 0:8d15dc761944 524 case (STATE_MOVING_BACKWARD):
aberk 0:8d15dc761944 525
aberk 0:8d15dc761944 526 leftMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 527 rightMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 528
aberk 0:8d15dc761944 529 //Velocity control.
aberk 0:8d15dc761944 530 leftController.setSetPoint(1000);
aberk 0:8d15dc761944 531 rightController.setSetPoint(1000);
aberk 0:8d15dc761944 532
aberk 1:ffef6386027b 533 logIndex = 0;
aberk 1:ffef6386027b 534
aberk 0:8d15dc761944 535 state_ = STATE_MOVING_BACKWARD;
aberk 0:8d15dc761944 536
aberk 0:8d15dc761944 537 break;
aberk 0:8d15dc761944 538
aberk 0:8d15dc761944 539 //Entering rotating clockwise state.
aberk 0:8d15dc761944 540 //1. Set correct direction for motors.
aberk 0:8d15dc761944 541 //2. Set velocity set point.
aberk 0:8d15dc761944 542 //3. Set working variables.
aberk 0:8d15dc761944 543 //4. Set state variable.
aberk 0:8d15dc761944 544 case (STATE_ROTATING_CLOCKWISE):
aberk 0:8d15dc761944 545
aberk 0:8d15dc761944 546 leftMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 547 rightMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 548
aberk 0:8d15dc761944 549 leftController.setSetPoint(500);
aberk 0:8d15dc761944 550 rightController.setSetPoint(500);
aberk 0:8d15dc761944 551
aberk 0:8d15dc761944 552 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 553 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 554 prevHeading_ = heading_;
aberk 0:8d15dc761944 555
aberk 1:ffef6386027b 556 logIndex = 0;
aberk 1:ffef6386027b 557
aberk 0:8d15dc761944 558 state_ = STATE_ROTATING_CLOCKWISE;
aberk 0:8d15dc761944 559
aberk 0:8d15dc761944 560 break;
aberk 0:8d15dc761944 561
aberk 0:8d15dc761944 562 //Entering rotating clockwise state.
aberk 0:8d15dc761944 563 //1. Set correct direction for motors.
aberk 0:8d15dc761944 564 //2. Set velocity set point.
aberk 0:8d15dc761944 565 //3. Set working variables.
aberk 0:8d15dc761944 566 //4. Set state variable.
aberk 0:8d15dc761944 567 case (STATE_ROTATING_COUNTER_CLOCKWISE):
aberk 0:8d15dc761944 568
aberk 0:8d15dc761944 569 leftMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 570 rightMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 571
aberk 0:8d15dc761944 572 leftController.setSetPoint(500);
aberk 0:8d15dc761944 573 rightController.setSetPoint(500);
aberk 0:8d15dc761944 574
aberk 0:8d15dc761944 575 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 576 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 577 prevHeading_ = heading_;
aberk 0:8d15dc761944 578
aberk 1:ffef6386027b 579 logIndex = 0;
aberk 1:ffef6386027b 580
aberk 0:8d15dc761944 581 state_ = STATE_ROTATING_COUNTER_CLOCKWISE;
aberk 0:8d15dc761944 582
aberk 0:8d15dc761944 583 break;
aberk 0:8d15dc761944 584
aberk 0:8d15dc761944 585 default:
aberk 0:8d15dc761944 586
aberk 1:ffef6386027b 587 state_ = STATE_STATIONARY;
aberk 1:ffef6386027b 588
aberk 0:8d15dc761944 589 break;
aberk 0:8d15dc761944 590
aberk 0:8d15dc761944 591 }
aberk 0:8d15dc761944 592
aberk 0:8d15dc761944 593 }