AIControl.h

00001 //*******************************************
00002 //* Robot motor control and drive functions *
00003 //*******************************************
00004 
00005 #define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms
00006 
00007 //Radio Calibration Signals - Need to implement calibration routine...
00008 #define CHAN3_MAX 2060  //Throttle Hi
00009 #define CHAN3_MIN 1150 ` //Throttle Lo
00010 #define CHAN2_MAX 1260  //Elevator Hi
00011 #define CHAN2_MIN 2290  //Elevator Lo
00012 #define CHAN1_MAX 2160  //Rudder Hi
00013 #define CHAN1_MIN 1210  //Rudder Lo
00014 
00015 //H-bridge
00016 PwmOut rightMotorPWM(p22);  //Channel A
00017 PwmOut leftMotorPWM(p21);   //Channel B
00018 DigitalOut leftMotor1(p23);
00019 DigitalOut leftMotor2(p24);
00020 DigitalOut rightMotor1(p25);
00021 DigitalOut rightMotor2(p26);
00022 
00023 //Angle state variable
00024 int rtheta;
00025 
00026 
00027 //Non-feedback corrected 'dumb' driving
00028 void setMove(float speed, bool reverse)
00029 {
00030     //Set speed
00031     rightMotorPWM = speed;
00032     leftMotorPWM = speed;
00033 
00034     //Set motor function
00035     leftMotor1 = (!reverse) ? 0 : 1;
00036     leftMotor2 = (!reverse) ? 1 : 0;
00037     rightMotor1 = (!reverse) ? 0 : 1;
00038     rightMotor2 = (!reverse) ? 1 : 0;
00039 }
00040 
00041 void setTurnLeft(float speed)
00042 {
00043     //Set speed
00044     rightMotorPWM = speed;
00045     leftMotorPWM = speed;
00046 
00047     //Set motor function
00048     leftMotor1 = 0;
00049     leftMotor2 = 1;
00050     rightMotor1 = 1;
00051     rightMotor2 = 0;
00052 }
00053 
00054 void setTurnRight(float speed)
00055 {
00056     //Set speed
00057     rightMotorPWM = speed;
00058     leftMotorPWM = speed;
00059 
00060     //Set motor function
00061     leftMotor1 = 1;
00062     leftMotor2 = 0;
00063     rightMotor1 = 0;
00064     rightMotor2 = 1;
00065 }
00066 
00067 //Stop with braking
00068 void stop()
00069 {
00070     rightMotorPWM = 0;
00071     leftMotorPWM = 0;
00072     leftMotor1 = 0;
00073     leftMotor2 = 1;
00074     rightMotor1 = 1;
00075     rightMotor2 = 0;
00076 }
00077 
00078 //Turn left about the center
00079 void centerTurnLeft(bool gameRun)
00080 {
00081     //Reset the filter and re-init the IMU
00082     imuFilter.reset();
00083     rpy_init();
00084     int delta_degrees = 0;
00085 
00086     //So there's this weird thing where the gyro treats left turns as POSITIVE degrees...
00087     float initial = toDegrees(imuFilter.getYaw());
00088     float target = initial + delta_degrees;
00089     double sample;
00090     // pc.printf("Init: %f Target: %f\r\n", initial, target);
00091 
00092     //Continue to turn to target
00093     while (!gameOver) {
00094         setTurnLeft(0.6);
00095         if(gameRun) {
00096             receivePosition(NULL);
00097         }
00098         sample = toDegrees(imuFilter.getYaw());
00099         wait(0.02);
00100         if((sonar3.read()*100) > (SONAR_STOP+6))
00101             break;
00102     }
00103     stop();
00104 }
00105 
00106 //Turn right about the center
00107 void centerTurnRight(bool gameRun)
00108 {
00109     //Reset the filter and re-init the IMU
00110     imuFilter.reset();
00111     rpy_init();
00112     
00113     int delta_degrees = 0;
00114 
00115     //So there's this weird thing where the gyro treats right turns as NEGATIVE degrees...
00116     float initial = toDegrees(imuFilter.getYaw());
00117     float target = initial - delta_degrees;
00118     double sample;
00119 
00120     // pc.printf("Init: %f Target: %f\r\n", initial, target);
00121 
00122     //Continue to turn to target
00123     while (!gameOver) {
00124         setTurnRight(0.6);
00125         if(gameRun) {
00126             receivePosition(NULL);
00127         }
00128         sample = toDegrees(imuFilter.getYaw());
00129         wait(0.02);
00130         if((sonar1.read()*100) > (SONAR_STOP+6))
00131             break;
00132     }
00133     stop();
00134 }
00135 
00136 void checkSonars(bool gameRun)
00137 {
00138     //Collision Detection
00139     // read all sonar sensors
00140     float s1 = sonar1.read() * 100;
00141     float s2 = sonar2.read() * 100;
00142     float s3 = sonar3.read() * 100;
00143 
00144     // check if obstacle is near and adjust
00145     // pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP);
00146     if(s2 < SONAR_STOP) {
00147 
00148         if(s1 <= s3) {
00149             centerTurnRight(gameRun);
00150         } else {
00151             centerTurnLeft(gameRun);
00152         }
00153         //imuFilter.reset();
00154         //rpy_init();
00155     }
00156 }
00157 
00158 //Drive straight with compass correction
00159 void gyroDriveStraight(float speed, bool gameRun, int ms)
00160 {
00161     Timer timer;
00162     double sample;
00163 
00164     //Reset IMU
00165     imuFilter.reset();
00166     rpy_init();
00167 
00168     wait(0.2);
00169 
00170     timer.start();
00171     while (timer.read_ms() < ms) {
00172 
00173         timer.stop();
00174         checkSonars(gameRun);
00175         if(gameRun) {
00176             receivePosition(NULL);
00177         }
00178         timer.start();
00179 
00180         imuFilter.computeEuler();
00181         sample = toDegrees(imuFilter.getYaw());
00182 
00183         //Drift Correction
00184         sample = sample + float(timer.read_ms() / 800);
00185         // pc.printf("%f :::", sample);
00186 
00187         if (sample < 3) {
00188             // pc.printf("Steer Left\r\n");
00189             leftMotorPWM = speed;
00190             rightMotorPWM = speed;
00191             leftMotor1 = 0;
00192             leftMotor2 = 1;
00193             rightMotor1 = 0;
00194             rightMotor2 = 1;
00195         } else if (sample > -3) {
00196             // pc.printf("Steer Right \r\n");
00197             leftMotorPWM = speed;
00198             rightMotorPWM = speed - 0.2;
00199             leftMotor1 = 0;
00200             leftMotor2 = 1;
00201             rightMotor1 = 0;
00202             rightMotor2 = 1;
00203         } else {
00204             // pc.printf("Straight\r\n");
00205             setMove(speed, false);
00206         }
00207     }
00208     stop();
00209 }
00210 
00211 //Check where the evil honeywell-wielding human scum is and perform avoidance
00212 void avoidHoneywell()
00213 {
00214     //Grab telemetry from human car
00215     int human_x = x_hum;
00216     int human_y = y_hum;
00217 
00218     //Calculate avoidance angle
00219     int dx = x_position - human_x;
00220     int dy = y_position - human_y;
00221 
00222     //Set to current angle just in case everything screws up
00223     int desiredAngle = 0;// t_position;
00224 
00225     if (dx >= 0 && dy >= 0)
00226         desiredAngle = (dx > dy) ? 180 : 270;
00227     if (dx >= 0 && dy < 0)
00228         desiredAngle = (dx > dy) ? 180 : 90;
00229     if (dx < 0 && dy >= 0 )
00230         desiredAngle = (dx > dy) ? 0 : 270;
00231     if (dx < 0 && dy < 0)
00232         desiredAngle = (dx > dy) ? 0 : 90;
00233 
00234     int delta_angle = 0;// t_position - desiredAngle;
00235     if (delta_angle >= 0) {
00236         //Right turn to some degree
00237         centerTurnRight(delta_angle);
00238     } else {
00239         centerTurnLeft(delta_angle);
00240     }
00241 }