Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the human driver (RF controller) program for the Robotics Cat and Mouse program.
Dependencies: ADXL345_I2C_NEST HMC6352 IMUfilter ITG3200_NEST USBHost mbed-rtos mbed
Fork of Project by
Diff: RobotControl.h
- Revision:
- 5:210cd333f770
- Parent:
- 4:b2a30c313297
- Child:
- 6:3fb9f96765f6
diff -r b2a30c313297 -r 210cd333f770 RobotControl.h --- a/RobotControl.h Fri Apr 25 23:08:01 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,167 +0,0 @@ -//******************************************* -//* 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"); - } - } - - } - } - - \ No newline at end of file