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

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

Rover.h@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 1:ffef6386027b 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 #ifndef ROVER_H
aberk 0:8d15dc761944 60 #define ROVER_H
aberk 0:8d15dc761944 61
aberk 0:8d15dc761944 62 /**
aberk 0:8d15dc761944 63 * Includes
aberk 0:8d15dc761944 64 */
aberk 0:8d15dc761944 65 #include "mbed.h"
aberk 0:8d15dc761944 66 #include "RSEDP_AM_MC1.h"
aberk 0:8d15dc761944 67 #include "QEI.h"
aberk 0:8d15dc761944 68 #include "PID.h"
aberk 0:8d15dc761944 69 #include "IMU.h"
aberk 0:8d15dc761944 70 #include "SDFileSystem.h"
aberk 1:ffef6386027b 71 #include "HMC6352.h"
aberk 0:8d15dc761944 72
aberk 0:8d15dc761944 73 /**
aberk 0:8d15dc761944 74 * Defines
aberk 0:8d15dc761944 75 */
aberk 0:8d15dc761944 76 //---------------------
aberk 0:8d15dc761944 77 // Physical attributes
aberk 0:8d15dc761944 78 //---------------------
aberk 0:8d15dc761944 79 #define PULSES_PER_REV 624
aberk 0:8d15dc761944 80 #define WHEEL_DIAMETER 58.928 //mm
aberk 0:8d15dc761944 81 #define ROTATION_DISTANCE 220.0 //mm
aberk 0:8d15dc761944 82 #define REVS_PER_ROTATION (ROTATION_DISTANCE / WHEEL_DIAMETER)
aberk 0:8d15dc761944 83 #define PULSES_PER_ROTATION (REVS_PER_ROTATION * PULSES_PER_REV)
aberk 0:8d15dc761944 84 #define PULSES_PER_MM (PULSES_PER_REV / WHEEL_DIAMETER)
aberk 1:ffef6386027b 85 #define DISTANCE_PER_PULSE (WHEEL_DIAMETER / PULSES_PER_REV)
aberk 1:ffef6386027b 86 #define ENCODING 2 //Use X2 encoding
aberk 1:ffef6386027b 87 #define WHEEL_DISTANCE (ROTATION_DISTANCE / DISTANCE_PER_PULSE)
aberk 0:8d15dc761944 88 //-----
aberk 0:8d15dc761944 89 // PID
aberk 0:8d15dc761944 90 //-----
aberk 0:8d15dc761944 91 #define PID_RATE 0.01
aberk 0:8d15dc761944 92 #define PID_BIAS 1.0
aberk 0:8d15dc761944 93 #define PID_IN_MIN 0.0
aberk 0:8d15dc761944 94 #define PID_IN_MAX 10500.0
aberk 0:8d15dc761944 95 #define PID_OUT_MIN 0.0
aberk 0:8d15dc761944 96 #define PID_OUT_MAX 1.0
aberk 0:8d15dc761944 97 #define Kc -0.9
aberk 0:8d15dc761944 98 #define Ti 0.08
aberk 0:8d15dc761944 99 #define Td 0.0
aberk 0:8d15dc761944 100 //---------
aberk 0:8d15dc761944 101 // Logging
aberk 0:8d15dc761944 102 //---------
aberk 1:ffef6386027b 103 #define LOG_RATE 0.025
aberk 0:8d15dc761944 104 //-----
aberk 0:8d15dc761944 105 // IMU
aberk 0:8d15dc761944 106 //-----
aberk 0:8d15dc761944 107 #define ACCELEROMETER_RATE 0.005
aberk 0:8d15dc761944 108 #define GYROSCOPE_RATE 0.005
aberk 1:ffef6386027b 109 #define IMU_RATE_ 0.025
aberk 0:8d15dc761944 110 #define GYRO_MEAS_ERROR 0.3
aberk 0:8d15dc761944 111
aberk 0:8d15dc761944 112 class Rover {
aberk 0:8d15dc761944 113
aberk 0:8d15dc761944 114 public:
aberk 0:8d15dc761944 115
aberk 0:8d15dc761944 116 typedef enum State {
aberk 0:8d15dc761944 117
aberk 0:8d15dc761944 118 STATE_STATIONARY,
aberk 0:8d15dc761944 119 STATE_MOVING_FORWARD,
aberk 0:8d15dc761944 120 STATE_MOVING_BACKWARD,
aberk 0:8d15dc761944 121 STATE_ROTATING_CLOCKWISE,
aberk 0:8d15dc761944 122 STATE_ROTATING_COUNTER_CLOCKWISE
aberk 0:8d15dc761944 123
aberk 0:8d15dc761944 124 } State;
aberk 0:8d15dc761944 125
aberk 0:8d15dc761944 126 /**
aberk 0:8d15dc761944 127 * Constructor.
aberk 0:8d15dc761944 128 *
aberk 0:8d15dc761944 129 * Creates left and right motor control module objects using the specified
aberk 0:8d15dc761944 130 * pins and initializes them. Sets the direction appropriately so both
aberk 0:8d15dc761944 131 * wheels are going "forward".
aberk 0:8d15dc761944 132 * Creates the left and right wheel quadrature encoder interfaces with the
aberk 0:8d15dc761944 133 * specified pins.
aberk 0:8d15dc761944 134 *
aberk 0:8d15dc761944 135 * @param leftMotorPwm Pin to use for PWM input for the left motors.
aberk 0:8d15dc761944 136 * @param leftMotorBrake Pin to use for brake input for left motors.
aberk 0:8d15dc761944 137 * @param leftMotorDirection Pin to use for direction input for the left
aberk 0:8d15dc761944 138 * motors.
aberk 0:8d15dc761944 139 * @param rightMotorPwm Pin to use for PWM input for the right motors.
aberk 0:8d15dc761944 140 * @param rightMotorBrake Pin to use for brake input for right motors.
aberk 0:8d15dc761944 141 * @param rightMotorDirection Pin to use for direction input for the right
aberk 0:8d15dc761944 142 * motors.
aberk 0:8d15dc761944 143 * @param leftQeiChannelA Pin to use for channel A on the left wheel
aberk 0:8d15dc761944 144 * quadrature encoder.
aberk 0:8d15dc761944 145 * @param leftQeiChannelB Pin to use for channel B on the left wheel
aberk 0:8d15dc761944 146 * quadrature encoder.
aberk 0:8d15dc761944 147 * @param leftQeiIndex Pin to use for the index channel on the left wheel
aberk 0:8d15dc761944 148 * quadrature encoder.
aberk 0:8d15dc761944 149 * @param leftPulsesPerRev The number of pulses per revolution on the left
aberk 0:8d15dc761944 150 * quadrature encoder.
aberk 0:8d15dc761944 151 * @param rightQeiChannelA Pin to use for channel A on the right wheel
aberk 0:8d15dc761944 152 * quadrature encoder.
aberk 0:8d15dc761944 153 * @param rightQeiChannelB Pin to use for channel B on the right wheel
aberk 0:8d15dc761944 154 * quadrature encoder.
aberk 0:8d15dc761944 155 * @param rightQeiIndex Pin to use for the index channel on the left wheel
aberk 0:8d15dc761944 156 * quadrature encoder.
aberk 0:8d15dc761944 157 * @param rightPulsesPerRev The number of pulses per revolution on the
aberk 0:8d15dc761944 158 * right quadrature encoder.
aberk 0:8d15dc761944 159 */
aberk 0:8d15dc761944 160 Rover(PinName leftMotorPwm,
aberk 0:8d15dc761944 161 PinName leftMotorBrake,
aberk 0:8d15dc761944 162 PinName leftMotorDirection,
aberk 0:8d15dc761944 163 PinName rightMotorPwm,
aberk 0:8d15dc761944 164 PinName rightMotorBrake,
aberk 0:8d15dc761944 165 PinName rightMotorDirection,
aberk 0:8d15dc761944 166 PinName leftQeiChannelA,
aberk 0:8d15dc761944 167 PinName leftQeiChannelB,
aberk 0:8d15dc761944 168 PinName leftQeiIndex,
aberk 0:8d15dc761944 169 int leftPulsesPerRev,
aberk 0:8d15dc761944 170 PinName rightQeiChannelA,
aberk 0:8d15dc761944 171 PinName rightQeiChannelB,
aberk 0:8d15dc761944 172 PinName rightQeiIndex,
aberk 0:8d15dc761944 173 int rightPulsesPerRev);
aberk 0:8d15dc761944 174
aberk 0:8d15dc761944 175 /**
aberk 0:8d15dc761944 176 * Move the rover directly forward/backward a certain distance.
aberk 0:8d15dc761944 177 *
aberk 0:8d15dc761944 178 * Distance measured in pulses/stripes of the encoder wheel.
aberk 0:8d15dc761944 179 * +ve distance -> forward
aberk 0:8d15dc761944 180 * -ve distance -> backward
aberk 0:8d15dc761944 181 *
aberk 1:ffef6386027b 182 * @param distance The distance to move in metres.
aberk 0:8d15dc761944 183 */
aberk 1:ffef6386027b 184 void move(float distance);
aberk 0:8d15dc761944 185
aberk 0:8d15dc761944 186 /**
aberk 0:8d15dc761944 187 * Turn the rover left or right a certain number of degrees.
aberk 1:ffef6386027b 188 *
aberk 1:ffef6386027b 189 * +ve degrees -> clockwise
aberk 1:ffef6386027b 190 * -ve degrees -> counter-clockwise
aberk 1:ffef6386027b 191 *
aberk 1:ffef6386027b 192 * @param degrees The number of degrees to rotate.
aberk 0:8d15dc761944 193 */
aberk 0:8d15dc761944 194 void turn(int degrees);
aberk 0:8d15dc761944 195
aberk 0:8d15dc761944 196 /**
aberk 0:8d15dc761944 197 * Get the current state of the rover.
aberk 0:8d15dc761944 198 *
aberk 0:8d15dc761944 199 * @return The current state of the rover.
aberk 0:8d15dc761944 200 */
aberk 0:8d15dc761944 201 State getState(void);
aberk 0:8d15dc761944 202
aberk 0:8d15dc761944 203 /**
aberk 0:8d15dc761944 204 * Start logging position, velocity and heading data.
aberk 0:8d15dc761944 205 */
aberk 0:8d15dc761944 206 void startLogging(void);
aberk 0:8d15dc761944 207
aberk 0:8d15dc761944 208 /**
aberk 0:8d15dc761944 209 * Stop logging position, velocity and heading data.
aberk 0:8d15dc761944 210 */
aberk 0:8d15dc761944 211 void stopLogging(void);
aberk 0:8d15dc761944 212
aberk 0:8d15dc761944 213 private:
aberk 0:8d15dc761944 214
aberk 0:8d15dc761944 215 /**
aberk 0:8d15dc761944 216 * Takes appropriate action for the current state of the Rover.
aberk 0:8d15dc761944 217 */
aberk 0:8d15dc761944 218 void doState(void);
aberk 0:8d15dc761944 219
aberk 0:8d15dc761944 220 /**
aberk 0:8d15dc761944 221 * Actions to take on entering a new state.
aberk 0:8d15dc761944 222 *
aberk 0:8d15dc761944 223 * @param state The new state to enter.
aberk 0:8d15dc761944 224 */
aberk 0:8d15dc761944 225 void enterState(State state);
aberk 0:8d15dc761944 226
aberk 0:8d15dc761944 227 /**
aberk 0:8d15dc761944 228 * Set up the accelerometer for our application.
aberk 0:8d15dc761944 229 */
aberk 0:8d15dc761944 230 void initializeAccelerometer(void);
aberk 0:8d15dc761944 231
aberk 0:8d15dc761944 232 /**
aberk 0:8d15dc761944 233 * Calibrate the accelerometer.
aberk 0:8d15dc761944 234 */
aberk 0:8d15dc761944 235 void calibrateAccelerometer(void);
aberk 0:8d15dc761944 236
aberk 0:8d15dc761944 237 /**
aberk 0:8d15dc761944 238 * Get the latest accelerometer data.
aberk 0:8d15dc761944 239 */
aberk 0:8d15dc761944 240 void sampleAccelerometer(void);
aberk 0:8d15dc761944 241
aberk 0:8d15dc761944 242 /**
aberk 0:8d15dc761944 243 * Log the current position, velocity and heading data.
aberk 0:8d15dc761944 244 */
aberk 0:8d15dc761944 245 void log(void);
aberk 0:8d15dc761944 246
aberk 0:8d15dc761944 247 RSEDP_AM_MC1 leftMotors;
aberk 0:8d15dc761944 248 RSEDP_AM_MC1 rightMotors;
aberk 0:8d15dc761944 249 QEI leftQei;
aberk 0:8d15dc761944 250 QEI rightQei;
aberk 0:8d15dc761944 251 PID leftController;
aberk 0:8d15dc761944 252 PID rightController;
aberk 0:8d15dc761944 253 Ticker stateTicker;
aberk 0:8d15dc761944 254 Ticker logTicker;
aberk 0:8d15dc761944 255 IMU imu;
aberk 0:8d15dc761944 256
aberk 0:8d15dc761944 257 FILE* logFile;
aberk 1:ffef6386027b 258
aberk 0:8d15dc761944 259 volatile int leftStopFlag_;
aberk 0:8d15dc761944 260 volatile int rightStopFlag_;
aberk 0:8d15dc761944 261
aberk 0:8d15dc761944 262 volatile int positionSetPoint_;
aberk 1:ffef6386027b 263 volatile float headingSetPoint_;
aberk 0:8d15dc761944 264 volatile float degreesTurned_;
aberk 0:8d15dc761944 265
aberk 0:8d15dc761944 266 volatile int leftPulses_;
aberk 0:8d15dc761944 267 volatile int leftPrevPulses_;
aberk 0:8d15dc761944 268 volatile float leftPwmDuty_;
aberk 0:8d15dc761944 269 volatile float leftVelocity_;
aberk 0:8d15dc761944 270
aberk 0:8d15dc761944 271 volatile int rightPulses_;
aberk 0:8d15dc761944 272 volatile int rightPrevPulses_;
aberk 0:8d15dc761944 273 volatile float rightPwmDuty_;
aberk 0:8d15dc761944 274 volatile float rightVelocity_;
aberk 0:8d15dc761944 275
aberk 0:8d15dc761944 276 volatile float prevHeading_;
aberk 0:8d15dc761944 277 volatile float heading_;
aberk 0:8d15dc761944 278
aberk 0:8d15dc761944 279 volatile State state_;
aberk 0:8d15dc761944 280
aberk 1:ffef6386027b 281 volatile float headingBuffer[1024];
aberk 1:ffef6386027b 282 volatile int leftPositionBuffer[1024];
aberk 1:ffef6386027b 283 volatile int rightPositionBuffer[1024];
aberk 1:ffef6386027b 284 volatile float leftVelocityBuffer[1024];
aberk 1:ffef6386027b 285 volatile float rightVelocityBuffer[1024];
aberk 1:ffef6386027b 286 volatile int logIndex;
aberk 1:ffef6386027b 287
aberk 1:ffef6386027b 288 volatile float startHeading_;
aberk 1:ffef6386027b 289 volatile float endHeading_;
aberk 1:ffef6386027b 290
aberk 0:8d15dc761944 291 };
aberk 0:8d15dc761944 292
aberk 0:8d15dc761944 293 #endif /* ROVER_H */