Mine, not yours justin
Dependencies: HMC6352 USBHost mbed-rtos mbed
Fork of Project by
Revision 3:6a7620e9abd9, committed 2014-04-25
- Comitter:
- ganeshsubram1
- Date:
- Fri Apr 25 23:06:47 2014 +0000
- Parent:
- 2:56eb726bdb0d
- Child:
- 4:b2a30c313297
- Commit message:
- first commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PositionSystem.h Fri Apr 25 23:06:47 2014 +0000 @@ -0,0 +1,77 @@ +#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 Tue Apr 22 23:16:32 2014 +0000 +++ b/RobotControl.h Fri Apr 25 23:06:47 2014 +0000 @@ -1,9 +1,14 @@ //******************************************* //* 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 @@ -11,13 +16,10 @@ #define CHAN2_MIN 2290 //Elevator Lo #define CHAN1_MAX 2160 //Rudder Hi #define CHAN1_MIN 1210 //Rudder Lo - -//Debug -Serial pc(USBTX, USBRX); - + //Gyro HMC6352 compass(p28, p27); - + //H-bridge PwmOut rightMotorPWM(p22); //Channel A PwmOut leftMotorPWM(p21); //Channel B @@ -26,18 +28,16 @@ DigitalOut rightMotor1(p25); DigitalOut rightMotor2(p26); -//Compass sampling functions - TBA if compass keeps freaking out -int prev_sample, cur_sample; -void prepCompass() { - int i; - for (i = 0; i <= 4; i++) { - //Chew through a few samples, the compass seems to return "stale" values if unsampled for awhile - compass.sample(); - } +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 setDriveStraight(float speed, bool reverse) { +void setMove(float speed, bool reverse) { //Set speed rightMotorPWM = speed; leftMotorPWM = speed; @@ -48,7 +48,7 @@ rightMotor1 = (!reverse) ? 0 : 1; rightMotor2 = (!reverse) ? 1 : 0; } - + void setTurnLeft(float speed) { //Set speed rightMotorPWM = speed; @@ -60,7 +60,7 @@ rightMotor1 = 1; rightMotor2 = 0; } - + void setTurnRight(float speed) { //Set speed rightMotorPWM = speed; @@ -72,7 +72,7 @@ rightMotor1 = 0; rightMotor2 = 1; } - + //Stop with braking void stop() { rightMotorPWM = 0; @@ -83,130 +83,85 @@ rightMotor2 = 0; } -//Turn left about the center -void centerTurnLeft(int delta_degrees) { - prepCompass(); - - float initial = (compass.sample() / 10); - float target = initial - delta_degrees; - double sample; - bool rollover = false; +// calibrate 90 degree turns +void turnCalibrate() { - if (target < 0) { - target += 360; - rollover = true; - } - - pc.printf("Init: %f Target: %f\r\n", initial, target); - - //Take care of 360 degree rollover - if(rollover) { - pc.printf("begin rollover \r\n"); - while(true) { - setTurnLeft(0.6); - sample = compass.sample() / 10; - wait(0.02); - if (sample > 345) - break; - } - pc.printf("rollover complete\r\n"); - } - - //Continue to turn to target - while (true) { - setTurnLeft(0.6); - sample = compass.sample()/10; - wait(0.02); - if(sample < target) - break; - } + // 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 centerTurnRight(int delta_degrees) +void turnRight(int delta_degrees) { - prepCompass(); + pc.printf("Turnleft: \n\r"); + + // turn positioning system off during turns since + // no x and y movements + PFlag = PFLAG_OFF; - float initial = (compass.sample() / 10); - float target = initial + delta_degrees; - double sample; - bool rollover = false; + // store new orientation after this turn executes + t_position += 90; + t_position %= 360; + + // initialize turn variables + turnInit(); - if (target > 360) { - target -= 360; - rollover = true; + // 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); } - - pc.printf("Init: %f Target: %f\r\n", initial, target); - - //Take care of 360 degree rollover - if(rollover) { - pc.printf("begin rollover \r\n"); - - while(true) { - setTurnRight(0.6); - sample = compass.sample() / 10; - wait(0.02); - if (sample > 0 && sample < 10) - break; - } - pc.printf("rollover complete\r\n"); - } - - //Continue to turn to target - while (true) { - setTurnRight(0.6); - sample = compass.sample()/10; - wait(0.02); - if(sample > target) - break; - } + + // stop turning stop(); + + // turn positioning system back on + PFlag = PFLAG_ON; } //Drive straight with compass correction -void compassDriveStraight(float speed, bool reverse, int ms) { - Timer timer; - double sample; - - setDriveStraight(speed, reverse); - wait(0.2); - //really hacky way to compensate for motor EM interference - prepCompass(); - float initial = compass.sample() / 10; - - //Really hacky way to deal with 360 rollover - if (initial + 6 > 360) - centerTurnLeft(7); - if (initial - 6 < 0) - centerTurnRight(7); +void move(float speed, bool reverse) { + + // begin moving + setMove(speed, reverse); - timer.start(); - while (timer.read_ms() < ms) { - sample = compass.sample() / 10; - pc.printf("%f :::", sample); - - if (sample > initial + 3) { - pc.printf("Steer Left\r\n"); - leftMotorPWM = speed - 0.1; - rightMotorPWM = speed + 0.1; - leftMotor1 = 0; - leftMotor2 = 1; - rightMotor1 = 0; - rightMotor2 = 1; - } else if (sample < initial - 3) { - pc.printf("Steer Right \r\n"); - leftMotorPWM = speed + 0.1; - rightMotorPWM = speed - 0.1; - leftMotor1 = 0; - leftMotor2 = 1; - rightMotor1 = 0; - rightMotor2 = 1; - } else { - pc.printf("Straight\r\n"); - setDriveStraight(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"); + } + } + } - stop(); -} + } + + \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SonarSystem.h Fri Apr 25 23:06:47 2014 +0000 @@ -0,0 +1,8 @@ +#include "mbed.h" + +#define SONAR_STOP (1.8) + +AnalogIn sonar1(p16); // FL +AnalogIn sonar2(p17); // FC +AnalogIn sonar3(p15); // FR +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBHost.lib Fri Apr 25 23:06:47 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/USBHost/#b010bddab593
--- a/main.cpp Tue Apr 22 23:16:32 2014 +0000 +++ b/main.cpp Fri Apr 25 23:06:47 2014 +0000 @@ -1,48 +1,41 @@ #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); } - -void InitCompass() { - //Init compass - compass.setReset(); - compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); + +int main() { - //Calibration level - Garrus - //Rotate robot one full rotation during this section - pc.printf("Begin Calibration...\r\n"); - compass.setCalibrationMode(ENTERCALIB); - wait(10); - compass.setCalibrationMode(EXITCALIB); - - //Sample a few to clean out buffer - compass.sample(); - compass.sample(); - compass.sample(); -} - -int main() { //Emergency stop mechanism sw.rise(&StopISR); - - //Setup mode and perform calibration - InitCompass(); - compassDriveStraight(0.7, false, 20000); + // 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(); - while(1) { - float sample = compass.sample() / 10; - pc.printf("%f\r\n", sample); - } -} + // 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