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

Roving robot using the RS-EDP.

Dependencies:   mbed RSEDP_AM_MC1_lib SDFileSystem

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