StingerBot Project
Dependencies: mbed HMC6352 mbed-rtos
RobotControl.h
- Committer:
- Strikewolf
- Date:
- 2014-04-22
- Revision:
- 2:56eb726bdb0d
- Parent:
- 1:41cee26b35cc
File content as of revision 2:56eb726bdb0d:
//******************************************* //* Robot motor control and drive functions * //******************************************* #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(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 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) { 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(); } //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(); }