Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the human driver (RF controller) program for the Robotics Cat and Mouse program.
Dependencies: ADXL345_I2C_NEST HMC6352 IMUfilter ITG3200_NEST USBHost mbed-rtos mbed
Fork of Project by
Revision 5:210cd333f770, committed 2014-04-30
- Comitter:
- Strikewolf
- Date:
- Wed Apr 30 05:53:51 2014 +0000
- Parent:
- 4:b2a30c313297
- Child:
- 6:3fb9f96765f6
- Commit message:
- works without any motors
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ADXL345_I2C.lib Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/peterswanson87/code/ADXL345_I2C/#7e41b9136e7a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AI.cpp Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,53 @@ +#include "mbed.h" +#include "rtos.h" +#include "IMU_RPY.h" +#include "GameCode.h" +#include "AIPosition.h" +#include "AIControl.h" + +InterruptIn sw(p30); + +void StopISR() +{ + rightMotorPWM = 0; + leftMotorPWM = 0; + exit(1); +} + +int main() +{ + pc.printf("\n\rStartup...\r\n"); + + //Emergency stop mechanism + sw.rise(&StopISR); + + //Reset xbee + pc.printf("Resetting XBee...\r\n"); + rst1 = 0; + wait_us(1); + rst1 = 1; + wait_us(1); + + //Initialize IMU and filter + pc.printf("Initializing IMU...\r\n"); + imuFilter.reset(); + rpy_init(); + + // start positioning system + pc.printf("Starting Positioning System...\n\r"); + Thread position(PositionSystemMain, NULL, osPriorityRealtime, 256 * 4); + wait(3); // wait for the mouse sensor to boot up + + // get away from user before starting game + pc.printf("Getting head start (straight)...\n\r"); + wait(5); + // gyroDriveStraight(0.7, false, 10000); + // pc.printf("Getting head start (turn right)...\n\r"); + // centerTurnRight(90); + + // bursts of 10 seconds for driving straight + pc.printf("Running!\r\n"); + while(!gameOver) { + gyroDriveStraight(0.7, true, 10000); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AIControl.h Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,232 @@ +//******************************************* +//* Robot motor control and drive functions * +//******************************************* + +#define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms + +//Radio Calibration Signals - Need to implement calibration routine... +#define CHAN3_MAX 2060 //Throttle Hi +#define CHAN3_MIN 1150 ` //Throttle Lo +#define CHAN2_MAX 1260 //Elevator Hi +#define CHAN2_MIN 2290 //Elevator Lo +#define CHAN1_MAX 2160 //Rudder Hi +#define CHAN1_MIN 1210 //Rudder Lo + +//H-bridge +PwmOut rightMotorPWM(p22); //Channel A +PwmOut leftMotorPWM(p21); //Channel B +DigitalOut leftMotor1(p23); +DigitalOut leftMotor2(p24); +DigitalOut rightMotor1(p25); +DigitalOut rightMotor2(p26); + +//Angle state variable +int rtheta; + + +//Non-feedback corrected 'dumb' driving +void setMove(float speed, bool reverse) +{ + //Set speed + rightMotorPWM = speed; + leftMotorPWM = speed; + + //Set motor function + leftMotor1 = (!reverse) ? 0 : 1; + leftMotor2 = (!reverse) ? 1 : 0; + rightMotor1 = (!reverse) ? 0 : 1; + rightMotor2 = (!reverse) ? 1 : 0; +} + +void setTurnLeft(float speed) +{ + //Set speed + rightMotorPWM = speed; + leftMotorPWM = speed; + + //Set motor function + leftMotor1 = 0; + leftMotor2 = 1; + rightMotor1 = 1; + rightMotor2 = 0; +} + +void setTurnRight(float speed) +{ + //Set speed + rightMotorPWM = speed; + leftMotorPWM = speed; + + //Set motor function + leftMotor1 = 1; + leftMotor2 = 0; + rightMotor1 = 0; + rightMotor2 = 1; +} + +//Stop with braking +void stop() +{ + rightMotorPWM = 0; + leftMotorPWM = 0; + leftMotor1 = 0; + leftMotor2 = 1; + rightMotor1 = 1; + rightMotor2 = 0; +} + +//Turn left about the center +void centerTurnLeft(int delta_degrees) +{ + //Reset the filter and re-init the IMU + imuFilter.reset(); + rpy_init(); + + //So there's this weird thing where the gyro treats left turns as POSITIVE degrees... + float initial = toDegrees(imuFilter.getYaw()); + float target = initial + delta_degrees; + double sample; + // pc.printf("Init: %f Target: %f\r\n", initial, target); + + //Continue to turn to target + while (!gameOver) { + setTurnLeft(0.6); + sample = toDegrees(imuFilter.getYaw()); + wait(0.02); + if(sample > target - 5) + break; + } + stop(); +} + +//Turn right about the center +void centerTurnRight(int delta_degrees) +{ + //Reset the filter and re-init the IMU + imuFilter.reset(); + rpy_init(); + + //So there's this weird thing where the gyro treats right turns as NEGATIVE degrees... + float initial = toDegrees(imuFilter.getYaw()); + float target = initial - delta_degrees; + double sample; + + // pc.printf("Init: %f Target: %f\r\n", initial, target); + + //Continue to turn to target + while (!gameOver) { + setTurnRight(0.6); + sample = toDegrees(imuFilter.getYaw()); + wait(0.02); + if(sample < target) + break; + } + stop(); +} + +void checkSonars() +{ + //Collision Detection + // read all sonar sensors + float s1 = sonar1.read() * 100; + float s2 = sonar2.read() * 100; + float s3 = sonar3.read() * 100; + + // check if obstacle is near and adjust + pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP); + if(s2 < SONAR_STOP) { + + if(s1 <= s3) { + centerTurnRight(90); + } else { + centerTurnLeft(90); + } + imuFilter.reset(); + rpy_init(); + } +} + +//Drive straight with compass correction +void gyroDriveStraight(float speed, bool gameRun, int ms) +{ + Timer timer; + double sample; + + //Reset IMU + imuFilter.reset(); + rpy_init(); + + wait(0.2); + + timer.start(); + while (timer.read_ms() < ms) { + + timer.stop(); + //checkSonars(); + if(gameRun) { + receivePosition(NULL); + } + timer.start(); + + imuFilter.computeEuler(); + sample = toDegrees(imuFilter.getYaw()); +/* + //Drift Correction + sample = sample + float(timer.read_ms() / 800); + // pc.printf("%f :::", sample); + + if (sample < 3) { + // pc.printf("Steer Left\r\n"); + leftMotorPWM = speed - 0.2; + rightMotorPWM = speed; + leftMotor1 = 0; + leftMotor2 = 1; + rightMotor1 = 0; + rightMotor2 = 1; + } else if (sample > -3) { + // pc.printf("Steer Right \r\n"); + leftMotorPWM = speed; + rightMotorPWM = speed - 0.18; + leftMotor1 = 0; + leftMotor2 = 1; + rightMotor1 = 0; + rightMotor2 = 1; + } else { + // pc.printf("Straight\r\n"); + setMove(speed, false); + }*/ + } + stop(); +} + +//Check where the evil honeywell-wielding human scum is and perform avoidance +void avoidHoneywell() +{ + //Grab telemetry from human car + int human_x = x_hum; + int human_y = y_hum; + + //Calculate avoidance angle + int dx = x_position - human_x; + int dy = y_position - human_y; + + //Set to current angle just in case everything screws up + int desiredAngle = 0;// t_position; + + if (dx >= 0 && dy >= 0) + desiredAngle = (dx > dy) ? 180 : 270; + if (dx >= 0 && dy < 0) + desiredAngle = (dx > dy) ? 180 : 90; + if (dx < 0 && dy >= 0 ) + desiredAngle = (dx > dy) ? 0 : 270; + if (dx < 0 && dy < 0) + desiredAngle = (dx > dy) ? 0 : 90; + + int delta_angle = 0;// t_position - desiredAngle; + if (delta_angle >= 0) { + //Right turn to some degree + centerTurnRight(delta_angle); + } else { + centerTurnLeft(delta_angle); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AIPosition.h Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,168 @@ +#include <math.h> +#include "USBHostMouse.h" + +#define SONAR_STOP (2.0) + +#define PFLAG_ON 0 +#define PFLAG_OFF 1 +#define PFLAG_CALIB 2 + +#define SERIAL_BUFFER_SIZE 20 +#define PACKET_SIZE 10 + +#define X_HUMAN_UPPER 1 +#define X_HUMAN_LOWER 2 +#define Y_HUMAN_UPPER 4 +#define Y_HUMAN_LOWER 5 + +//Convert from radians to degrees. +#define toDegrees(x) (x * 57.2957795) + +//Quadrant enum +typedef enum {FORWARD_LEFT, FORWARD_RIGHT, BACKWARD_LEFT, BACKWARD_RIGHT} QUADRANT; + +// sonar sensors +AnalogIn sonar1(p16); // FL +AnalogIn sonar2(p17); // FC +AnalogIn sonar3(p18); // FR + +// updates xy position if on, does nothing if off +extern char PFlag = PFLAG_ON; + +// variables to keep track of coordinate position +double x_position = 0; +double y_position = 0; + +double x_hum = 123456; +double y_hum = 123456; + +// human position +Serial xbee(p13, p14); +DigitalOut rst1(p15); + +/* mouse event handler */ +void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z) +{ + // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch + double y_temp = y_mickey / 232.6 * 100; + double g_temp = imuFilter.getYaw(); + + // determine direction we are facing and add to that direction + //y_position += y_temp; + //x_position += y_temp * tan(g_temp); + double dx = 0; + double dy = 0; + QUADRANT quadrant; + if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90) + quadrant = FORWARD_LEFT; + if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90) + quadrant = FORWARD_RIGHT; + if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180) + quadrant = BACKWARD_LEFT; + if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180) + quadrant = BACKWARD_RIGHT; + + switch (quadrant) { + case FORWARD_LEFT: + dy = y_temp * cos(g_temp); + dx = -y_temp * sin(g_temp); + break; + case FORWARD_RIGHT: + dy = y_temp * cos(g_temp); + dx = -y_temp * sin(g_temp); + break; + case BACKWARD_LEFT: + dy = -y_temp * sin(g_temp); + dx = y_temp * cos(g_temp); + break; + case BACKWARD_RIGHT: + dy = y_temp * sin(g_temp); + dx = -y_temp * cos(g_temp); + break; + } + + x_position += dx; + y_position += dy; + + // pc.printf("sin(g): %f, cos(g): %f\n\r", sin(g_temp), cos(g_temp)); + // pc.printf("DEBUG: dx: %f, dy: %f, gyro: %f, quadrant: %d\n\r", dx, dy, toDegrees(g_temp), quadrant); + // pc.printf("x: %f, y: %f, dx: %f, dy: %f, g: %f, q: %d\n\r", x_position, y_position, dx, dy, toDegrees(g_temp), quadrant); + + // check if human car is close enough to end game + gameOver = isGameOver(x_hum, y_hum, x_position, y_position); + if(gameOver) { + // game is over at this point + xbee.putc('d'); + pc.printf("Game over sent!\n\r"); + + // go to end game routine + endGame(); + } +} + +/* positioning system thread function */ +void PositionSystemMain(void const *) +{ + + USBHostMouse mouse; + + while(!gameOver) { + // try to connect a USB mouse + while(!mouse.connect() && !gameOver) + Thread::wait(500); + + // when connected, attach handler called on mouse event + mouse.attachEvent(onMouseEvent); + + // wait until the mouse is disconnected + while(mouse.connected() && !gameOver) + Thread::wait(500); + } +} + +void receivePosition(void const *) +{ + char buffer[SERIAL_BUFFER_SIZE]; + int index = 0; + int xt = 0; + int yt = 0; + + // clear any garbage + // xbee.getc(); + //xbee.getc(); + + // while(!gameOver) { + + // wait for start character + while(xbee.readable() && xbee.getc() != 'x' && !gameOver); + + // receive data packet of size PACKET_SIZE bytes + pc.printf("Receiving...\n\r"); + index = 0; + while(index < PACKET_SIZE && !gameOver) { + if(xbee.readable()) + buffer[index++] = xbee.getc(); + } + buffer[index] = NULL; + + // reassemble data + xt = buffer[1]; + xt = xt << 8; + xt = xt | buffer[2]; + xt = xt << 8; + xt = xt | buffer[3]; + xt = xt << 8; + xt = xt | buffer[4]; + x_hum = (double) xt; + + yt = buffer[6]; + yt = yt << 8; + yt = yt | buffer[7]; + yt = yt << 8; + yt = yt | buffer[8]; + yt = yt << 8; + yt = yt | buffer[9]; + y_hum = (double) yt; + + pc.printf("Recieve complete: %d, %d\n\r", (int) x_hum, (int) y_hum); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GameCode.h Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,21 @@ +#define THRESHOLD 500 + +Serial pc(USBTX, USBRX); + +bool gameOver = false; + +bool isGameOver(double x_hum, double y_hum, double x_cpu, double y_cpu) +{ + pc.printf("AI:(%f, %f), HUM: (%f, %f)\n\r", x_cpu, y_cpu, x_hum, y_hum); + if(abs(x_hum - x_cpu) < THRESHOLD && abs(y_hum - y_cpu) < THRESHOLD) { + return true; + } else { + return false; + } +} + +void endGame() +{ + pc.printf("GAME OVER\n\r"); + exit(1); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMU_RPY.h Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,279 @@ +/** + * IMU filter example. + * + * Calculate the roll, pitch and yaw angles. + */ +#include "IMUfilter.h" +#include "ADXL345_I2C.h" +#include "ITG3200.h" + +//Gravity at Earth's surface in m/s/s +#define g0 9.812865328 +//Number of samples to average. +#define SAMPLES 4 +//Number of samples to be averaged for a null bias calculation +//during calibration. +#define CALIBRATION_SAMPLES 32 +//Convert from radians to degrees. +#define toDegrees(x) (x * 57.2957795) +//Convert from degrees to radians. +#define toRadians(x) (x * 0.01745329252) +//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). +#define GYROSCOPE_GAIN (1 / 14.375) +//Full scale resolution on the ADXL345 is 4mg/LSB. +#define ACCELEROMETER_GAIN (0.004 * g0) +//Sampling gyroscope at 200Hz. +#define GYRO_RATE 0.005 +//Sampling accelerometer at 200Hz. +#define ACC_RATE 0.005 +//Updating filter at 40Hz. +#define FILTER_RATE 0.1 + +//At rest the gyroscope is centred around 0 and goes between about +//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly +//5/15 = 0.3 degrees/sec. +IMUfilter imuFilter(FILTER_RATE, 0.3); +ADXL345_I2C accelerometer(p9, p10); +ITG3200 gyroscope(p9, p10); +Ticker accelerometerTicker; +Ticker gyroscopeTicker; +Ticker filterTicker; + +//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; + +//Offsets for the accelerometer. +//Same as with the gyroscope. +double a_xBias; +double a_yBias; +double a_zBias; + +//Accumulators used for oversampling and then averaging. +volatile double a_xAccumulator = 0; +volatile double a_yAccumulator = 0; +volatile double a_zAccumulator = 0; +volatile double w_xAccumulator = 0; +volatile double w_yAccumulator = 0; +volatile double w_zAccumulator = 0; + +//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]; +//Number of accelerometer samples we're on. +int accelerometerSamples = 0; +//Number of gyroscope samples we're on. +int gyroscopeSamples = 0; + +/** + * Prototypes + */ +//Set up the ADXL345 appropriately. +void initializeAcceleromter(void); +//Calculate the null bias. +void calibrateAccelerometer(void); +//Take a set of samples and average them. +void sampleAccelerometer(void); +//Set up the ITG3200 appropriately. +void initializeGyroscope(void); +//Calculate the null bias. +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); + +void 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 sampleAccelerometer(void) +{ + + //Have we taken enough samples? + if (accelerometerSamples == SAMPLES) { + + //Average the samples, remove the bias, and calculate the acceleration + //in m/s/s. + 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; + + } else { + //Take another sample. + accelerometer.getOutput(readings); + + a_xAccumulator += (int16_t) readings[0]; + a_yAccumulator += (int16_t) readings[1]; + a_zAccumulator += (int16_t) readings[2]; + + accelerometerSamples++; + + } + +} + +void calibrateAccelerometer(void) +{ + + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + + //Take a number of readings and average them + //to calculate the zero g offset. + 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(ACC_RATE); + + } + + a_xAccumulator /= CALIBRATION_SAMPLES; + a_yAccumulator /= CALIBRATION_SAMPLES; + a_zAccumulator /= CALIBRATION_SAMPLES; + + //At 4mg/LSB, 250 LSBs is 1g. + a_xBias = a_xAccumulator; + a_yBias = a_yAccumulator; + a_zBias = (a_zAccumulator - 250); + + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; + +} + +void initializeGyroscope(void) +{ + + //Low pass filter bandwidth of 42Hz. + gyroscope.setLpBandwidth(LPFBW_42HZ); + //Internal sample rate of 200Hz. (1kHz / 5). + gyroscope.setSampleRateDivider(4); + +} + +void calibrateGyroscope(void) +{ + + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + + //Take a number of readings and average them + //to calculate the gyroscope bias offset. + for (int i = 0; i < CALIBRATION_SAMPLES; i++) { + + w_xAccumulator += gyroscope.getGyroX(); + w_yAccumulator += gyroscope.getGyroY(); + w_zAccumulator += gyroscope.getGyroZ(); + wait(GYRO_RATE); + + } + + //Average the samples. + w_xAccumulator /= CALIBRATION_SAMPLES; + w_yAccumulator /= CALIBRATION_SAMPLES; + w_zAccumulator /= CALIBRATION_SAMPLES; + + w_xBias = w_xAccumulator; + w_yBias = w_yAccumulator; + w_zBias = w_zAccumulator; + + w_xAccumulator = 0; + w_yAccumulator = 0; + w_zAccumulator = 0; + +} + +void sampleGyroscope(void) +{ + + //Have we taken enough samples? + if (gyroscopeSamples == SAMPLES) { + + //Average the samples, remove the bias, and calculate the angular + //velocity in rad/s. + 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; + + } else { + //Take another sample. + w_xAccumulator += gyroscope.getGyroX(); + w_yAccumulator += gyroscope.getGyroY(); + w_zAccumulator += gyroscope.getGyroZ(); + + gyroscopeSamples++; + + } + +} + +void 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 rpy_init() +{ + //Initialize inertial sensors. + initializeAccelerometer(); + calibrateAccelerometer(); + initializeGyroscope(); + calibrateGyroscope(); + + + //Set up timers. + //Accelerometer data rate is 200Hz, so we'll sample at this speed. + accelerometerTicker.attach(&sampleAccelerometer, 0.005); + //Gyroscope data rate is 200Hz, so we'll sample at this speed. + gyroscopeTicker.attach(&sampleGyroscope, 0.005); + //Update the filter variables at the correct rate. + filterTicker.attach(&filter, FILTER_RATE); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMUfilter.lib Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200.lib Wed Apr 30 05:53:51 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/ITG3200/#9e6042257318
--- a/PositionSystem.h Fri Apr 25 23:08:01 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,77 +0,0 @@ -#include "USBHostMouse.h" -#include "HMC6352.h" - -#define PFLAG_ON 0 -#define PFLAG_OFF 1 -#define PFLAG_CALIB 2 -#define PTURN_SPEED (0.5) - -float PTURN_RIGHT = 0; - -extern Serial pc(USBTX, USBRX); - -// updates xy position if on, does nothing if off -extern char PFlag = PFLAG_ON; - -// whenever a turn is complete, this should store -// the degrees facing (0, 90, 180, 270) in system -extern int t_position = 0; - -// variables to keep track of coordinate position -float x_position = 0; -float y_position = 0; - -// variables to keep track of movement during turning -float pturn_x = 0; -float pturn_y = 0; - -/* mouse event handler */ -void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z) { - - // calculate new position - if(PFlag == PFLAG_ON) { - - // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch - float temp = y_mickey / 232.6; - - // determine direction we are facing and add to that direction - if(t_position == 0) - y_position += temp; - else if(t_position == 90) - x_position += temp; - else if(t_position == 180) - y_position -= temp; - else - x_position -= temp; - } else if(PFlag == PFLAG_CALIB) { - PTURN_RIGHT += x_mickey / 232.6; - } else { - pturn_x += x_mickey / 232.6; - pturn_y += y_mickey / 232.6; - } -} - -// intialize x and y variables for turning -void turnInit() { - pturn_x = 0; - pturn_y = 0; -} - -/* positioning system thread function */ -void PositionSystemMain(void const *) { - - USBHostMouse mouse; - - while(1) { - // try to connect a USB mouse - while(!mouse.connect()) - Thread::wait(500); - - // when connected, attach handler called on mouse event - mouse.attachEvent(onMouseEvent); - - // wait until the mouse is disconnected - while(mouse.connected()) - Thread::wait(500); - } -} \ No newline at end of file
--- a/RobotControl.h Fri Apr 25 23:08:01 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,167 +0,0 @@ -//******************************************* -//* Robot motor control and drive functions * -//******************************************* - -#include "HMC6352.h" -#include "PositionSystem.h" -#include "SonarSystem.h" -#include <math.h> - -#define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms - -//Radio Calibration Signals - Need to implement calibration routine... -#define CHAN3_MAX 2060 //Throttle Hi -#define CHAN3_MIN 1150 //Throttle Lo -#define CHAN2_MAX 1260 //Elevator Hi -#define CHAN2_MIN 2290 //Elevator Lo -#define CHAN1_MAX 2160 //Rudder Hi -#define CHAN1_MIN 1210 //Rudder Lo - -//Gyro -HMC6352 compass(p28, p27); - -//H-bridge -PwmOut rightMotorPWM(p22); //Channel A -PwmOut leftMotorPWM(p21); //Channel B -DigitalOut leftMotor1(p23); -DigitalOut leftMotor2(p24); -DigitalOut rightMotor1(p25); -DigitalOut rightMotor2(p26); - -float sampleCompass(int sample_size) { - float c = 0; - for(int i = 0; i < sample_size; i++) - c += compass.sample() / 10.0; - c = c / (double) sample_size; - return c; -} - -//Non-feedback corrected 'dumb' driving -void setMove(float speed, bool reverse) { - //Set speed - rightMotorPWM = speed; - leftMotorPWM = speed; - - //Set motor function - leftMotor1 = (!reverse) ? 0 : 1; - leftMotor2 = (!reverse) ? 1 : 0; - rightMotor1 = (!reverse) ? 0 : 1; - rightMotor2 = (!reverse) ? 1 : 0; -} - -void setTurnLeft(float speed) { - //Set speed - rightMotorPWM = speed; - leftMotorPWM = speed; - - //Set motor function - leftMotor1 = 0; - leftMotor2 = 1; - rightMotor1 = 1; - rightMotor2 = 0; -} - -void setTurnRight(float speed) { - //Set speed - rightMotorPWM = speed; - leftMotorPWM = speed; - - //Set motor function - leftMotor1 = 1; - leftMotor2 = 0; - rightMotor1 = 0; - rightMotor2 = 1; -} - -//Stop with braking -void stop() { - rightMotorPWM = 0; - leftMotorPWM = 0; - leftMotor1 = 0; - leftMotor2 = 1; - rightMotor1 = 1; - rightMotor2 = 0; -} - -// calibrate 90 degree turns -void turnCalibrate() { - - // tell positioning system to set the turn - // calibration variable, PTURN_RIGHT - PFlag = PFLAG_CALIB; - - // start turning hardcoded speed PTURN_SPEED - setTurnRight(PTURN_SPEED); - - // wait until ROTATE_90_TIME has passed. This should - // be the time to complete a 90 degree turn - wait_ms(ROTATE_90_TIME); - stop(); - - // done calibrating, turn positioning system back on - PFlag = PFLAG_ON; -} - -// ------------------------------------------------------------------- - -//Turn right about the center -void turnRight(int delta_degrees) -{ - pc.printf("Turnleft: \n\r"); - - // turn positioning system off during turns since - // no x and y movements - PFlag = PFLAG_OFF; - - // store new orientation after this turn executes - t_position += 90; - t_position %= 360; - - // initialize turn variables - turnInit(); - - // start turning right - setTurnRight(PTURN_SPEED); - - // read mouse sensor until 90 degress has completed - while(pturn_x > PTURN_RIGHT) { - pc.printf("%f, %f\n\r", pturn_x, pturn_y); - } - - // stop turning - stop(); - - // turn positioning system back on - PFlag = PFLAG_ON; -} - -//Drive straight with compass correction -void move(float speed, bool reverse) { - - // begin moving - setMove(speed, reverse); - - // blocking call - while(1) { - - // read all sonar sensors - float s1 = sonar1.read() * 100; - float s2 = sonar2.read() * 100; - float s3 = sonar3.read() * 100; - - // check if obstacle is near and adjust - if(s2 < SONAR_STOP) { - pc.printf("%.2f, %.2f, %.2f\n\r", s1, s2, s3); - if(s1 >= SONAR_STOP) { - //turnLeft(90); - } else if(s3 >= SONAR_STOP) { - turnRight(90); - } else { - pc.printf("IM STUCK HALP\n\t"); - } - } - - } - } - - \ No newline at end of file
--- a/SonarSystem.h Fri Apr 25 23:08:01 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,8 +0,0 @@ -#include "mbed.h" - -#define SONAR_STOP (1.8) - -AnalogIn sonar1(p16); // FL -AnalogIn sonar2(p17); // FC -AnalogIn sonar3(p15); // FR -
--- a/main.cpp Fri Apr 25 23:08:01 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,41 +0,0 @@ -#include "mbed.h" -#include "RobotControl.h" -#include "rtos.h" - -#define ENTERCALIB 0x43 -#define EXITCALIB 0x45 - -DigitalOut led3(LED3); -DigitalOut led4(LED4); -DigitalOut rst1(p12); - -Serial xbee1(p13, p14); - -InterruptIn sw(p30); - -void StopISR() -{ - rightMotorPWM = 0; - leftMotorPWM = 0; - exit(1); -} - -int main() { - - //Emergency stop mechanism - sw.rise(&StopISR); - - // start positioning system - Thread position(PositionSystemMain, NULL, osPriorityNormal, 256 * 4); - wait(3); // wait for the mouse sensor to boot up - - // calibrate 90 degree turns - turnCalibrate(); - - // PTURN_RIGHT is the measured x value for a 90 degree turn - pc.printf("PTURN_RIGHT: %f\n\r", PTURN_RIGHT); - wait(1); - - // try a 90 degree turn with the measured PTURN_RIGHT value - turnRight(90); -} \ No newline at end of file