![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StingerBot Project
Dependencies: mbed HMC6352 mbed-rtos
Diff: RobotControl.h
- Revision:
- 2:56eb726bdb0d
- Parent:
- 1:41cee26b35cc
--- a/RobotControl.h Fri Apr 11 17:54:21 2014 +0000 +++ b/RobotControl.h Tue Apr 22 23:16:32 2014 +0000 @@ -4,17 +4,38 @@ #include "HMC6352.h" +//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 + +//Debug +Serial pc(USBTX, USBRX); + //Gyro HMC6352 compass(p28, p27); //H-bridge -PwmOut rightMotorPWM(p21); //Channel A -PwmOut leftMotorPWM(p22); //Channel B +PwmOut rightMotorPWM(p22); //Channel A +PwmOut leftMotorPWM(p21); //Channel B DigitalOut leftMotor1(p23); DigitalOut leftMotor2(p24); 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(); + } +} + //Non-feedback corrected 'dumb' driving void setDriveStraight(float speed, bool reverse) { //Set speed @@ -28,6 +49,30 @@ 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; @@ -38,21 +83,130 @@ rightMotor2 = 0; } -//These functions are not completed yet... -void gyroDriveStraight(float speed, bool reverse); +//Turn left about the center void centerTurnLeft(int delta_degrees) { - double initial = compass.sample() / 10; - while ((compass.sample() / 10) > initial - delta_degrees){ - //Set speed - rightMotorPWM = 0.8; - leftMotorPWM = 0.8; - leftMotor1 = 1; - leftMotor2 = 0; - rightMotor1 = 1; - rightMotor1 = 0; + prepCompass(); + + float initial = (compass.sample() / 10); + float target = initial - delta_degrees; + double sample; + bool rollover = false; + + 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; } stop(); } -void centerTurnRight(int delta_degrees); -void driveTurnLeft(int delta_degrees); -void driveTurnRight(int delta_degrees); + +//Turn right about the center +void centerTurnRight(int delta_degrees) +{ + prepCompass(); + + float initial = (compass.sample() / 10); + float target = initial + delta_degrees; + double sample; + bool rollover = false; + + if (target > 360) { + 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) { + 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(); +} + +//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); + + 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); + } + } + stop(); +}