flywheel control

flywheel angler.cpp

/**
* Distance Control
*/
if(pDigital(DOWN)) {
  robot.shooter->setDistanceToFlag(3.5_ft);
  lastShootMacro = ShootController::shootMacros::off;
} else if(pDigital(LEFT)) {
  robot.shooter->setDistanceToFlag(4.5_ft);
  lastShootMacro = ShootController::shootMacros::off;
} else if(pDigital(UP)) {
  robot.shooter->setDistanceToFlag(8.5_ft);
  lastShootMacro = ShootController::shootMacros::off;
} else if(pDigital(RIGHT)) {
  robot.shooter->setDistanceToFlag(11_ft);
  lastShootMacro = ShootController::shootMacros::off;
}

//set wanted action
if(pDigital(L1) && pDigital(L2)) {
  shootMacro = ShootController::shootMacros::shootBoth;
} else if(pDigital(L1)) {
  shootMacro = ShootController::shootMacros::shootTop;
} else if(pDigital(L2)) {
  shootMacro = ShootController::shootMacros::shootMiddle;
}	else if(pDigital(X)) {
  shootMacro = ShootController::shootMacros::shootOut;
}

//cycle, then wait for button released, then go back to action
if(pDigitalPressed(Y)) {
  robot.shooter->doMacro(ShootController::shootMacros::cycle);
  //cause it to go until pressed unless trigger pressed
  lastShootMacro = ShootController::shootMacros::off;
} else if(pDigital(R1) && pDigital(R2)) {
  robot.shooter->doMacro(ShootController::shootMacros::shoot);
} else if(pDigitalPressed(R1)) {
  if(shootMacro == ShootController::shootMacros::off) {
    robot.shooter->doMacro(ShootController::shootMacros::shoot);
  } else {
    robot.shooter->doMacro(shootMacro);
  }
  //when completed it will go back to other action
  lastShootMacro = ShootController::shootMacros::off;
} else if(!robot.shooter->macroCompleted) { //not completed
  //nothing
} else {
  if(shootMacro != lastShootMacro) {
    switch(shootMacro) {
      case ShootController::shootMacros::shootBoth : {
        robot.shooter->doJob(ShootController::angleTop);
        break;
      } case ShootController::shootMacros::shootTop : {
        robot.shooter->doJob(ShootController::angleTop);
        break;
      } case ShootController::shootMacros::shootMiddle : {
        robot.shooter->doJob(ShootController::angleMiddle);
        break;
      } case ShootController::shootMacros::shootOut : {
        robot.shooter->doJob(ShootController::angleOut);
        break;
      } default: {
        std::cerr << "DriverControl: Invalid shootMacro" << std::endl;
        break;
      }
    }
    lastShootMacro = shootMacro;
  }
}