Rover.cpp

00001 /**
00002  * @author Aaron Berk
00003  *
00004  * @section LICENSE
00005  *
00006  * Copyright (c) 2010 ARM Limited
00007  *
00008  * Permission is hereby granted, free of charge, to any person obtaining a copy
00009  * of this software and associated documentation files (the "Software"), to deal
00010  * in the Software without restriction, including without limitation the rights
00011  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00012  * copies of the Software, and to permit persons to whom the Software is
00013  * furnished to do so, subject to the following conditions:
00014  *
00015  * The above copyright notice and this permission notice shall be included in
00016  * all copies or substantial portions of the Software.
00017  *
00018  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00019  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00020  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00021  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00022  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00023  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00024  * THE SOFTWARE.
00025  *
00026  * @section DESCRIPTION
00027  *
00028  * RS-EDP Rover application class.
00029  *
00030  * Demonstrates four action states: moving {forward, backward},
00031  *                                  rotating {clockwise, counter-clockwise}.
00032  *
00033  * Logs heading and left and right motor position and velocity data.
00034  *
00035  * Performs PID velocity control on the motors.
00036  *
00037  * ---------------
00038  *  CONFIGURATION
00039  * ---------------
00040  *
00041  * The set up assumes the H-bridge being used has pins for:
00042  *
00043  * - PWM input
00044  * - Brake
00045  * - Direction
00046  *
00047  * The constructor arguments will need to be changed if a different type of
00048  * H-bridge is used.
00049  *
00050  * The PID controllers are configured using the #defines below.
00051  *
00052  * The set up also assumes two quadrature encoders are used, each using the
00053  * default X2 encoding.
00054  *
00055  * The constructor arguments will need to be changed if a different number
00056  * of encoders are used or if a different encoding is required.
00057  */
00058 
00059 /**
00060  * Includes
00061  */
00062 #include "Rover.h"
00063 
00064 Rover::Rover(PinName leftMotorPwm,
00065              PinName leftMotorBrake,
00066              PinName leftMotorDirection,
00067              PinName rightMotorPwm,
00068              PinName rightMotorBrake,
00069              PinName rightMotorDirection,
00070              PinName leftQeiChannelA,
00071              PinName leftQeiChannelB,
00072              PinName leftQeiIndex,
00073              int leftPulsesPerRev,
00074              PinName rightQeiChannelA,
00075              PinName rightQeiChannelB,
00076              PinName rightQeiIndex,
00077              int rightPulsesPerRev) :
00078         leftMotors(),
00079         rightMotors(),
00080         leftQei(leftQeiChannelA,
00081                 leftQeiChannelB,
00082                 leftQeiIndex,
00083                 leftPulsesPerRev),
00084         rightQei(rightQeiChannelA,
00085                  rightQeiChannelB,
00086                  rightQeiIndex,
00087                  rightPulsesPerRev),
00088         leftController(Kc, Ti, Td, PID_RATE),
00089         rightController(Kc, Ti, Td, PID_RATE),
00090         stateTicker(),
00091         logTicker(),
00092         imu(IMU_RATE_,
00093             GYRO_MEAS_ERROR,
00094             ACCELEROMETER_RATE,
00095             GYROSCOPE_RATE) {
00096 
00097     //---------------------------------
00098     // Left motors and PID controller.
00099     //---------------------------------
00100 
00101     //Motors.
00102     leftMotors.setPwmPin(leftMotorPwm);
00103     leftMotors.setBrakePin(leftMotorBrake);
00104     leftMotors.setDirectionPin(leftMotorDirection);
00105     leftMotors.initialize();
00106     leftMotors.setDirection(BACKWARD);
00107     leftMotors.setBrake(BRAKE_OFF);
00108 
00109     //PID.
00110     leftController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
00111     leftController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
00112     leftController.setBias(PID_BIAS);
00113     leftController.setMode(AUTO_MODE);
00114     leftPulses_     = 0;
00115     leftPrevPulses_ = 0;
00116     leftPwmDuty_    = 1.0;
00117     leftVelocity_   = 0.0;
00118 
00119     //----------------------------------
00120     // Right motors and PID controller.
00121     //----------------------------------
00122 
00123     //Motors.
00124     rightMotors.setPwmPin(rightMotorPwm);
00125     rightMotors.setBrakePin(rightMotorBrake);
00126     rightMotors.setDirectionPin(rightMotorDirection);
00127     rightMotors.initialize();
00128     rightMotors.setDirection(FORWARD);
00129     rightMotors.setBrake(BRAKE_OFF);
00130 
00131     //PID.
00132     rightController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
00133     rightController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
00134     rightController.setBias(PID_BIAS);
00135     rightController.setMode(AUTO_MODE);
00136     rightPulses_     = 0;
00137     rightPrevPulses_ = 0;
00138     rightPwmDuty_    = 1.0;
00139     rightVelocity_   = 0.0;
00140 
00141     //--------------------
00142     // Working Variables.
00143     //--------------------
00144     positionSetPoint_ = 0.0;
00145     headingSetPoint_  = 0.0;
00146     heading_          = 0.0;
00147     prevHeading_      = 0.0;
00148     degreesTurned_    = 0.0;
00149     leftStopFlag_     = 0;
00150     rightStopFlag_    = 0;
00151     logIndex          = 0;
00152 
00153     //--------
00154     // BEGIN!
00155     //--------
00156     state_ = STATE_STATIONARY;
00157     stateTicker.attach(this, &Rover::doState, PID_RATE);
00158     startLogging();
00159 
00160 }
00161 
00162 void Rover::move(float distance) {
00163 
00164     //Convert from metres into millimetres.
00165     distance *= 1000;
00166     //Work out how many pulses are required to go that many millimetres.
00167     distance *= PULSES_PER_MM;
00168     //Make sure we scale the number of pulses according to our encoding method.
00169     distance /= ENCODING;
00170 
00171     positionSetPoint_ = distance;
00172 
00173     //Moving forward.
00174     if (distance > 0) {
00175 
00176         enterState(STATE_MOVING_FORWARD);
00177 
00178     }
00179     //Moving backward.
00180     else if (distance < 0) {
00181 
00182         enterState(STATE_MOVING_BACKWARD);
00183 
00184     }
00185 
00186     //If distance == 0, then do nothing, i.e. stay stationary.
00187 
00188 }
00189 
00190 void Rover::turn(int degrees) {
00191 
00192     //Correct the amount to turn based on deviation during last segment.
00193     headingSetPoint_ = abs(degrees) + (endHeading_ - startHeading_);
00194     
00195     //In case the rover tries to [pointlessly] turn >360 degrees.
00196     if (headingSetPoint_ > 359.8){
00197     
00198         headingSetPoint_ -= 359.8;
00199     
00200     }
00201 
00202     //Rotating clockwise.
00203     if (degrees > 0) {
00204 
00205         enterState(STATE_ROTATING_CLOCKWISE);
00206 
00207     }
00208     //Rotating counter-clockwise.
00209     else if (degrees < 0) {
00210 
00211         enterState(STATE_ROTATING_COUNTER_CLOCKWISE);
00212 
00213     }
00214 
00215     //If degrees == 0, then do nothing, i.e. stay stationary.
00216 
00217 }
00218 
00219 Rover::State Rover::getState(void) {
00220 
00221     return state_;
00222 
00223 }
00224 
00225 void Rover::startLogging(void) {
00226 
00227     logFile = fopen("/local/roverlog.csv", "w");
00228     fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading\n");
00229     //logTicker.attach(this, &Rover::log, LOG_RATE);
00230 
00231 }
00232 
00233 void Rover::stopLogging(void) {
00234 
00235     //logFile = fopen("/local/roverlog.csv", "w");
00236     //fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
00237     //fprintf(logFile, "leftVelocity, rightVelocity\n");
00238     //for(int i = 0; i < 1024; i++){
00239     //    fprintf(logFile, "%f, %f\n", leftVelocityBuffer[i], rightVelocityBuffer[i]);
00240     //}
00241     fclose(logFile);
00242 
00243 }
00244 
00245 void Rover::log(void) {
00246 
00247     //fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
00248     //        leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
00249 
00250 }
00251 
00252 void Rover::doState(void) {
00253 
00254     switch (state_) {
00255 
00256             //We're not moving so don't do anything!
00257         case (STATE_STATIONARY):
00258 
00259             break;
00260 
00261         case (STATE_MOVING_FORWARD):
00262 
00263             //If we haven't hit the position set point yet,
00264             //perform velocity control on the motors.
00265             if (leftPulses_ < positionSetPoint_) {
00266 
00267                 leftPulses_ = leftQei.getPulses();
00268                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
00269                 leftPrevPulses_ = leftPulses_;
00270                 leftController.setProcessValue(leftVelocity_);
00271                 leftPwmDuty_ = leftController.compute();
00272 
00273             } else {
00274                 leftStopFlag_ = 1;
00275             }
00276 
00277             leftMotors.setPwmDuty(leftPwmDuty_);
00278 
00279             if (rightPulses_ < positionSetPoint_) {
00280 
00281                 rightPulses_ = rightQei.getPulses();
00282                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
00283                 rightPrevPulses_ = rightPulses_;
00284                 rightController.setProcessValue(rightVelocity_);
00285                 rightPwmDuty_ = rightController.compute();
00286 
00287             } else {
00288                 rightStopFlag_ = 1;
00289             }
00290 
00291             rightMotors.setPwmDuty(rightPwmDuty_);
00292 
00293             //We've hit the end position set point.
00294             if (leftStopFlag_ == 1 && rightStopFlag_ == 1) {
00295                 leftPwmDuty_  = 1.0;
00296                 rightPwmDuty_ = 1.0;
00297                 leftMotors.setPwmDuty(leftPwmDuty_);
00298                 rightMotors.setPwmDuty(rightPwmDuty_);
00299                 endHeading_ = imu.getYaw();
00300                 enterState(STATE_STATIONARY);
00301             }
00302 
00303             break;
00304 
00305         case (STATE_MOVING_BACKWARD):
00306 
00307             //If we haven't hit the position set point yet,
00308             //perform velocity control on the motors.
00309             if (leftPulses_ > positionSetPoint_) {
00310                 leftPulses_ = leftQei.getPulses();
00311                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
00312                 leftPrevPulses_ = leftPulses_;
00313                 leftController.setProcessValue(fabs(leftVelocity_));
00314                 leftPwmDuty_ = leftController.compute();
00315             } else {
00316                 leftStopFlag_ = 1;
00317             }
00318 
00319             leftMotors.setPwmDuty(leftPwmDuty_);
00320 
00321             if (rightPulses_ > positionSetPoint_) {
00322                 rightPulses_ = rightQei.getPulses();
00323                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
00324                 rightPrevPulses_ = rightPulses_;
00325                 rightController.setProcessValue(fabs(rightVelocity_));
00326                 rightPwmDuty_ = rightController.compute();
00327 
00328             } else {
00329                 rightStopFlag_ = 1;
00330             }
00331 
00332             rightMotors.setPwmDuty(rightPwmDuty_);
00333 
00334             //We've hit the end position set point.
00335             if (leftStopFlag_ == 1.0 && rightStopFlag_ == 1.0) {
00336                 leftPwmDuty_  = 1.0;
00337                 rightPwmDuty_ = 1.0;
00338                 leftMotors.setPwmDuty(leftPwmDuty_);
00339                 rightMotors.setPwmDuty(rightPwmDuty_);
00340                 enterState(STATE_STATIONARY);
00341             }
00342 
00343             break;
00344 
00345         case (STATE_ROTATING_CLOCKWISE):
00346 
00347             //If we haven't hit the position set point yet,
00348             //perform velocity control on the motors.
00349             if (degreesTurned_ < headingSetPoint_) {
00350 
00351                 heading_ = fabs(imu.getYaw());
00352                 degreesTurned_ += fabs(heading_ - prevHeading_);
00353                 prevHeading_ = heading_;
00354 
00355                 leftPulses_ = leftQei.getPulses();
00356                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
00357                 leftPrevPulses_ = leftPulses_;
00358                 leftController.setProcessValue(leftVelocity_);
00359                 leftPwmDuty_ = leftController.compute();
00360 
00361                 rightPulses_ = rightQei.getPulses();
00362                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
00363                 rightPrevPulses_ = rightPulses_;
00364                 rightController.setProcessValue(fabs(rightVelocity_));
00365                 rightPwmDuty_ = rightController.compute();
00366 
00367                 leftMotors.setPwmDuty(leftPwmDuty_);
00368                 rightMotors.setPwmDuty(rightPwmDuty_);
00369 
00370             } else {
00371 
00372                 leftPwmDuty_  = 1.0;
00373                 rightPwmDuty_ = 1.0;
00374                 leftMotors.setPwmDuty(leftPwmDuty_);
00375                 rightMotors.setPwmDuty(rightPwmDuty_);
00376                 enterState(STATE_STATIONARY);
00377 
00378             }
00379 
00380             break;
00381 
00382         case (STATE_ROTATING_COUNTER_CLOCKWISE):
00383 
00384             //If we haven't hit the position set point yet,
00385             //perform velocity control on the motors.
00386             if (degreesTurned_ < headingSetPoint_) {
00387 
00388                 heading_ = fabs(imu.getYaw());
00389                 degreesTurned_ += fabs(heading_ - prevHeading_);
00390                 prevHeading_ = heading_;
00391 
00392                 leftPulses_ = leftQei.getPulses();
00393                 leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
00394                 leftPrevPulses_ = leftPulses_;
00395                 leftController.setProcessValue(fabs(leftVelocity_));
00396                 leftPwmDuty_ = leftController.compute();
00397 
00398                 rightPulses_ = rightQei.getPulses();
00399                 rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
00400                 rightPrevPulses_ = rightPulses_;
00401                 rightController.setProcessValue(rightVelocity_);
00402                 rightPwmDuty_ = rightController.compute();
00403 
00404                 leftMotors.setPwmDuty(leftPwmDuty_);
00405                 rightMotors.setPwmDuty(rightPwmDuty_);
00406 
00407             } else {
00408 
00409                 leftPwmDuty_  = 1.0;
00410                 rightPwmDuty_ = 1.0;
00411                 leftMotors.setPwmDuty(leftPwmDuty_);
00412                 rightMotors.setPwmDuty(rightPwmDuty_);
00413                 enterState(STATE_STATIONARY);
00414 
00415             }
00416 
00417             break;
00418 
00419             //If we've fallen into a black hole, the least we can do is turn
00420             //the motors off.
00421         default:
00422 
00423             leftMotors.setPwmDuty(1.0);
00424             rightMotors.setPwmDuty(1.0);
00425 
00426             break;
00427 
00428     }
00429 
00430     if (logIndex < 1024) {
00431         headingBuffer[logIndex] = imu.getYaw();
00432         leftVelocityBuffer[logIndex] = leftVelocity_;
00433         rightVelocityBuffer[logIndex] = rightVelocity_;
00434         leftPositionBuffer[logIndex] = leftPulses_;
00435         rightPositionBuffer[logIndex] = rightPulses_;
00436         logIndex++;
00437     }
00438 
00439 }
00440 
00441 void Rover::enterState(State state) {
00442 
00443     switch (state) {
00444 
00445             //Entering stationary state.
00446             //1. Turn motors off.
00447             //2. Reset QEIs.
00448             //3. Reset PID working variables.
00449             //4. Reset PIDs.
00450             //5. Reset set points and working variables.
00451             //7. Set state variable.
00452         case (STATE_STATIONARY):
00453 
00454             leftMotors.setPwmDuty(1.0);
00455             rightMotors.setPwmDuty(1.0);
00456 
00457             leftQei.reset();
00458             rightQei.reset();
00459 
00460             leftPulses_     = 0;
00461             leftPrevPulses_ = 0;
00462             leftPwmDuty_    = 1.0;
00463             leftVelocity_   = 0.0;
00464 
00465             rightPulses_     = 0;
00466             rightPrevPulses_ = 0;
00467             rightPwmDuty_    = 1.0;
00468             rightVelocity_   = 0.0;
00469 
00470             leftController.setSetPoint(0.0);
00471             leftController.setProcessValue(0.0);
00472             rightController.setSetPoint(0.0);
00473             rightController.setProcessValue(0.0);
00474 
00475             positionSetPoint_ = 0.0;
00476             headingSetPoint_  = 0.0;
00477             heading_          = 0.0;
00478             prevHeading_      = 0.0;
00479             degreesTurned_    = 0.0;
00480             leftStopFlag_     = 0;
00481             rightStopFlag_    = 0;
00482 
00483             for (int i = 0; i < logIndex; i++) {
00484                 fprintf(logFile, "%i, %i, %f, %f, %f\n", leftPositionBuffer[i],
00485                         rightPositionBuffer[i],
00486                         leftVelocityBuffer[i],
00487                         rightVelocityBuffer[i],
00488                         headingBuffer[i]);
00489             }
00490 
00491             logIndex = 0;
00492             
00493             imu.reset();
00494 
00495             state_ = STATE_STATIONARY;
00496 
00497             break;
00498 
00499             //Entering moving forward state.
00500             //1. Set correct direction for motors.
00501             //2. Set velocity set point.
00502             //3. Set state variable.
00503         case (STATE_MOVING_FORWARD):
00504 
00505             leftMotors.setDirection(BACKWARD);
00506             rightMotors.setDirection(FORWARD);
00507 
00508             //Velocity control.
00509             leftController.setSetPoint(1000);
00510             rightController.setSetPoint(1000);
00511 
00512             logIndex = 0;
00513             
00514             startHeading_ = imu.getYaw();
00515 
00516             state_ = STATE_MOVING_FORWARD;
00517 
00518             break;
00519 
00520             //Entering moving backward state.
00521             //1. Set correct direction for motors.
00522             //2. Set velocity set point.
00523             //3. Set state variable.
00524         case (STATE_MOVING_BACKWARD):
00525 
00526             leftMotors.setDirection(FORWARD);
00527             rightMotors.setDirection(BACKWARD);
00528 
00529             //Velocity control.
00530             leftController.setSetPoint(1000);
00531             rightController.setSetPoint(1000);
00532 
00533             logIndex = 0;
00534 
00535             state_ = STATE_MOVING_BACKWARD;
00536 
00537             break;
00538 
00539             //Entering rotating clockwise state.
00540             //1. Set correct direction for motors.
00541             //2. Set velocity set point.
00542             //3. Set working variables.
00543             //4. Set state variable.
00544         case (STATE_ROTATING_CLOCKWISE):
00545 
00546             leftMotors.setDirection(BACKWARD);
00547             rightMotors.setDirection(BACKWARD);
00548 
00549             leftController.setSetPoint(500);
00550             rightController.setSetPoint(500);
00551 
00552             degreesTurned_ = 0.0;
00553             heading_ = fabs(imu.getYaw());
00554             prevHeading_ = heading_;
00555 
00556             logIndex = 0;
00557 
00558             state_ = STATE_ROTATING_CLOCKWISE;
00559 
00560             break;
00561 
00562             //Entering rotating clockwise state.
00563             //1. Set correct direction for motors.
00564             //2. Set velocity set point.
00565             //3. Set working variables.
00566             //4. Set state variable.
00567         case (STATE_ROTATING_COUNTER_CLOCKWISE):
00568 
00569             leftMotors.setDirection(FORWARD);
00570             rightMotors.setDirection(FORWARD);
00571 
00572             leftController.setSetPoint(500);
00573             rightController.setSetPoint(500);
00574 
00575             degreesTurned_ = 0.0;
00576             heading_ = fabs(imu.getYaw());
00577             prevHeading_ = heading_;
00578 
00579             logIndex = 0;
00580 
00581             state_ = STATE_ROTATING_COUNTER_CLOCKWISE;
00582 
00583             break;
00584 
00585         default:
00586 
00587             state_ = STATE_STATIONARY;
00588 
00589             break;
00590 
00591     }
00592 
00593 }