示例#1
0
int main() {
  //Handle Ctrl-C quit
  signal(SIGINT, sig_handler);

  Shield *shield = new Shield();

  //two motor setup
  Motor left_motor(15, 0);
  Motor right_motor(4, 2);

  Gyro gyro;

  IR medA = IR(2, 6149.816568, 4.468768853);
  IR medB = IR(1, 2391.189039, -0.079559138);

  //was 0.015, 0, 0.4
  //also 0.05, 0, 0.2
  //for medA:
  PIDDrive driveA(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.2);
  PIDDrive driveB(&left_motor, &right_motor, shield, 0.00001, 0.0001, 0.1);

  int straight = 0;
  int turning = 0;
  double curAngle = gyro.get_angle();
  /*double startDist = medA.getDistance();
  double dist[5] = {startDist, startDist, startDist, startDist, startDist};
  double avg = startDist;
  double sum = 0;
*/
  while (running) {
    /*sum = 0;
    for (int i = 1; i <= 4; i++) {
      dist[i-1] = dist[i];
    }
    dist[4] = medA.getDistance();
    for (int i = 0; i < 5; i++) {
       sum +=  dist[i];
    }
    avg = sum / 5.;
    */
    std::cout << "sensor A: " << medA.getDistance() << "\t";
    std::cout << "sensor B: " << medB.getDistance() << std::endl;    
//std::cout << "avg: " << avg << std::endl;

      /*if (medA.getDistance() < 20 && medB.getDistance() > 30) {
      turning = 0;
      if (straight == 0) {
        curAngle = gyro.get_angle();
      }
      straight++;
      driv.drive(curAngle, gyro.get_angle(), 0.25); //keep driving straight
      std::cout << "straight\t" << "angle: " << curAngle <<  std::endl;
    } else if ((medA.getDistance() < 20 && medB.getDistance() < 30) || (medA.getDistance() > 30)) {
      straight = 0;
      if (turning == 0) {
        curAngle = gyro.get_angle();
      }
      turning++;
      //drive.drive(curAngle + 10, gyro.get_angle(), .2); //turn away from wall
      drive.stop();
      straight = 0;
      sleep(.2);
      std::cout << "turning " << "angle: " << curAngle << std::endl;
    } else {
      drive.drive(gyro.get_angle(), gyro.get_angle(), 0.2);
    }*/
    
    if (medB.getDistance() < 15) {
      driveA.stop();
      driveB.stop();
      while (medB.getDistance() < 30) {
        left_motor.setSpeed(shield, 0.2);
        right_motor.setSpeed(shield, -0.2);
        std::cout << "B" << std::endl;
      }
    } else if (medA.getDistance() > 80) {
      left_motor.setSpeed(shield, 0.2);
      right_motor.setSpeed(shield, -0.2);
      usleep(300000);
    } else {
      turning = 0;
      driveA.drive(15, medA.getDistance(), 0.2);
      std::cout << "A" << std::endl;
      usleep(100000);
    }

    /*
    if (medA.getDistance() < 14) {
       left_motor.setSpeed(shield, 0.2);
       right_motor.setSpeed(shield, -0.15);
    } else if (medA.getDistance() > 16) {
       left_motor.setSpeed(shield, -0.15);
       right_motor.setSpeed(shield, 0.2);
    } else {
       left_motor.setSpeed(shield, 0.2);
       right_motor.setSpeed(shield, 0.2);
    }
    */
    /*if (avg < 30) {
      drive.drive(gyro.get_angle(), gyro.get_angle(), -.25);
    } else {
      drive.drive(gyro.get_angle(), gyro.get_angle(), 0.25);
    }*/
    usleep(50000);
    //sleep(1);
  }
}
示例#2
0
int main() {
  //Handle Ctrl-C quit
  signal(SIGINT, sig_handler);

  Shield *shield = new Shield();

  //two motor setup
  Motor left_motor(15, 2);
  Motor right_motor(4, 8);

  Gyro gyro;

  IR medA = IR(1, 6149.816568, 4.468768853);

  //was 0.015, 0, 0.4
  PIDDrive drive(&left_motor, &right_motor, shield, 0.05, 0, 0.2);

  int straight = 0;
  int turning = 0;
  double curAngle = gyro.get_angle();
  /*double startDist = medA.getDistance();
  double dist[5] = {startDist, startDist, startDist, startDist, startDist};
  double avg = startDist;
  double sum = 0;
*/
  while (running) {
    /*sum = 0;
    for (int i = 1; i <= 4; i++) {
      dist[i-1] = dist[i];
    }
    dist[4] = medA.getDistance();
    for (int i = 0; i < 5; i++) {
       sum +=  dist[i];
    }
    avg = sum / 5.;
    */
    std::cout << "sensor: " << medA.getDistance() << std::endl;
    //std::cout << "avg: " << avg << std::endl;

    if (medA.getDistance() < 30) {
      if (turning == 0) {
        curAngle = gyro.get_angle();
      }
      turning++;
      drive.drive(curAngle + 90, gyro.get_angle(), .25); //turn away from wall
      straight = 0;
      sleep(.75);
      std::cout << "turning " << "angle: " << curAngle << std::endl;
    } else {
      turning = 0;
      if (straight == 0) {
        curAngle = gyro.get_angle();
      }
      straight++;
      drive.drive(curAngle, gyro.get_angle(), 0.25); //keep driving straight
      std::cout << "straight\t" << "angle: " << curAngle <<  std::endl;
      //sleep(0.25);
    }

    /*if (avg < 30) {
      drive.drive(gyro.get_angle(), gyro.get_angle(), -.25);
    } else {
      drive.drive(gyro.get_angle(), gyro.get_angle(), 0.25);
    }*/
    usleep(30000);
  }
}