Rover.h

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 #ifndef ROVER_H
00060 #define ROVER_H
00061 
00062 /**
00063  * Includes
00064  */
00065 #include "mbed.h"
00066 #include "RSEDP_AM_MC1.h"
00067 #include "QEI.h"
00068 #include "PID.h"
00069 #include "IMU.h"
00070 #include "SDFileSystem.h"
00071 #include "HMC6352.h"
00072 
00073 /**
00074  * Defines
00075  */
00076 //---------------------
00077 // Physical attributes
00078 //---------------------
00079 #define PULSES_PER_REV        624
00080 #define WHEEL_DIAMETER        58.928 //mm
00081 #define ROTATION_DISTANCE     220.0  //mm
00082 #define REVS_PER_ROTATION     (ROTATION_DISTANCE / WHEEL_DIAMETER)
00083 #define PULSES_PER_ROTATION   (REVS_PER_ROTATION * PULSES_PER_REV)
00084 #define PULSES_PER_MM         (PULSES_PER_REV / WHEEL_DIAMETER)
00085 #define DISTANCE_PER_PULSE    (WHEEL_DIAMETER / PULSES_PER_REV)
00086 #define ENCODING              2 //Use X2 encoding
00087 #define WHEEL_DISTANCE        (ROTATION_DISTANCE / DISTANCE_PER_PULSE)
00088 //-----
00089 // PID
00090 //-----
00091 #define PID_RATE     0.01
00092 #define PID_BIAS     1.0
00093 #define PID_IN_MIN   0.0
00094 #define PID_IN_MAX   10500.0
00095 #define PID_OUT_MIN  0.0
00096 #define PID_OUT_MAX  1.0
00097 #define Kc          -0.9
00098 #define Ti           0.08
00099 #define Td           0.0
00100 //---------
00101 // Logging
00102 //---------
00103 #define LOG_RATE     0.025
00104 //-----
00105 // IMU
00106 //-----
00107 #define ACCELEROMETER_RATE 0.005
00108 #define GYROSCOPE_RATE     0.005
00109 #define IMU_RATE_          0.025
00110 #define GYRO_MEAS_ERROR    0.3
00111 
00112 class Rover {
00113 
00114 public:
00115 
00116     typedef enum State {
00117 
00118         STATE_STATIONARY,
00119         STATE_MOVING_FORWARD,
00120         STATE_MOVING_BACKWARD,
00121         STATE_ROTATING_CLOCKWISE,
00122         STATE_ROTATING_COUNTER_CLOCKWISE
00123 
00124     } State;
00125 
00126     /**
00127      * Constructor.
00128      *
00129      * Creates left and right motor control module objects using the specified
00130      * pins and initializes them. Sets the direction appropriately so both
00131      * wheels are going "forward".
00132      * Creates the left and right wheel quadrature encoder interfaces with the
00133      * specified pins.
00134      *
00135      * @param leftMotorPwm Pin to use for PWM input for the left motors.
00136      * @param leftMotorBrake Pin to use for brake input for left motors.
00137      * @param leftMotorDirection Pin to use for direction input for the left
00138      *                           motors.
00139      * @param rightMotorPwm Pin to use for PWM input for the right motors.
00140      * @param rightMotorBrake Pin to use for brake input for right motors.
00141      * @param rightMotorDirection Pin to use for direction input for the right
00142      *                            motors.
00143      * @param leftQeiChannelA Pin to use for channel A on the left wheel
00144      *                        quadrature encoder.
00145      * @param leftQeiChannelB Pin to use for channel B on the left wheel
00146      *                        quadrature encoder.
00147      * @param leftQeiIndex Pin to use for the index channel on the left wheel
00148      *                     quadrature encoder.
00149      * @param leftPulsesPerRev The number of pulses per revolution on the left
00150      *                         quadrature encoder.
00151      * @param rightQeiChannelA Pin to use for channel A on the right wheel
00152      *                         quadrature encoder.
00153      * @param rightQeiChannelB Pin to use for channel B on the right wheel
00154      *                         quadrature encoder.
00155      * @param rightQeiIndex Pin to use for the index channel on the left wheel
00156      *                      quadrature encoder.
00157      * @param rightPulsesPerRev The number of pulses per revolution on the
00158      *                          right quadrature encoder.
00159      */
00160     Rover(PinName leftMotorPwm,
00161           PinName leftMotorBrake,
00162           PinName leftMotorDirection,
00163           PinName rightMotorPwm,
00164           PinName rightMotorBrake,
00165           PinName rightMotorDirection,
00166           PinName leftQeiChannelA,
00167           PinName leftQeiChannelB,
00168           PinName leftQeiIndex,
00169           int     leftPulsesPerRev,
00170           PinName rightQeiChannelA,
00171           PinName rightQeiChannelB,
00172           PinName rightQeiIndex,
00173           int     rightPulsesPerRev);
00174 
00175     /**
00176      * Move the rover directly forward/backward a certain distance.
00177      *
00178      * Distance measured in pulses/stripes of the encoder wheel.
00179      * +ve distance -> forward
00180      * -ve distance -> backward
00181      *
00182      * @param distance The distance to move in metres.
00183      */
00184     void move(float distance);
00185 
00186     /**
00187      * Turn the rover left or right a certain number of degrees.
00188      *
00189      * +ve degrees -> clockwise
00190      * -ve degrees -> counter-clockwise
00191      *
00192      * @param degrees The number of degrees to rotate.
00193      */
00194     void turn(int degrees);
00195 
00196     /**
00197      * Get the current state of the rover.
00198      *
00199      * @return The current state of the rover.
00200      */
00201     State getState(void);
00202 
00203     /**
00204      * Start logging position, velocity and heading data.
00205      */
00206     void startLogging(void);
00207 
00208     /**
00209      * Stop logging position, velocity and heading data.
00210      */
00211     void stopLogging(void);
00212 
00213 private:
00214 
00215     /**
00216      * Takes appropriate action for the current state of the Rover.
00217      */
00218     void doState(void);
00219 
00220     /**
00221      * Actions to take on entering a new state.
00222      *
00223      * @param state The new state to enter.
00224      */
00225     void enterState(State state);
00226 
00227     /**
00228      * Set up the accelerometer for our application.
00229      */
00230     void initializeAccelerometer(void);
00231 
00232     /**
00233      * Calibrate the accelerometer.
00234      */
00235     void calibrateAccelerometer(void);
00236 
00237     /**
00238      * Get the latest accelerometer data.
00239      */
00240     void sampleAccelerometer(void);
00241 
00242     /**
00243      * Log the current position, velocity and heading data.
00244      */
00245     void log(void);
00246 
00247     RSEDP_AM_MC1  leftMotors;
00248     RSEDP_AM_MC1  rightMotors;
00249     QEI           leftQei;
00250     QEI           rightQei;
00251     PID           leftController;
00252     PID           rightController;
00253     Ticker        stateTicker;
00254     Ticker        logTicker;
00255     IMU           imu;
00256 
00257     FILE*          logFile;
00258 
00259     volatile int   leftStopFlag_;
00260     volatile int   rightStopFlag_;
00261 
00262     volatile int   positionSetPoint_;
00263     volatile float headingSetPoint_;
00264     volatile float degreesTurned_;
00265 
00266     volatile int   leftPulses_;
00267     volatile int   leftPrevPulses_;
00268     volatile float leftPwmDuty_;
00269     volatile float leftVelocity_;
00270 
00271     volatile int   rightPulses_;
00272     volatile int   rightPrevPulses_;
00273     volatile float rightPwmDuty_;
00274     volatile float rightVelocity_;
00275 
00276     volatile float prevHeading_;
00277     volatile float heading_;
00278 
00279     volatile State state_;
00280 
00281     volatile float headingBuffer[1024];
00282     volatile int   leftPositionBuffer[1024];
00283     volatile int   rightPositionBuffer[1024];
00284     volatile float leftVelocityBuffer[1024];
00285     volatile float rightVelocityBuffer[1024];
00286     volatile int   logIndex;
00287     
00288     volatile float startHeading_;
00289     volatile float endHeading_;
00290     
00291 };
00292 
00293 #endif /* ROVER_H */