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

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

Rover.cpp@0:8d15dc761944, 2010-08-16 (annotated)

Committer:
aberk
Date:
Mon Aug 16 09:46:28 2010 +0000
Revision:
0:8d15dc761944
Child:
1:ffef6386027b
Version 1.0

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 Serial pc2(USBTX, USBRX);
aberk 0:8d15dc761944 65
aberk 0:8d15dc761944 66 Rover::Rover(PinName leftMotorPwm,
aberk 0:8d15dc761944 67 PinName leftMotorBrake,
aberk 0:8d15dc761944 68 PinName leftMotorDirection,
aberk 0:8d15dc761944 69 PinName rightMotorPwm,
aberk 0:8d15dc761944 70 PinName rightMotorBrake,
aberk 0:8d15dc761944 71 PinName rightMotorDirection,
aberk 0:8d15dc761944 72 PinName leftQeiChannelA,
aberk 0:8d15dc761944 73 PinName leftQeiChannelB,
aberk 0:8d15dc761944 74 PinName leftQeiIndex,
aberk 0:8d15dc761944 75 int leftPulsesPerRev,
aberk 0:8d15dc761944 76 PinName rightQeiChannelA,
aberk 0:8d15dc761944 77 PinName rightQeiChannelB,
aberk 0:8d15dc761944 78 PinName rightQeiIndex,
aberk 0:8d15dc761944 79 int rightPulsesPerRev) :
aberk 0:8d15dc761944 80 leftMotors(),
aberk 0:8d15dc761944 81 rightMotors(),
aberk 0:8d15dc761944 82 leftQei(leftQeiChannelA,
aberk 0:8d15dc761944 83 leftQeiChannelB,
aberk 0:8d15dc761944 84 leftQeiIndex,
aberk 0:8d15dc761944 85 leftPulsesPerRev),
aberk 0:8d15dc761944 86 rightQei(rightQeiChannelA,
aberk 0:8d15dc761944 87 rightQeiChannelB,
aberk 0:8d15dc761944 88 rightQeiIndex,
aberk 0:8d15dc761944 89 rightPulsesPerRev),
aberk 0:8d15dc761944 90 leftController(Kc, Ti, Td, PID_RATE),
aberk 0:8d15dc761944 91 rightController(Kc, Ti, Td, PID_RATE),
aberk 0:8d15dc761944 92 stateTicker(),
aberk 0:8d15dc761944 93 logTicker(),
aberk 0:8d15dc761944 94 imu(IMU_RATE,
aberk 0:8d15dc761944 95 GYRO_MEAS_ERROR,
aberk 0:8d15dc761944 96 ACCELEROMETER_RATE,
aberk 0:8d15dc761944 97 GYROSCOPE_RATE) {
aberk 0:8d15dc761944 98
aberk 0:8d15dc761944 99 //---------------------------------
aberk 0:8d15dc761944 100 // Left motors and PID controller.
aberk 0:8d15dc761944 101 //---------------------------------
aberk 0:8d15dc761944 102
aberk 0:8d15dc761944 103 //Motors.
aberk 0:8d15dc761944 104 leftMotors.setPwmPin(leftMotorPwm);
aberk 0:8d15dc761944 105 leftMotors.setBrakePin(leftMotorBrake);
aberk 0:8d15dc761944 106 leftMotors.setDirectionPin(leftMotorDirection);
aberk 0:8d15dc761944 107 leftMotors.initialize();
aberk 0:8d15dc761944 108 leftMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 109 leftMotors.setBrake(BRAKE_OFF);
aberk 0:8d15dc761944 110
aberk 0:8d15dc761944 111 //PID.
aberk 0:8d15dc761944 112 leftController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
aberk 0:8d15dc761944 113 leftController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
aberk 0:8d15dc761944 114 leftController.setBias(PID_BIAS);
aberk 0:8d15dc761944 115 leftController.setMode(AUTO_MODE);
aberk 0:8d15dc761944 116 leftPulses_ = 0;
aberk 0:8d15dc761944 117 leftPrevPulses_ = 0;
aberk 0:8d15dc761944 118 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 119 leftVelocity_ = 0.0;
aberk 0:8d15dc761944 120
aberk 0:8d15dc761944 121 //----------------------------------
aberk 0:8d15dc761944 122 // Right motors and PID controller.
aberk 0:8d15dc761944 123 //----------------------------------
aberk 0:8d15dc761944 124
aberk 0:8d15dc761944 125 //Motors.
aberk 0:8d15dc761944 126 rightMotors.setPwmPin(rightMotorPwm);
aberk 0:8d15dc761944 127 rightMotors.setBrakePin(rightMotorBrake);
aberk 0:8d15dc761944 128 rightMotors.setDirectionPin(rightMotorDirection);
aberk 0:8d15dc761944 129 rightMotors.initialize();
aberk 0:8d15dc761944 130 rightMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 131 rightMotors.setBrake(BRAKE_OFF);
aberk 0:8d15dc761944 132
aberk 0:8d15dc761944 133 //PID.
aberk 0:8d15dc761944 134 rightController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
aberk 0:8d15dc761944 135 rightController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
aberk 0:8d15dc761944 136 rightController.setBias(PID_BIAS);
aberk 0:8d15dc761944 137 rightController.setMode(AUTO_MODE);
aberk 0:8d15dc761944 138 rightPulses_ = 0;
aberk 0:8d15dc761944 139 rightPrevPulses_ = 0;
aberk 0:8d15dc761944 140 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 141 rightVelocity_ = 0.0;
aberk 0:8d15dc761944 142
aberk 0:8d15dc761944 143 //--------------------
aberk 0:8d15dc761944 144 // Working Variables.
aberk 0:8d15dc761944 145 //--------------------
aberk 0:8d15dc761944 146 positionSetPoint_ = 0.0;
aberk 0:8d15dc761944 147 headingSetPoint_ = 0.0;
aberk 0:8d15dc761944 148 heading_ = 0.0;
aberk 0:8d15dc761944 149 prevHeading_ = 0.0;
aberk 0:8d15dc761944 150 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 151 leftStopFlag_ = 0;
aberk 0:8d15dc761944 152 rightStopFlag_ = 0;
aberk 0:8d15dc761944 153
aberk 0:8d15dc761944 154 //--------
aberk 0:8d15dc761944 155 // BEGIN!
aberk 0:8d15dc761944 156 //--------
aberk 0:8d15dc761944 157 state_ = STATE_STATIONARY;
aberk 0:8d15dc761944 158 stateTicker.attach(this, &Rover::doState, PID_RATE);
aberk 0:8d15dc761944 159 startLogging();
aberk 0:8d15dc761944 160
aberk 0:8d15dc761944 161 }
aberk 0:8d15dc761944 162
aberk 0:8d15dc761944 163 void Rover::move(int distance) {
aberk 0:8d15dc761944 164
aberk 0:8d15dc761944 165 positionSetPoint_ = distance;
aberk 0:8d15dc761944 166
aberk 0:8d15dc761944 167 //Moving forward.
aberk 0:8d15dc761944 168 if (distance > 0) {
aberk 0:8d15dc761944 169
aberk 0:8d15dc761944 170 enterState(STATE_MOVING_FORWARD);
aberk 0:8d15dc761944 171
aberk 0:8d15dc761944 172 }
aberk 0:8d15dc761944 173 //Moving backward.
aberk 0:8d15dc761944 174 else if (distance < 0) {
aberk 0:8d15dc761944 175
aberk 0:8d15dc761944 176 enterState(STATE_MOVING_BACKWARD);
aberk 0:8d15dc761944 177
aberk 0:8d15dc761944 178 }
aberk 0:8d15dc761944 179
aberk 0:8d15dc761944 180 //If distance == 0, then do nothing, i.e. stay stationary.
aberk 0:8d15dc761944 181
aberk 0:8d15dc761944 182 }
aberk 0:8d15dc761944 183
aberk 0:8d15dc761944 184 void Rover::turn(int degrees) {
aberk 0:8d15dc761944 185
aberk 0:8d15dc761944 186 headingSetPoint_ = abs(degrees);
aberk 0:8d15dc761944 187
aberk 0:8d15dc761944 188 //Rotating clockwise.
aberk 0:8d15dc761944 189 if (degrees > 0) {
aberk 0:8d15dc761944 190
aberk 0:8d15dc761944 191 enterState(STATE_ROTATING_CLOCKWISE);
aberk 0:8d15dc761944 192
aberk 0:8d15dc761944 193 }
aberk 0:8d15dc761944 194 //Rotating counter-clockwise.
aberk 0:8d15dc761944 195 else if (degrees < 0) {
aberk 0:8d15dc761944 196
aberk 0:8d15dc761944 197 enterState(STATE_ROTATING_COUNTER_CLOCKWISE);
aberk 0:8d15dc761944 198
aberk 0:8d15dc761944 199 }
aberk 0:8d15dc761944 200
aberk 0:8d15dc761944 201 //If degrees == 0, then do nothing, i.e. stay stationary.
aberk 0:8d15dc761944 202
aberk 0:8d15dc761944 203 }
aberk 0:8d15dc761944 204
aberk 0:8d15dc761944 205 Rover::State Rover::getState(void) {
aberk 0:8d15dc761944 206
aberk 0:8d15dc761944 207 return state_;
aberk 0:8d15dc761944 208
aberk 0:8d15dc761944 209 }
aberk 0:8d15dc761944 210
aberk 0:8d15dc761944 211 void Rover::startLogging(void) {
aberk 0:8d15dc761944 212
aberk 0:8d15dc761944 213 logFile = fopen("/sd/roverlog.csv", "w");
aberk 0:8d15dc761944 214 fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
aberk 0:8d15dc761944 215 logTicker.attach(this, &Rover::log, LOG_RATE);
aberk 0:8d15dc761944 216
aberk 0:8d15dc761944 217 }
aberk 0:8d15dc761944 218
aberk 0:8d15dc761944 219 void Rover::stopLogging(void) {
aberk 0:8d15dc761944 220
aberk 0:8d15dc761944 221 fclose(logFile);
aberk 0:8d15dc761944 222
aberk 0:8d15dc761944 223 }
aberk 0:8d15dc761944 224
aberk 0:8d15dc761944 225 void Rover::log(void) {
aberk 0:8d15dc761944 226
aberk 0:8d15dc761944 227 fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
aberk 0:8d15dc761944 228 leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
aberk 0:8d15dc761944 229
aberk 0:8d15dc761944 230 }
aberk 0:8d15dc761944 231
aberk 0:8d15dc761944 232 void Rover::doState(void) {
aberk 0:8d15dc761944 233
aberk 0:8d15dc761944 234 switch (state_) {
aberk 0:8d15dc761944 235
aberk 0:8d15dc761944 236 //We're not moving so don't do anything!
aberk 0:8d15dc761944 237 case (STATE_STATIONARY):
aberk 0:8d15dc761944 238 break;
aberk 0:8d15dc761944 239
aberk 0:8d15dc761944 240 case (STATE_MOVING_FORWARD):
aberk 0:8d15dc761944 241
aberk 0:8d15dc761944 242 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 243 //perform velocity control on the motors.
aberk 0:8d15dc761944 244 if (leftPulses_ < positionSetPoint_) {
aberk 0:8d15dc761944 245
aberk 0:8d15dc761944 246 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 247 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 248 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 249 leftController.setProcessValue(leftVelocity_);
aberk 0:8d15dc761944 250 leftPwmDuty_ = leftController.getRealOutput();
aberk 0:8d15dc761944 251
aberk 0:8d15dc761944 252 } else {
aberk 0:8d15dc761944 253 leftStopFlag_ = 1;
aberk 0:8d15dc761944 254 }
aberk 0:8d15dc761944 255
aberk 0:8d15dc761944 256 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 257
aberk 0:8d15dc761944 258 if (rightPulses_ < positionSetPoint_) {
aberk 0:8d15dc761944 259
aberk 0:8d15dc761944 260 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 261 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 262 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 263 rightController.setProcessValue(rightVelocity_);
aberk 0:8d15dc761944 264 rightPwmDuty_ = rightController.getRealOutput();
aberk 0:8d15dc761944 265
aberk 0:8d15dc761944 266 } else {
aberk 0:8d15dc761944 267 rightStopFlag_ = 1;
aberk 0:8d15dc761944 268 }
aberk 0:8d15dc761944 269
aberk 0:8d15dc761944 270 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 271
aberk 0:8d15dc761944 272 //We've hit the end position set point.
aberk 0:8d15dc761944 273 if (leftStopFlag_ == 1 && rightStopFlag_ == 1) {
aberk 0:8d15dc761944 274 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 275 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 276 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 277 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 278 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 279 }
aberk 0:8d15dc761944 280
aberk 0:8d15dc761944 281 break;
aberk 0:8d15dc761944 282
aberk 0:8d15dc761944 283 case (STATE_MOVING_BACKWARD):
aberk 0:8d15dc761944 284
aberk 0:8d15dc761944 285 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 286 //perform velocity control on the motors.
aberk 0:8d15dc761944 287 if (leftPulses_ > positionSetPoint_) {
aberk 0:8d15dc761944 288 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 289 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 290 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 291 leftController.setProcessValue(fabs(leftVelocity_));
aberk 0:8d15dc761944 292 leftPwmDuty_ = leftController.getRealOutput();
aberk 0:8d15dc761944 293 } else {
aberk 0:8d15dc761944 294 leftStopFlag_ = 1;
aberk 0:8d15dc761944 295 }
aberk 0:8d15dc761944 296
aberk 0:8d15dc761944 297 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 298
aberk 0:8d15dc761944 299 if (rightPulses_ > positionSetPoint_) {
aberk 0:8d15dc761944 300 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 301 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 302 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 303 rightController.setProcessValue(fabs(rightVelocity_));
aberk 0:8d15dc761944 304 rightPwmDuty_ = rightController.getRealOutput();
aberk 0:8d15dc761944 305
aberk 0:8d15dc761944 306 } else {
aberk 0:8d15dc761944 307 rightStopFlag_ = 1;
aberk 0:8d15dc761944 308 }
aberk 0:8d15dc761944 309
aberk 0:8d15dc761944 310 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 311
aberk 0:8d15dc761944 312 //We've hit the end position set point.
aberk 0:8d15dc761944 313 if (leftStopFlag_ == 1.0 && rightStopFlag_ == 1.0) {
aberk 0:8d15dc761944 314 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 315 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 316 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 317 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 318 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 319 }
aberk 0:8d15dc761944 320
aberk 0:8d15dc761944 321 break;
aberk 0:8d15dc761944 322
aberk 0:8d15dc761944 323 case (STATE_ROTATING_CLOCKWISE):
aberk 0:8d15dc761944 324
aberk 0:8d15dc761944 325 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 326 //perform velocity control on the motors.
aberk 0:8d15dc761944 327 if (degreesTurned_ < headingSetPoint_) {
aberk 0:8d15dc761944 328
aberk 0:8d15dc761944 329 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 330 degreesTurned_ += fabs(heading_ - prevHeading_);
aberk 0:8d15dc761944 331 prevHeading_ = heading_;
aberk 0:8d15dc761944 332
aberk 0:8d15dc761944 333 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 334 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 335 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 336 leftController.setProcessValue(leftVelocity_);
aberk 0:8d15dc761944 337 leftPwmDuty_ = leftController.getRealOutput();
aberk 0:8d15dc761944 338
aberk 0:8d15dc761944 339 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 340 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 341 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 342 rightController.setProcessValue(fabs(rightVelocity_));
aberk 0:8d15dc761944 343 rightPwmDuty_ = rightController.getRealOutput();
aberk 0:8d15dc761944 344
aberk 0:8d15dc761944 345 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 346 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 347
aberk 0:8d15dc761944 348 } else {
aberk 0:8d15dc761944 349
aberk 0:8d15dc761944 350 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 351 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 352 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 353 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 354 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 355
aberk 0:8d15dc761944 356 }
aberk 0:8d15dc761944 357
aberk 0:8d15dc761944 358 break;
aberk 0:8d15dc761944 359
aberk 0:8d15dc761944 360 case (STATE_ROTATING_COUNTER_CLOCKWISE):
aberk 0:8d15dc761944 361
aberk 0:8d15dc761944 362 //If we haven't hit the position set point yet,
aberk 0:8d15dc761944 363 //perform velocity control on the motors.
aberk 0:8d15dc761944 364 if (degreesTurned_ < headingSetPoint_) {
aberk 0:8d15dc761944 365
aberk 0:8d15dc761944 366 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 367 degreesTurned_ += fabs(heading_ - prevHeading_);
aberk 0:8d15dc761944 368 prevHeading_ = heading_;
aberk 0:8d15dc761944 369
aberk 0:8d15dc761944 370 leftPulses_ = leftQei.getPulses();
aberk 0:8d15dc761944 371 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 372 leftPrevPulses_ = leftPulses_;
aberk 0:8d15dc761944 373 leftController.setProcessValue(fabs(leftVelocity_));
aberk 0:8d15dc761944 374 leftPwmDuty_ = leftController.getRealOutput();
aberk 0:8d15dc761944 375
aberk 0:8d15dc761944 376 rightPulses_ = rightQei.getPulses();
aberk 0:8d15dc761944 377 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
aberk 0:8d15dc761944 378 rightPrevPulses_ = rightPulses_;
aberk 0:8d15dc761944 379 rightController.setProcessValue(rightVelocity_);
aberk 0:8d15dc761944 380 rightPwmDuty_ = rightController.getRealOutput();
aberk 0:8d15dc761944 381
aberk 0:8d15dc761944 382 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 383 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 384
aberk 0:8d15dc761944 385 } else {
aberk 0:8d15dc761944 386
aberk 0:8d15dc761944 387 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 388 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 389 leftMotors.setPwmDuty(leftPwmDuty_);
aberk 0:8d15dc761944 390 rightMotors.setPwmDuty(rightPwmDuty_);
aberk 0:8d15dc761944 391 enterState(STATE_STATIONARY);
aberk 0:8d15dc761944 392
aberk 0:8d15dc761944 393 }
aberk 0:8d15dc761944 394
aberk 0:8d15dc761944 395 break;
aberk 0:8d15dc761944 396
aberk 0:8d15dc761944 397 //If we've fallen into a black hole, the least we can do is turn
aberk 0:8d15dc761944 398 //the motors off.
aberk 0:8d15dc761944 399 default:
aberk 0:8d15dc761944 400
aberk 0:8d15dc761944 401 leftMotors.setPwmDuty(1.0);
aberk 0:8d15dc761944 402 rightMotors.setPwmDuty(1.0);
aberk 0:8d15dc761944 403
aberk 0:8d15dc761944 404 break;
aberk 0:8d15dc761944 405
aberk 0:8d15dc761944 406 }
aberk 0:8d15dc761944 407
aberk 0:8d15dc761944 408 }
aberk 0:8d15dc761944 409
aberk 0:8d15dc761944 410 void Rover::enterState(State state) {
aberk 0:8d15dc761944 411
aberk 0:8d15dc761944 412 switch (state) {
aberk 0:8d15dc761944 413
aberk 0:8d15dc761944 414 //Entering stationary state.
aberk 0:8d15dc761944 415 //1. Turn motors off.
aberk 0:8d15dc761944 416 //2. Reset QEIs.
aberk 0:8d15dc761944 417 //3. Reset PID working variables.
aberk 0:8d15dc761944 418 //4. Reset PIDs.
aberk 0:8d15dc761944 419 //5. Reset set points and working variables.
aberk 0:8d15dc761944 420 //7. Set state variable.
aberk 0:8d15dc761944 421 case (STATE_STATIONARY):
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 leftQei.reset();
aberk 0:8d15dc761944 427 rightQei.reset();
aberk 0:8d15dc761944 428
aberk 0:8d15dc761944 429 leftPulses_ = 0;
aberk 0:8d15dc761944 430 leftPrevPulses_ = 0;
aberk 0:8d15dc761944 431 leftPwmDuty_ = 1.0;
aberk 0:8d15dc761944 432 leftVelocity_ = 0.0;
aberk 0:8d15dc761944 433
aberk 0:8d15dc761944 434 rightPulses_ = 0;
aberk 0:8d15dc761944 435 rightPrevPulses_ = 0;
aberk 0:8d15dc761944 436 rightPwmDuty_ = 1.0;
aberk 0:8d15dc761944 437 rightVelocity_ = 0.0;
aberk 0:8d15dc761944 438
aberk 0:8d15dc761944 439 leftController.setSetPoint(0.0);
aberk 0:8d15dc761944 440 leftController.setProcessValue(0.0);
aberk 0:8d15dc761944 441 rightController.setSetPoint(0.0);
aberk 0:8d15dc761944 442 rightController.setProcessValue(0.0);
aberk 0:8d15dc761944 443
aberk 0:8d15dc761944 444 positionSetPoint_ = 0.0;
aberk 0:8d15dc761944 445 headingSetPoint_ = 0.0;
aberk 0:8d15dc761944 446 heading_ = 0.0;
aberk 0:8d15dc761944 447 prevHeading_ = 0.0;
aberk 0:8d15dc761944 448 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 449 leftStopFlag_ = 0;
aberk 0:8d15dc761944 450 rightStopFlag_ = 0;
aberk 0:8d15dc761944 451
aberk 0:8d15dc761944 452 state_ = STATE_STATIONARY;
aberk 0:8d15dc761944 453
aberk 0:8d15dc761944 454 break;
aberk 0:8d15dc761944 455
aberk 0:8d15dc761944 456 //Entering moving forward state.
aberk 0:8d15dc761944 457 //1. Set correct direction for motors.
aberk 0:8d15dc761944 458 //2. Set velocity set point.
aberk 0:8d15dc761944 459 //3. Set state variable.
aberk 0:8d15dc761944 460 case (STATE_MOVING_FORWARD):
aberk 0:8d15dc761944 461
aberk 0:8d15dc761944 462 leftMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 463 rightMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 464
aberk 0:8d15dc761944 465 //Velocity control.
aberk 0:8d15dc761944 466 leftController.setSetPoint(1000);
aberk 0:8d15dc761944 467 rightController.setSetPoint(1000);
aberk 0:8d15dc761944 468
aberk 0:8d15dc761944 469 state_ = STATE_MOVING_FORWARD;
aberk 0:8d15dc761944 470
aberk 0:8d15dc761944 471 break;
aberk 0:8d15dc761944 472
aberk 0:8d15dc761944 473 //Entering moving backward state.
aberk 0:8d15dc761944 474 //1. Set correct direction for motors.
aberk 0:8d15dc761944 475 //2. Set velocity set point.
aberk 0:8d15dc761944 476 //3. Set state variable.
aberk 0:8d15dc761944 477 case (STATE_MOVING_BACKWARD):
aberk 0:8d15dc761944 478
aberk 0:8d15dc761944 479 leftMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 480 rightMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 481
aberk 0:8d15dc761944 482 //Velocity control.
aberk 0:8d15dc761944 483 leftController.setSetPoint(1000);
aberk 0:8d15dc761944 484 rightController.setSetPoint(1000);
aberk 0:8d15dc761944 485
aberk 0:8d15dc761944 486 state_ = STATE_MOVING_BACKWARD;
aberk 0:8d15dc761944 487
aberk 0:8d15dc761944 488 break;
aberk 0:8d15dc761944 489
aberk 0:8d15dc761944 490 //Entering rotating clockwise state.
aberk 0:8d15dc761944 491 //1. Set correct direction for motors.
aberk 0:8d15dc761944 492 //2. Set velocity set point.
aberk 0:8d15dc761944 493 //3. Set working variables.
aberk 0:8d15dc761944 494 //4. Set state variable.
aberk 0:8d15dc761944 495 case (STATE_ROTATING_CLOCKWISE):
aberk 0:8d15dc761944 496
aberk 0:8d15dc761944 497 leftMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 498 rightMotors.setDirection(BACKWARD);
aberk 0:8d15dc761944 499
aberk 0:8d15dc761944 500 leftController.setSetPoint(500);
aberk 0:8d15dc761944 501 rightController.setSetPoint(500);
aberk 0:8d15dc761944 502
aberk 0:8d15dc761944 503 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 504 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 505 prevHeading_ = heading_;
aberk 0:8d15dc761944 506
aberk 0:8d15dc761944 507 state_ = STATE_ROTATING_CLOCKWISE;
aberk 0:8d15dc761944 508
aberk 0:8d15dc761944 509 break;
aberk 0:8d15dc761944 510
aberk 0:8d15dc761944 511 //Entering rotating clockwise state.
aberk 0:8d15dc761944 512 //1. Set correct direction for motors.
aberk 0:8d15dc761944 513 //2. Set velocity set point.
aberk 0:8d15dc761944 514 //3. Set working variables.
aberk 0:8d15dc761944 515 //4. Set state variable.
aberk 0:8d15dc761944 516 case (STATE_ROTATING_COUNTER_CLOCKWISE):
aberk 0:8d15dc761944 517
aberk 0:8d15dc761944 518 leftMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 519 rightMotors.setDirection(FORWARD);
aberk 0:8d15dc761944 520
aberk 0:8d15dc761944 521 leftController.setSetPoint(500);
aberk 0:8d15dc761944 522 rightController.setSetPoint(500);
aberk 0:8d15dc761944 523
aberk 0:8d15dc761944 524 degreesTurned_ = 0.0;
aberk 0:8d15dc761944 525 heading_ = fabs(imu.getYaw());
aberk 0:8d15dc761944 526 prevHeading_ = heading_;
aberk 0:8d15dc761944 527
aberk 0:8d15dc761944 528 state_ = STATE_ROTATING_COUNTER_CLOCKWISE;
aberk 0:8d15dc761944 529
aberk 0:8d15dc761944 530 break;
aberk 0:8d15dc761944 531
aberk 0:8d15dc761944 532 default:
aberk 0:8d15dc761944 533
aberk 0:8d15dc761944 534 break;
aberk 0:8d15dc761944 535
aberk 0:8d15dc761944 536 }
aberk 0:8d15dc761944 537
aberk 0:8d15dc761944 538 }