HumanControl.h

00001 //Radio Calibration Signals - Need to implement calibration routine...
00002 #define CHAN2_MID 1470  //Elevator Midpoint
00003 #define CHAN1_MID 1470  //Rudder Midpoint
00004 #define DEADZONE 105
00005 #define MAXMIN_OFFSET 400
00006 
00007 //Drive Modes
00008 typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
00009 DRIVEMODE mode;
00010 float xpwm, ypwm;
00011 
00012 //H-bridge
00013 PwmOut rightMotorPWM(p22);  //Channel A
00014 PwmOut leftMotorPWM(p21);   //Channel B
00015 DigitalOut leftMotor1(p23);
00016 DigitalOut leftMotor2(p24);
00017 DigitalOut rightMotor1(p25);
00018 DigitalOut rightMotor2(p26);
00019 
00020 //Radio
00021 DigitalIn rudder(p11);
00022 DigitalIn elevator(p12);
00023 
00024 void setTurnLeft(float speed)
00025 {
00026     //Set speed
00027     rightMotorPWM = speed;
00028     leftMotorPWM = speed;
00029 
00030     //Set motor function
00031     leftMotor1 = 0;
00032     leftMotor2 = 1;
00033     rightMotor1 = 1;
00034     rightMotor2 = 0;
00035 }
00036 
00037 void setTurnRight(float speed)
00038 {
00039     //Set speed
00040     rightMotorPWM = speed;
00041     leftMotorPWM = speed;
00042 
00043     //Set motor function
00044     leftMotor1 = 1;
00045     leftMotor2 = 0;
00046     rightMotor1 = 0;
00047     rightMotor2 = 1;
00048 }
00049 
00050 //Stop with braking
00051 void stop()
00052 {
00053     rightMotorPWM = 0;
00054     leftMotorPWM = 0;
00055     leftMotor1 = 0;
00056     leftMotor2 = 1;
00057     rightMotor1 = 1;
00058     rightMotor2 = 0;
00059 }
00060 
00061 
00062 int getRadioX()
00063 {
00064     Timer timer;
00065     timer.reset();
00066     int dur;
00067     while(!rudder && !gameOver);
00068     timer.start();
00069     while(rudder && !gameOver);
00070     dur = timer.read_us();
00071     return dur;
00072 }
00073 
00074 int getRadioY()
00075 {
00076     Timer timer;
00077     timer.reset();
00078     int dur;
00079     while(!elevator && !gameOver);
00080     timer.start();
00081     while(elevator && !gameOver);
00082     dur = timer.read_us();
00083     return dur;
00084 }
00085 
00086 //Primary function for setting motors for radio driving
00087 void radioDrive()
00088 {   
00089     __disable_irq(); // disable interrupts
00090     int radioY = getRadioY();
00091     int radioX = getRadioX();
00092     __enable_irq(); // enable interrupts
00093 
00094     //Identify drive mode from radio values
00095     if (radioY < CHAN2_MID - DEADZONE)
00096         mode = FORWARD;
00097     if (radioY > CHAN2_MID + DEADZONE)
00098         mode = REVERSE;
00099     if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
00100             radioX < CHAN1_MID - DEADZONE)
00101         mode = CENTERLEFT;
00102     if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
00103             radioX > CHAN1_MID + DEADZONE)
00104         mode = CENTERRIGHT;
00105     if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
00106             radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
00107         mode = STOP;
00108 
00109     //Normalize values for PWM magnitude
00110     xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
00111     ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
00112 
00113     //Set Motors Accordingly
00114     switch(mode) {
00115         case FORWARD:
00116             //Handle variable turns
00117             if (radioX > CHAN1_MID + DEADZONE) {
00118                 rightMotorPWM = 0.2;
00119                 leftMotorPWM = 0.85;
00120             } else if (radioX < CHAN1_MID - DEADZONE) {
00121                 rightMotorPWM = 0.85;
00122                 leftMotorPWM = 0.2;
00123             } else {
00124                 rightMotorPWM = .85;
00125                 leftMotorPWM = .85;
00126             }
00127             leftMotor1 =  0;
00128             leftMotor2 = 1;
00129             rightMotor1 = 0;
00130             rightMotor2 = 1;
00131             break;
00132 
00133         case REVERSE:
00134             //Handle variable turns
00135             if (radioX > CHAN1_MID + DEADZONE) {
00136                 rightMotorPWM = 0.2;
00137                 leftMotorPWM = 0.85;
00138             } else if (radioX < CHAN1_MID - DEADZONE) {
00139                 rightMotorPWM = 0.85;
00140                 leftMotorPWM = 0.2;
00141             } else {
00142                 rightMotorPWM = .85;
00143                 leftMotorPWM = .85;
00144             }
00145             leftMotor1 =  1;
00146             leftMotor2 = 0;
00147             rightMotor1 = 1;
00148             rightMotor2 = 0;
00149             break;
00150 
00151         case CENTERRIGHT:
00152             rightMotorPWM = xpwm;
00153             leftMotorPWM = xpwm;
00154             leftMotor1 = 1;
00155             leftMotor2 = 0;
00156             rightMotor1 = 0;
00157             rightMotor2 = 1;
00158             break;
00159 
00160         case CENTERLEFT:
00161             rightMotorPWM = xpwm;
00162             leftMotorPWM = xpwm;
00163             leftMotor1 = 0;
00164             leftMotor2 = 1;
00165             rightMotor1 = 1;
00166             rightMotor2 = 0;
00167             break;
00168 
00169         case STOP:
00170             rightMotorPWM = 0;
00171             leftMotorPWM = 0;
00172             leftMotor1 = 0;
00173             leftMotor2 = 1;
00174             rightMotor1 = 1;
00175             rightMotor2 = 0;
00176             break;
00177     }
00178 }