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
Diff: HumanControl.h
- Revision:
- 1:dacf7db790f6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HumanControl.h Wed Apr 30 05:54:17 2014 +0000 @@ -0,0 +1,178 @@ +//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; + } +} \ No newline at end of file