Spring 2014, ECE 4180 project, Georgia Institute of Technolgoy. This is the autonomous driver program for the Robotics Cat and Mouse program.
Dependencies: IMUfilter ADXL345_I2C mbed ITG3200 USBHost mbed-rtos
HumanControl.h
- Committer:
- Strikewolf
- Date:
- 2014-04-30
- Revision:
- 3:0a6e4d139b86
- Parent:
- 1:dacf7db790f6
File content as of revision 3:0a6e4d139b86:
//Radio Calibration Signals - Need to implement calibration routine... #define CHAN2_MID 1470 //Elevator Midpoint #define CHAN1_MID 1470 //Rudder Midpoint #define DEADZONE 105 #define MAXMIN_OFFSET 400 //Drive Modes typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE; DRIVEMODE mode; float xpwm, ypwm; //H-bridge PwmOut rightMotorPWM(p22); //Channel A PwmOut leftMotorPWM(p21); //Channel B DigitalOut leftMotor1(p23); DigitalOut leftMotor2(p24); DigitalOut rightMotor1(p25); DigitalOut rightMotor2(p26); //Radio DigitalIn rudder(p11); DigitalIn elevator(p12); 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; } int getRadioX() { Timer timer; timer.reset(); int dur; while(!rudder && !gameOver); timer.start(); while(rudder && !gameOver); dur = timer.read_us(); return dur; } int getRadioY() { Timer timer; timer.reset(); int dur; while(!elevator && !gameOver); timer.start(); while(elevator && !gameOver); dur = timer.read_us(); return dur; } //Primary function for setting motors for radio driving void radioDrive() { __disable_irq(); // disable interrupts int radioY = getRadioY(); int radioX = getRadioX(); __enable_irq(); // enable interrupts //Identify drive mode from radio values if (radioY < CHAN2_MID - DEADZONE) mode = FORWARD; if (radioY > CHAN2_MID + DEADZONE) mode = REVERSE; if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE && radioX < CHAN1_MID - DEADZONE) mode = CENTERLEFT; if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE && radioX > CHAN1_MID + DEADZONE) mode = CENTERRIGHT; if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE && radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE) mode = STOP; //Normalize values for PWM magnitude xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET; ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET; //Set Motors Accordingly switch(mode) { case FORWARD: //Handle variable turns if (radioX > CHAN1_MID + DEADZONE) { rightMotorPWM = 0.2; leftMotorPWM = 0.85; } else if (radioX < CHAN1_MID - DEADZONE) { rightMotorPWM = 0.85; leftMotorPWM = 0.2; } else { rightMotorPWM = .85; leftMotorPWM = .85; } leftMotor1 = 0; leftMotor2 = 1; rightMotor1 = 0; rightMotor2 = 1; break; case REVERSE: //Handle variable turns if (radioX > CHAN1_MID + DEADZONE) { rightMotorPWM = 0.2; leftMotorPWM = 0.85; } else if (radioX < CHAN1_MID - DEADZONE) { rightMotorPWM = 0.85; leftMotorPWM = 0.2; } else { rightMotorPWM = .85; leftMotorPWM = .85; } leftMotor1 = 1; leftMotor2 = 0; rightMotor1 = 1; rightMotor2 = 0; break; case CENTERRIGHT: rightMotorPWM = xpwm; leftMotorPWM = xpwm; leftMotor1 = 1; leftMotor2 = 0; rightMotor1 = 0; rightMotor2 = 1; break; case CENTERLEFT: rightMotorPWM = xpwm; leftMotorPWM = xpwm; leftMotor1 = 0; leftMotor2 = 1; rightMotor1 = 1; rightMotor2 = 0; break; case STOP: rightMotorPWM = 0; leftMotorPWM = 0; leftMotor1 = 0; leftMotor2 = 1; rightMotor1 = 1; rightMotor2 = 0; break; } }