![](/media/cache/profiles/79a49b3e3762632813f9e35f4ba53d6c.50x50_q85.jpg)
An advanced robot that uses PID control on the motor speed, and an IMU for making accurate turns.
Dependencies: mbed Motor ITG3200 QEI ADXL345 IMUfilter PID
Revision 0:7440a03255a7, committed 2010-09-10
- Comitter:
- aberk
- Date:
- Fri Sep 10 14:03:00 2010 +0000
- Commit message:
- Version 1.0
Changed in this revision
diff -r 000000000000 -r 7440a03255a7 ADXL345.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ADXL345.lib Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/ADXL345/#bd8f0f20f433
diff -r 000000000000 -r 7440a03255a7 IMU.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU.cpp Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,338 @@ +/** + * @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 + * + * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using + * orientation filter developed by Sebastian Madgwick. + * + * Find more details about his paper here: + * + * http://code.google.com/p/imumargalgorithm30042010sohm/ + */ + + +/** + * Includes + */ +#include "IMU.h" + +IMU::IMU(float imuRate, + double gyroscopeMeasurementError, + float accelerometerRate, + float gyroscopeRate) : accelerometer(p5, p6, p7, p8), + gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) { + + imuRate_ = imuRate; + accelerometerRate_ = accelerometerRate; + gyroscopeRate_ = gyroscopeRate; + + //Initialize sampling variables. + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + + accelerometerSamples = 0; + gyroscopeSamples = 0; + + //Initialize and calibrate sensors. + initializeAccelerometer(); + calibrateAccelerometer(); + + initializeGyroscope(); + calibrateGyroscope(); + + //To reduce the number of interrupts we'll remove the separate tickers for + //the accelerometer, gyro and filter update and combine them all into one. + + //accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_); + //gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_); + sampleTicker.attach(this, &IMU::sample, accelerometerRate_); + //filterTicker.attach(this, &IMU::filter, imuRate_); + +} + +double IMU::getRoll(void) { + + return toDegrees(imuFilter.getRoll()); + +} + +double IMU::getPitch(void) { + + return toDegrees(imuFilter.getPitch()); + +} + +double IMU::getYaw(void) { + + return toDegrees(imuFilter.getYaw()); + +} + +void IMU::initializeAccelerometer(void) { + + //Go into standby mode to configure the device. + accelerometer.setPowerControl(0x00); + //Full resolution, +/-16g, 4mg/LSB. + accelerometer.setDataFormatControl(0x0B); + //200Hz data rate. + accelerometer.setDataRate(ADXL345_200HZ); + //Measurement mode. + accelerometer.setPowerControl(0x08); + //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf + wait_ms(22); + +} + +void IMU::sampleAccelerometer(void) { + + //If we've taken a certain number of samples, + //average them, remove the bias and convert the units. + if (accelerometerSamples == SAMPLES) { + + a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; + a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; + a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; + + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + accelerometerSamples = 0; + + } + //Otherwise, accumulate another reading. + else { + + accelerometer.getOutput(readings); + + a_xAccumulator += (int16_t) readings[0]; + a_yAccumulator += (int16_t) readings[1]; + a_zAccumulator += (int16_t) readings[2]; + + accelerometerSamples++; + + } + +} + +void IMU::calibrateAccelerometer(void) { + + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + + //Accumulate a certain number of samples. + for (int i = 0; i < CALIBRATION_SAMPLES; i++) { + + accelerometer.getOutput(readings); + + a_xAccumulator += (int16_t) readings[0]; + a_yAccumulator += (int16_t) readings[1]; + a_zAccumulator += (int16_t) readings[2]; + + wait(accelerometerRate_); + + } + + //Average the samples. + a_xAccumulator /= CALIBRATION_SAMPLES; + a_yAccumulator /= CALIBRATION_SAMPLES; + a_zAccumulator /= CALIBRATION_SAMPLES; + + //These are our zero g offsets. + //250 = 9.81m/s/s @ 4mg/LSB. + a_xBias = a_xAccumulator; + a_yBias = a_yAccumulator; + a_zBias = (a_zAccumulator - 250); + + //Reset accumulators. + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + +} + +void IMU::initializeGyroscope(void) { + + //Low pass filter bandwidth of 42Hz. + gyroscope.setLpBandwidth(LPFBW_42HZ); + //Internal sample rate of 200Hz. + gyroscope.setSampleRateDivider(4); + +} + +void IMU::calibrateGyroscope(void) { + + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + + //Accumulate a certain number of samples. + for (int i = 0; i < CALIBRATION_SAMPLES; i++) { + + w_xAccumulator += gyroscope.getGyroX(); + w_yAccumulator += gyroscope.getGyroY(); + w_zAccumulator += gyroscope.getGyroZ(); + wait(gyroscopeRate_); + + } + + //Average the samples. + w_xAccumulator /= CALIBRATION_SAMPLES; + w_yAccumulator /= CALIBRATION_SAMPLES; + w_zAccumulator /= CALIBRATION_SAMPLES; + + //Set the null bias. + w_xBias = w_xAccumulator; + w_yBias = w_yAccumulator; + w_zBias = w_zAccumulator; + + //Reset the accumulators. + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + +} + +void IMU::sampleGyroscope(void) { + + //If we've taken the required number of samples then, + //average the samples, removed the null bias and convert the units + //to rad/s. + if (gyroscopeSamples == SAMPLES) { + + w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); + w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); + w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); + + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + gyroscopeSamples = 0; + + } + //Accumulate another sample. + else { + + w_xAccumulator += gyroscope.getGyroX(); + w_yAccumulator += gyroscope.getGyroY(); + w_zAccumulator += gyroscope.getGyroZ(); + + gyroscopeSamples++; + + } + +} + +void IMU::sample(void) { + + //If we've taken enough samples then, + //average the samples, remove the offsets and convert to appropriate units. + //Feed this information into the filter to calculate the new Euler angles. + if (accelerometerSamples == SAMPLES) { + + a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; + a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; + a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; + + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + + accelerometerSamples = 0; + + w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); + w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); + w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); + + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + gyroscopeSamples = 0; + + //Update the filter variables. + imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); + //Calculate the new Euler angles. + imuFilter.computeEuler(); + + } + //Accumulate another sample. + else { + + accelerometer.getOutput(readings); + + a_xAccumulator += (int16_t) readings[0]; + a_yAccumulator += (int16_t) readings[1]; + a_zAccumulator += (int16_t) readings[2]; + + w_xAccumulator += gyroscope.getGyroX(); + w_yAccumulator += gyroscope.getGyroY(); + w_zAccumulator += gyroscope.getGyroZ(); + + accelerometerSamples++; + + } + +} + +void IMU::filter(void) { + + //Update the filter variables. + imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); + //Calculate the new Euler angles. + imuFilter.computeEuler(); + +} + +void IMU::reset(void) { + + //Disable interrupts. + sampleTicker.detach(); + + //Recalibrate sensors. + calibrateAccelerometer(); + calibrateGyroscope(); + + //Reset the IMU filter. + //imuFilter.reset(); + + //Reset the working variables. + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + accelerometerSamples = 0; + gyroscopeSamples = 0; + + //Enable interrupts. + sampleTicker.attach(this, &IMU::sample, accelerometerRate_); + +}
diff -r 000000000000 -r 7440a03255a7 IMU.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU.h Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,210 @@ +/** + * @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 + * + * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using + * orientation filter developed by Sebastian Madgwick. + * + * Find more details about his paper here: + * + * http://code.google.com/p/imumargalgorithm30042010sohm/ + */ + +#ifndef MBED_IMU_H +#define MBED_IMU_H + +/** + * Includes + */ +#include "mbed.h" +#include "ADXL345.h" +#include "ITG3200.h" +#include "IMUfilter.h" + +/** + * Defines + */ +#define IMU_RATE 0.025 +#define ACCELEROMETER_RATE 0.005 +#define GYROSCOPE_RATE 0.005 +#define GYRO_MEAS_ERROR 0.3 //IMUfilter tuning parameter. + +//Gravity at Earth's surface in m/s/s +#define g0 9.812865328 +//Number of samples to average +#define SAMPLES 4 +#define CALIBRATION_SAMPLES 128 +//Multiply radians to get degrees. +#define toDegrees(x) (x * 57.2957795) +//Multiply degrees to get radians. +#define toRadians(x) (x * 0.01745329252) +//Full scale resolution on the ADXL345 is 4mg/LSB. +//Multiply ADC count readings from ADXL345 to get acceleration in m/s/s. +#define toAcceleration(x) (x * (4 * g0 * 0.001)) +//14.375 LSB/(degrees/sec) +#define GYROSCOPE_GAIN (1 / 14.375) +#define ACCELEROMETER_GAIN (0.004 * g0) + +/** + * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope to calculate + * roll, pitch and yaw angles. + */ +class IMU { + +public: + + /** + * Constructor. + * + * @param imuRate Rate which IMUfilter update and Euler angle calculation + * occurs. + * @param gyroscopeMeasurementError IMUfilter tuning parameter. + * @param accelerometerRate Rate at which accelerometer data is sampled. + * @param gyroscopeRate Rate at which gyroscope data is sampled. + */ + IMU(float imuRate, + double gyroscopeMeasurementError, + float accelerometerRate, + float gyroscopeRate); + + /** + * Get the current roll angle. + * + * @return The current roll angle in degrees. + */ + double getRoll(void); + + /** + * Get the current pitch angle. + * + * @return The current pitch angle in degrees. + */ + double getPitch(void); + + /** + * Get the current yaw angle. + * + * @return The current yaw angle in degrees. + */ + double getYaw(void); + + /** + * Sample the sensors, and if enough samples have been taken, + * take an average, and compute the new Euler angles. + */ + void sample(void); + + /** + * Recalibrate the sensors and reset the IMU filter. + */ + void reset(void); + +private: + + /** + * Set up the ADXL345 appropriately. + */ + void initializeAccelerometer(void); + + /** + * Calculate the zero g offset. + */ + void calibrateAccelerometer(void); + + /** + * Take a set of samples and average them. + */ + void sampleAccelerometer(void); + + /** + * Set up the ITG-3200 appropriately. + */ + void initializeGyroscope(void); + + /** + * Calculate the bias offset. + */ + void calibrateGyroscope(void); + + /** + * Take a set of samples and average them. + */ + void sampleGyroscope(void); + + /** + * Update the filter and calculate the Euler angles. + */ + void filter(void); + + ADXL345 accelerometer; + ITG3200 gyroscope; + IMUfilter imuFilter; + + Ticker accelerometerTicker; + Ticker gyroscopeTicker; + Ticker sampleTicker; + Ticker filterTicker; + + float accelerometerRate_; + float gyroscopeRate_; + float imuRate_; + + //Offsets for the gyroscope. + //The readings we take when the gyroscope is stationary won't be 0, so we'll + //average a set of readings we do get when the gyroscope is stationary and + //take those away from subsequent readings to ensure the gyroscope is offset + //or biased to 0. + double w_xBias; + double w_yBias; + double w_zBias; + + double a_xBias; + double a_yBias; + double a_zBias; + + volatile double a_xAccumulator; + volatile double a_yAccumulator; + volatile double a_zAccumulator; + volatile double w_xAccumulator; + volatile double w_yAccumulator; + volatile double w_zAccumulator; + + //Accelerometer and gyroscope readings for x, y, z axes. + volatile double a_x; + volatile double a_y; + volatile double a_z; + volatile double w_x; + volatile double w_y; + volatile double w_z; + + //Buffer for accelerometer readings. + int readings[3]; + int accelerometerSamples; + int gyroscopeSamples; + +}; + +#endif /* MBED_IMU_H */
diff -r 000000000000 -r 7440a03255a7 IMUfilter.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMUfilter.lib Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510
diff -r 000000000000 -r 7440a03255a7 ITG3200.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200.lib Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/ITG3200/#b098d99dd81e
diff -r 000000000000 -r 7440a03255a7 Motor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/Motor/#c75b234558af
diff -r 000000000000 -r 7440a03255a7 PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r 7440a03255a7 QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r 7440a03255a7 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,345 @@ +/** + * Includes. + */ +#include "Motor.h" +#include "QEI.h" +#include "PID.h" +#include "IMU.h" + +/** + * Defines. + */ +#define BUFFER_SIZE 1024 //Used for data logging arrays +#define RATE 0.01 //PID loop timing + +//PID tuning constants. +#define L_KC 0.4312 //Forward left motor Kc +#define L_TI 0.1 //Forward left motor Ti +#define L_TD 0.0 //Forward left motor Td +#define R_KC 0.4620 //Forward right motor Kc +#define R_TI 0.1 //Forward right motor Ti +#define R_TD 0.0 //Forward right motor Td + +//PID input/output limits. +#define L_INPUT_MIN 0 //Forward left motor minimum input +#define L_INPUT_MAX 3000 //Forward left motor maximum input +#define L_OUTPUT_MIN 0.0 //Forward left motor minimum output +#define L_OUTPUT_MAX 0.9 //Forward left motor maximum output + +#define R_INPUT_MIN 0 //Forward right motor input minimum +#define R_INPUT_MAX 3200 //Forward right motor input maximum +#define R_OUTPUT_MIN 0.0 //Forward right motor output minimum +#define R_OUTPUT_MAX 0.9 //Forward right motor output maximum + +//Physical dimensions. +#define PULSES_PER_REV 624 +#define WHEEL_DIAMETER 58.928 //mm +#define ROTATION_DISTANCE 220.0 //mm +#define REVS_PER_ROTATION (ROTATION_DISTANCE / WHEEL_DIAMETER) +#define PULSES_PER_ROTATION (REVS_PER_ROTATION * PULSES_PER_REV) +#define PULSES_PER_MM (PULSES_PER_REV / WHEEL_DIAMETER) +#define DISTANCE_PER_PULSE (WHEEL_DIAMETER / PULSES_PER_REV) +#define ENCODING 2 //Use X2 encoding +#define WHEEL_DISTANCE (ROTATION_DISTANCE / DISTANCE_PER_PULSE) + +/** + * Function Prototypes + */ +void initializeMotors(void); +void initializePid(void); +//cmd = "move", "turn" +//direction = "forward", "backward", "left", "right" +//length = distance in metres or angle in degrees +void processCommand(char* cmd, char* direction, float length); + +/** + * Globals + */ + +//Debug. +Serial pc(USBTX, USBRX); + +//Motor control. +Motor leftMotor(p21, p20, p19); //pwm, inB, inA +Motor rightMotor(p22, p17, p18); //pwm, inA, inB +QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr +QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr +PID leftPid(L_KC, L_TI, L_TD, RATE); //Kc, Ti, Td +PID rightPid(R_KC, R_TI, R_TD, RATE); //Kc, Ti, Td +//IMU and peripherals run at a different rate to the main PID loop. +IMU imu(IMU_RATE, GYRO_MEAS_ERROR, ACCELEROMETER_RATE, GYROSCOPE_RATE); + +//Filesystem. +LocalFileSystem local("local"); + +//Left motor working variables. +int leftPulses = 0; +int leftPrevPulses = 0; +float leftVelocity = 0.0; +float leftVelocityBuffer[BUFFER_SIZE]; + +//Right motor working variables. +int rightPulses = 0; +int rightPrevPulses = 0; +float rightVelocity = 0.0; +float rightVelocityBuffer[BUFFER_SIZE]; + +//General working variables. +float heading = 0.0; +float prevHeading = 0.0; +float degreesTurned = 0.0; +float positionSetPoint = 0.0; +float headingSetPoint = 0.0; + +//Store the initial and end heading during a straight line section +//in order to be able to correct turns. +float startHeading = 0.0; +float endHeading = 0.0; + +//Command Parser. +char cmd0[16]; //{"move", "turn"} +char cmd1[16]; //{{"forward", "backward"}, {"left", "right"}} +float cmd2 = 0; //{distance in METRES, angle in DEGREES} + +void initializeMotors(void) { + + //Set motor PWM periods to 20KHz. + leftMotor.period(0.00005); + leftMotor.speed(0.0); + + rightMotor.period(0.00005); + rightMotor.speed(0.0); + +} + +void initializePid(void) { + + //Input and output limits have been determined + //empirically with the specific motors being used. + //Change appropriately for different motors. + //Input units: counts per second. + //Output units: PwmOut duty cycle as %. + //Default limits are for moving forward. + leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX); + leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX); + leftPid.setMode(AUTO_MODE); + rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX); + rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX); + rightPid.setMode(AUTO_MODE); + +} + +void processCommand(char* cmd, char* direction, float length) { + + //move command. + if (strcmp(cmd, "move") == 0) { + + int i = 0; //Data log array index. + + //The PID controller works in the % (unsigned) domain, so we'll + //need to multiply the output by -1 if our motors need + //to go "backwards". + int leftDirection = 1; + int rightDirection = 1; + + //Convert from metres into millimetres. + length *= 1000; + //Work out how many pulses are required to go that many millimetres. + length *= PULSES_PER_MM; + //Make sure we scale the number of pulses according to our encoding method. + length /= ENCODING; + + positionSetPoint = length; + + if (strcmp(direction, "forward") == 0) { + //Leave left and rightDirection as +ve. + } else if (strcmp(cmd1, "backward") == 0) { + //Change left and rightDirection to -ve. + leftDirection = -1; + rightDirection = -1; + } + + startHeading = imu.getYaw(); + + //With left set point == right set point, the rover veers off to the + //right - by slowing the right motor down slightly it goes relatively + //straight. + leftPid.setSetPoint(1000); + rightPid.setSetPoint(975); + + //Keep going until we've moved the correct distance defined by the + //position set point. Take the absolute value of the pulses as we might + //be moving backwards. + while ((abs(leftPulses) < positionSetPoint) && + (abs(rightPulses) < positionSetPoint)) { + + //Get the current pulse count and subtract the previous one to + //calculate the current velocity in counts per second. + leftPulses = leftQei.getPulses(); + leftVelocity = (leftPulses - leftPrevPulses) / RATE; + leftPrevPulses = leftPulses; + //Use the absolute value of velocity as the PID controller works + //in the % (unsigned) domain and will get confused with -ve values. + leftPid.setProcessValue(fabs(leftVelocity)); + leftMotor.speed(leftPid.compute() * leftDirection); + + rightPulses = rightQei.getPulses(); + rightVelocity = (rightPulses - rightPrevPulses) / RATE; + rightPrevPulses = rightPulses; + rightPid.setProcessValue(fabs(rightVelocity)); + rightMotor.speed(rightPid.compute() * rightDirection); + + i++; + + wait(RATE); + + } + + leftMotor.brake(); + rightMotor.brake(); + + endHeading = imu.getYaw(); + + } + //turn command. + else if (strcmp(cmd0, "turn") == 0) { + + //The PID controller works in the % (unsigned) domain, so we'll + //need to multiply the output by -1 for the motor which needs + //to go "backwards". + int leftDirection = 1; + int rightDirection = 1; + headingSetPoint = length + (endHeading - startHeading); + + //In case the rover tries to [pointlessly] turn >360 degrees. + if (headingSetPoint > 359.8) { + + headingSetPoint -= 359.8; + + } + + //Set up the right previous heading for the initial degreesTurned + //calculation. + prevHeading = fabs(imu.getYaw()); + + if (strcmp(cmd1, "left") == 0) { + + //When turning left, the left motor needs to go backwards + //while the right motor goes forwards. + leftDirection = -1; + + } else if (strcmp(cmd1, "right") == 0) { + + //When turning right, the right motor needs to go backwards + //while the left motor goes forwards. + rightDirection = -1; + + } + + //Turn slowly to ensure we don't overshoot the angle by missing + //the relatively slow readings [in comparison to the PID loop speed] + //from the IMU. + leftPid.setSetPoint(500); + rightPid.setSetPoint(500); + + //Keep turning until we've moved through the correct angle as defined + //by the heading set point. + while (degreesTurned < headingSetPoint) { + + //Get the new heading and subtract the previous heading to + //determine how many more degrees we've moved through. + //As we're only interested in the relative angle (as opposed to + //absolute) we'll take the absolute value of the heading to avoid + //any nastiness when trying to calculate angles as they jump from + //179.9 to -179.9 degrees. + heading = fabs(imu.getYaw()); + degreesTurned += fabs(heading - prevHeading); + prevHeading = heading; + + //Get the current pulse count and subtract the previous one to + //calculate the current velocity in counts per second. + leftPulses = leftQei.getPulses(); + leftVelocity = (leftPulses - leftPrevPulses) / RATE; + leftPrevPulses = leftPulses; + //Use the absolute value of velocity as the PID controller works + //in the % (unsigned) domain and will get confused with -ve values. + leftPid.setProcessValue(fabs(leftVelocity)); + leftMotor.speed(leftPid.compute() * leftDirection); + + rightPulses = rightQei.getPulses(); + rightVelocity = (rightPulses - rightPrevPulses) / RATE; + rightPrevPulses = rightPulses; + rightPid.setProcessValue(fabs(rightVelocity)); + rightMotor.speed(rightPid.compute() * rightDirection); + + wait(RATE); + + } + + leftMotor.brake(); + rightMotor.brake(); + + } + + //Reset working variables. + leftQei.reset(); + rightQei.reset(); + + leftPulses = 0; + leftPrevPulses = 0; + leftVelocity = 0.0; + leftMotor.speed(0.0); + + rightPulses = 0; + rightPrevPulses = 0; + rightVelocity = 0.0; + rightMotor.speed(0.0); + + leftPid.setSetPoint(0.0); + leftPid.setProcessValue(0.0); + rightPid.setSetPoint(0.0); + rightPid.setProcessValue(0.0); + + positionSetPoint = 0.0; + headingSetPoint = 0.0; + heading = 0.0; + prevHeading = 0.0; + degreesTurned = 0.0; + +} + +int main() { + + pc.printf("Starting mbed rover test...\n"); + + initializeMotors(); + initializePid(); + + //Some delay before we start moving. + wait(5); + + //Open the command script. + FILE* commands = fopen("/local/commands.txt", "r"); + + //Check if we were successful in opening the command script. + if (commands == NULL) { + pc.printf("Could not open command file...\n"); + exit(EXIT_FAILURE); + } else { + pc.printf("Succesfully opened command file!\n"); + } + + //While there's another line to read from the command script. + while (fscanf(commands, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) { + + pc.printf("%s %s %f\n", cmd0, cmd1, cmd2); + + processCommand(cmd0, cmd1, cmd2); + + wait(1); + + } + + fclose(commands); + +}
diff -r 000000000000 -r 7440a03255a7 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Sep 10 14:03:00 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/74b8d43b5817