Mine, not yours justin
Dependencies: HMC6352 USBHost mbed-rtos mbed
Fork of Project by
RobotControl.h
- Committer:
- ganeshsubram1
- Date:
- 2014-04-25
- Revision:
- 3:6a7620e9abd9
- Parent:
- 2:56eb726bdb0d
File content as of revision 3:6a7620e9abd9:
//******************************************* //* 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"); } } } }