Aaron Berk
Roving robot using the RS-EDP.
Dependencies: mbed RSEDP_AM_MC1_lib SDFileSystem
Diff: Rover.cpp
- Revision:
- 0:8d15dc761944
- Child:
- 1:ffef6386027b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Rover.cpp Mon Aug 16 09:46:28 2010 +0000
@@ -0,0 +1,538 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * RS-EDP Rover application class.
+ *
+ * Demonstrates four action states: moving {forward, backward},
+ * rotating {clockwise, counter-clockwise}.
+ *
+ * Logs heading and left and right motor position and velocity data.
+ *
+ * Performs PID velocity control on the motors.
+ *
+ * ---------------
+ * CONFIGURATION
+ * ---------------
+ *
+ * The set up assumes the H-bridge being used has pins for:
+ *
+ * - PWM input
+ * - Brake
+ * - Direction
+ *
+ * The constructor arguments will need to be changed if a different type of
+ * H-bridge is used.
+ *
+ * The PID controllers are configured using the #defines below.
+ *
+ * The set up also assumes two quadrature encoders are used, each using the
+ * default X2 encoding.
+ *
+ * The constructor arguments will need to be changed if a different number
+ * of encoders are used or if a different encoding is required.
+ */
+
+/**
+ * Includes
+ */
+#include "Rover.h"
+
+Serial pc2(USBTX, USBRX);
+
+Rover::Rover(PinName leftMotorPwm,
+ PinName leftMotorBrake,
+ PinName leftMotorDirection,
+ PinName rightMotorPwm,
+ PinName rightMotorBrake,
+ PinName rightMotorDirection,
+ PinName leftQeiChannelA,
+ PinName leftQeiChannelB,
+ PinName leftQeiIndex,
+ int leftPulsesPerRev,
+ PinName rightQeiChannelA,
+ PinName rightQeiChannelB,
+ PinName rightQeiIndex,
+ int rightPulsesPerRev) :
+ leftMotors(),
+ rightMotors(),
+ leftQei(leftQeiChannelA,
+ leftQeiChannelB,
+ leftQeiIndex,
+ leftPulsesPerRev),
+ rightQei(rightQeiChannelA,
+ rightQeiChannelB,
+ rightQeiIndex,
+ rightPulsesPerRev),
+ leftController(Kc, Ti, Td, PID_RATE),
+ rightController(Kc, Ti, Td, PID_RATE),
+ stateTicker(),
+ logTicker(),
+ imu(IMU_RATE,
+ GYRO_MEAS_ERROR,
+ ACCELEROMETER_RATE,
+ GYROSCOPE_RATE) {
+
+ //---------------------------------
+ // Left motors and PID controller.
+ //---------------------------------
+
+ //Motors.
+ leftMotors.setPwmPin(leftMotorPwm);
+ leftMotors.setBrakePin(leftMotorBrake);
+ leftMotors.setDirectionPin(leftMotorDirection);
+ leftMotors.initialize();
+ leftMotors.setDirection(BACKWARD);
+ leftMotors.setBrake(BRAKE_OFF);
+
+ //PID.
+ leftController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
+ leftController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
+ leftController.setBias(PID_BIAS);
+ leftController.setMode(AUTO_MODE);
+ leftPulses_ = 0;
+ leftPrevPulses_ = 0;
+ leftPwmDuty_ = 1.0;
+ leftVelocity_ = 0.0;
+
+ //----------------------------------
+ // Right motors and PID controller.
+ //----------------------------------
+
+ //Motors.
+ rightMotors.setPwmPin(rightMotorPwm);
+ rightMotors.setBrakePin(rightMotorBrake);
+ rightMotors.setDirectionPin(rightMotorDirection);
+ rightMotors.initialize();
+ rightMotors.setDirection(FORWARD);
+ rightMotors.setBrake(BRAKE_OFF);
+
+ //PID.
+ rightController.setInputLimits(PID_IN_MIN, PID_IN_MAX);
+ rightController.setOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
+ rightController.setBias(PID_BIAS);
+ rightController.setMode(AUTO_MODE);
+ rightPulses_ = 0;
+ rightPrevPulses_ = 0;
+ rightPwmDuty_ = 1.0;
+ rightVelocity_ = 0.0;
+
+ //--------------------
+ // Working Variables.
+ //--------------------
+ positionSetPoint_ = 0.0;
+ headingSetPoint_ = 0.0;
+ heading_ = 0.0;
+ prevHeading_ = 0.0;
+ degreesTurned_ = 0.0;
+ leftStopFlag_ = 0;
+ rightStopFlag_ = 0;
+
+ //--------
+ // BEGIN!
+ //--------
+ state_ = STATE_STATIONARY;
+ stateTicker.attach(this, &Rover::doState, PID_RATE);
+ startLogging();
+
+}
+
+void Rover::move(int distance) {
+
+ positionSetPoint_ = distance;
+
+ //Moving forward.
+ if (distance > 0) {
+
+ enterState(STATE_MOVING_FORWARD);
+
+ }
+ //Moving backward.
+ else if (distance < 0) {
+
+ enterState(STATE_MOVING_BACKWARD);
+
+ }
+
+ //If distance == 0, then do nothing, i.e. stay stationary.
+
+}
+
+void Rover::turn(int degrees) {
+
+ headingSetPoint_ = abs(degrees);
+
+ //Rotating clockwise.
+ if (degrees > 0) {
+
+ enterState(STATE_ROTATING_CLOCKWISE);
+
+ }
+ //Rotating counter-clockwise.
+ else if (degrees < 0) {
+
+ enterState(STATE_ROTATING_COUNTER_CLOCKWISE);
+
+ }
+
+ //If degrees == 0, then do nothing, i.e. stay stationary.
+
+}
+
+Rover::State Rover::getState(void) {
+
+ return state_;
+
+}
+
+void Rover::startLogging(void) {
+
+ logFile = fopen("/sd/roverlog.csv", "w");
+ fprintf(logFile, "leftPulses, rightPulses, leftVelocity, rightVelocity, heading, degreesTurned\n");
+ logTicker.attach(this, &Rover::log, LOG_RATE);
+
+}
+
+void Rover::stopLogging(void) {
+
+ fclose(logFile);
+
+}
+
+void Rover::log(void) {
+
+ fprintf(logFile, "%i,%i,%f,%f,%f,%f\n",
+ leftPulses_, rightPulses_, leftVelocity_, rightVelocity_, imu.getYaw(), degreesTurned_);
+
+}
+
+void Rover::doState(void) {
+
+ switch (state_) {
+
+ //We're not moving so don't do anything!
+ case (STATE_STATIONARY):
+ break;
+
+ case (STATE_MOVING_FORWARD):
+
+ //If we haven't hit the position set point yet,
+ //perform velocity control on the motors.
+ if (leftPulses_ < positionSetPoint_) {
+
+ leftPulses_ = leftQei.getPulses();
+ leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
+ leftPrevPulses_ = leftPulses_;
+ leftController.setProcessValue(leftVelocity_);
+ leftPwmDuty_ = leftController.getRealOutput();
+
+ } else {
+ leftStopFlag_ = 1;
+ }
+
+ leftMotors.setPwmDuty(leftPwmDuty_);
+
+ if (rightPulses_ < positionSetPoint_) {
+
+ rightPulses_ = rightQei.getPulses();
+ rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
+ rightPrevPulses_ = rightPulses_;
+ rightController.setProcessValue(rightVelocity_);
+ rightPwmDuty_ = rightController.getRealOutput();
+
+ } else {
+ rightStopFlag_ = 1;
+ }
+
+ rightMotors.setPwmDuty(rightPwmDuty_);
+
+ //We've hit the end position set point.
+ if (leftStopFlag_ == 1 && rightStopFlag_ == 1) {
+ leftPwmDuty_ = 1.0;
+ rightPwmDuty_ = 1.0;
+ leftMotors.setPwmDuty(leftPwmDuty_);
+ rightMotors.setPwmDuty(rightPwmDuty_);
+ enterState(STATE_STATIONARY);
+ }
+
+ break;
+
+ case (STATE_MOVING_BACKWARD):
+
+ //If we haven't hit the position set point yet,
+ //perform velocity control on the motors.
+ if (leftPulses_ > positionSetPoint_) {
+ leftPulses_ = leftQei.getPulses();
+ leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
+ leftPrevPulses_ = leftPulses_;
+ leftController.setProcessValue(fabs(leftVelocity_));
+ leftPwmDuty_ = leftController.getRealOutput();
+ } else {
+ leftStopFlag_ = 1;
+ }
+
+ leftMotors.setPwmDuty(leftPwmDuty_);
+
+ if (rightPulses_ > positionSetPoint_) {
+ rightPulses_ = rightQei.getPulses();
+ rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
+ rightPrevPulses_ = rightPulses_;
+ rightController.setProcessValue(fabs(rightVelocity_));
+ rightPwmDuty_ = rightController.getRealOutput();
+
+ } else {
+ rightStopFlag_ = 1;
+ }
+
+ rightMotors.setPwmDuty(rightPwmDuty_);
+
+ //We've hit the end position set point.
+ if (leftStopFlag_ == 1.0 && rightStopFlag_ == 1.0) {
+ leftPwmDuty_ = 1.0;
+ rightPwmDuty_ = 1.0;
+ leftMotors.setPwmDuty(leftPwmDuty_);
+ rightMotors.setPwmDuty(rightPwmDuty_);
+ enterState(STATE_STATIONARY);
+ }
+
+ break;
+
+ case (STATE_ROTATING_CLOCKWISE):
+
+ //If we haven't hit the position set point yet,
+ //perform velocity control on the motors.
+ if (degreesTurned_ < headingSetPoint_) {
+
+ heading_ = fabs(imu.getYaw());
+ degreesTurned_ += fabs(heading_ - prevHeading_);
+ prevHeading_ = heading_;
+
+ leftPulses_ = leftQei.getPulses();
+ leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
+ leftPrevPulses_ = leftPulses_;
+ leftController.setProcessValue(leftVelocity_);
+ leftPwmDuty_ = leftController.getRealOutput();
+
+ rightPulses_ = rightQei.getPulses();
+ rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
+ rightPrevPulses_ = rightPulses_;
+ rightController.setProcessValue(fabs(rightVelocity_));
+ rightPwmDuty_ = rightController.getRealOutput();
+
+ leftMotors.setPwmDuty(leftPwmDuty_);
+ rightMotors.setPwmDuty(rightPwmDuty_);
+
+ } else {
+
+ leftPwmDuty_ = 1.0;
+ rightPwmDuty_ = 1.0;
+ leftMotors.setPwmDuty(leftPwmDuty_);
+ rightMotors.setPwmDuty(rightPwmDuty_);
+ enterState(STATE_STATIONARY);
+
+ }
+
+ break;
+
+ case (STATE_ROTATING_COUNTER_CLOCKWISE):
+
+ //If we haven't hit the position set point yet,
+ //perform velocity control on the motors.
+ if (degreesTurned_ < headingSetPoint_) {
+
+ heading_ = fabs(imu.getYaw());
+ degreesTurned_ += fabs(heading_ - prevHeading_);
+ prevHeading_ = heading_;
+
+ leftPulses_ = leftQei.getPulses();
+ leftVelocity_ = (leftPulses_ - leftPrevPulses_) / PID_RATE;
+ leftPrevPulses_ = leftPulses_;
+ leftController.setProcessValue(fabs(leftVelocity_));
+ leftPwmDuty_ = leftController.getRealOutput();
+
+ rightPulses_ = rightQei.getPulses();
+ rightVelocity_ = (rightPulses_ - rightPrevPulses_) / PID_RATE;
+ rightPrevPulses_ = rightPulses_;
+ rightController.setProcessValue(rightVelocity_);
+ rightPwmDuty_ = rightController.getRealOutput();
+
+ leftMotors.setPwmDuty(leftPwmDuty_);
+ rightMotors.setPwmDuty(rightPwmDuty_);
+
+ } else {
+
+ leftPwmDuty_ = 1.0;
+ rightPwmDuty_ = 1.0;
+ leftMotors.setPwmDuty(leftPwmDuty_);
+ rightMotors.setPwmDuty(rightPwmDuty_);
+ enterState(STATE_STATIONARY);
+
+ }
+
+ break;
+
+ //If we've fallen into a black hole, the least we can do is turn
+ //the motors off.
+ default:
+
+ leftMotors.setPwmDuty(1.0);
+ rightMotors.setPwmDuty(1.0);
+
+ break;
+
+ }
+
+}
+
+void Rover::enterState(State state) {
+
+ switch (state) {
+
+ //Entering stationary state.
+ //1. Turn motors off.
+ //2. Reset QEIs.
+ //3. Reset PID working variables.
+ //4. Reset PIDs.
+ //5. Reset set points and working variables.
+ //7. Set state variable.
+ case (STATE_STATIONARY):
+
+ leftMotors.setPwmDuty(1.0);
+ rightMotors.setPwmDuty(1.0);
+
+ leftQei.reset();
+ rightQei.reset();
+
+ leftPulses_ = 0;
+ leftPrevPulses_ = 0;
+ leftPwmDuty_ = 1.0;
+ leftVelocity_ = 0.0;
+
+ rightPulses_ = 0;
+ rightPrevPulses_ = 0;
+ rightPwmDuty_ = 1.0;
+ rightVelocity_ = 0.0;
+
+ leftController.setSetPoint(0.0);
+ leftController.setProcessValue(0.0);
+ rightController.setSetPoint(0.0);
+ rightController.setProcessValue(0.0);
+
+ positionSetPoint_ = 0.0;
+ headingSetPoint_ = 0.0;
+ heading_ = 0.0;
+ prevHeading_ = 0.0;
+ degreesTurned_ = 0.0;
+ leftStopFlag_ = 0;
+ rightStopFlag_ = 0;
+
+ state_ = STATE_STATIONARY;
+
+ break;
+
+ //Entering moving forward state.
+ //1. Set correct direction for motors.
+ //2. Set velocity set point.
+ //3. Set state variable.
+ case (STATE_MOVING_FORWARD):
+
+ leftMotors.setDirection(BACKWARD);
+ rightMotors.setDirection(FORWARD);
+
+ //Velocity control.
+ leftController.setSetPoint(1000);
+ rightController.setSetPoint(1000);
+
+ state_ = STATE_MOVING_FORWARD;
+
+ break;
+
+ //Entering moving backward state.
+ //1. Set correct direction for motors.
+ //2. Set velocity set point.
+ //3. Set state variable.
+ case (STATE_MOVING_BACKWARD):
+
+ leftMotors.setDirection(FORWARD);
+ rightMotors.setDirection(BACKWARD);
+
+ //Velocity control.
+ leftController.setSetPoint(1000);
+ rightController.setSetPoint(1000);
+
+ state_ = STATE_MOVING_BACKWARD;
+
+ break;
+
+ //Entering rotating clockwise state.
+ //1. Set correct direction for motors.
+ //2. Set velocity set point.
+ //3. Set working variables.
+ //4. Set state variable.
+ case (STATE_ROTATING_CLOCKWISE):
+
+ leftMotors.setDirection(BACKWARD);
+ rightMotors.setDirection(BACKWARD);
+
+ leftController.setSetPoint(500);
+ rightController.setSetPoint(500);
+
+ degreesTurned_ = 0.0;
+ heading_ = fabs(imu.getYaw());
+ prevHeading_ = heading_;
+
+ state_ = STATE_ROTATING_CLOCKWISE;
+
+ break;
+
+ //Entering rotating clockwise state.
+ //1. Set correct direction for motors.
+ //2. Set velocity set point.
+ //3. Set working variables.
+ //4. Set state variable.
+ case (STATE_ROTATING_COUNTER_CLOCKWISE):
+
+ leftMotors.setDirection(FORWARD);
+ rightMotors.setDirection(FORWARD);
+
+ leftController.setSetPoint(500);
+ rightController.setSetPoint(500);
+
+ degreesTurned_ = 0.0;
+ heading_ = fabs(imu.getYaw());
+ prevHeading_ = heading_;
+
+ state_ = STATE_ROTATING_COUNTER_CLOCKWISE;
+
+ break;
+
+ default:
+
+ break;
+
+ }
+
+}