Mine, not yours justin
Dependencies: HMC6352 USBHost mbed-rtos mbed
Fork of Project by
Diff: RobotControl.h
- Revision:
- 3:6a7620e9abd9
- Parent:
- 2:56eb726bdb0d
diff -r 56eb726bdb0d -r 6a7620e9abd9 RobotControl.h --- 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