Java 类edu.wpi.first.wpilibj.command.InstantCommand 实例源码

/**
 * This function is run when the robot is first started up and should be
 * used for any initialization code.
 */
@Override
public void robotInit() {
    // Initialize all subsystems
    drivetrain = new Drivetrain();
    lift = new Lift();
    imu = new IMU();

    oi = new OI();
    System.out.println("Loaded " + name + " v" + version);
    chooser.addDefault("Auto 3", new PlayMacro("AutoRecord3"));
    chooser.addObject("Auto 4", new PlayMacro("AutoRecord4"));
    chooser.addObject("Auto 5", new PlayMacro("AutoRecord5"));
    chooser.addObject("Auto 6", new PlayMacro("AutoRecord6"));
    chooser.addObject("No Auto", new InstantCommand());
    SmartDashboard.putData("Auto mode", chooser);
}

/**
 * Create the instance of Shooter.
 */
public Shooter() {
  logger.info("Initialize");

  if (RobotMap.IS_ROADKILL) {
    return;
  }
  // account for missing Talons
  if (Robot.deviceFinder.isTalonAvailable(RobotMap.CT_ID_SHOOTER_MASTER)) {
    masterId = RobotMap.CT_ID_SHOOTER_MASTER;
    slaveId = RobotMap.CT_ID_SHOOTER_SLAVE;
  } else {
    logger.warn("Master talon is missing");
    masterId = RobotMap.CT_ID_SHOOTER_SLAVE;
    // in case it comes back
    slaveId = RobotMap.CT_ID_SHOOTER_MASTER;
  }

  shooterMaster = new CANTalon(masterId);
  // basic setup
  shooterMaster.changeControlMode(CANTalon.TalonControlMode.Speed);
  shooterMaster.enableBrakeMode(false); // probably bad for 775pros
  shooterMaster.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative);
  shooterMaster.reverseSensor(false);
  // the Talon needs peak and nominal output values
  shooterMaster.configNominalOutputVoltage(+0.0f, -0.0f);
  shooterMaster.configPeakOutputVoltage(+12.0f, -12.0f);
  // configure PID
  shooterMaster.setProfile(0);
  shooterMaster.setF(0);
  shooterMaster.setP(0.12);
  shooterMaster.setI(0.00012);
  shooterMaster.setD(0.5);
  // add to LiveWindow for easy testing
  LiveWindow.addActuator("Shooter", "Master", shooterMaster);

  shooterSlave = new CANTalon(slaveId);
  shooterSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
  shooterSlave.enableBrakeMode(false);
  shooterSlave.set(masterId);
  LiveWindow.addActuator("Shooter", "Slave", shooterSlave);

  Thread shooterWatchdog = new Thread(this::shooterWatchdogThread);
  // allow JVM to exit
  shooterWatchdog.setDaemon(true);
  // in the debugger, we'd like to know what this is
  shooterWatchdog.setName("Shooter Watchdog Thread");
  shooterWatchdog.start();

  SmartDashboard.putData(new InstantCommand("ForceOverrideShooterFault") {
    @Override
    public void execute() {
      shooterFault = false;
    }
  });
}