public void autonomousPeriodic() {
   relay.set(Relay.Value.kOn);
   smart.putNumber("distance", ultrasonic.getVoltage());
   drive.setRun(false);
   loadAndShoot.setRun(false);
   //    relay.set(Relay.Value.kOn);
   if (autoTimer.get() < autoMoveForwardTime) {
     System.out.println(
         "Moving forward, Timer at "
             + autoTimer.get()
             + ", Ultrasonic at "
             + ultrasonic.getAverageVoltage());
   }
   if (autoTimer.get() >= autoMoveForwardTime && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
     stop();
     if (!autonomousStopped) {
       autonomousStopped = true;
       autoTimeAtStop = autoTimer.get();
     }
     System.out.println("Stopped, Ultrasonic at " + ultrasonic.getAverageVoltage());
   }
   if (autoTimer.get() >= autoTimeAtStop + 1 && ultrasonic.getAverageVoltage() <= AUTO_DISTANCE) {
     autoStop = true;
     shoot();
     System.out.println("Shooting, Ultrasonic at " + ultrasonic.getAverageVoltage());
   }
 }
 /** This function is called periodically during operator control */
 public void teleopInit() {
   // relayCompressor.set(Relay.Value.kOn);
   relay.set(Relay.Value.kOff);
   drive.setRun(true);
   loadAndShoot.teleoperatedInit();
   loadAndShoot.setRun(true);
 }
  /**
   * This function is run when the robot is first started up and should be used for any
   * initialization code.
   */
  public void robotInit() {
    autoTimer = new Timer();
    jag1 = new Jaguar(1, 1);
    jag2 = new Jaguar(1, 2);
    jag3 = new Jaguar(1, 3);
    jag4 = new Jaguar(1, 4);
    victor = new Victor(5);

    sol1 = new Solenoid(1);
    sol2 = new Solenoid(2);

    sol4 = new Solenoid(4);
    sol5 = new Solenoid(5);

    sol7 = new Solenoid(7);
    sol8 = new Solenoid(8);

    relay = new Relay(1, 1);

    digi14 = new DigitalInput(1, 14);
    digi13 = new DigitalInput(1, 12);
    digi3 = new DigitalInput(1, 3);

    driverstation = DriverStation.getInstance();
    teamColor = new DigitalOutput(1, 7);
    speedColor = new DigitalOutput(1, 8);
    suckingLED = new DigitalOutput(1, 9);
    readyShoot = new DigitalOutput(1, 6);

    encoder = new AnalogChannel(2);
    ultrasonic = new AnalogChannel(3);

    gyro = new Gyro(1);
    gyro.setSensitivity(0.007);
    gyro.reset();

    xBox = new Joystick(1);

    drive = new Drive(jag1, jag2, jag3, jag4, sol1, sol2, xBox, speedColor);
    loadAndShoot =
        new loadAndShoot(
            encoder,
            victor,
            sol4,
            sol5,
            sol7,
            sol8,
            xBox,
            digi14,
            digi13,
            digi3,
            smart,
            readyShoot);

    drive.start();
    loadAndShoot.start();
    compressor = new Compressor(1, 13, 1, 8);
    compressor.start();
  }
  /** This function is called periodically during autonomous */
  public void autonomousInit() {
    autoTimer.reset();
    autoTimer.start();

    gyro.reset();
    relay.set(Relay.Value.kOn);

    setSpeedFast();
    setLEDTeamColour();

    sol4.set(false);
    sol5.set(true);
    sol7.set(true);
    sol8.set(false);

    autoStop = false;
    autonomousStopped = false;
    autoTimeAtStop = 99999;

    drive.setRun(false);
    loadAndShoot.setRun(false);
    autoForward();
  }
 public void disabledInit() {
   drive.setRun(false);
   loadAndShoot.resetBooleans();
   loadAndShoot.setRun(false);
 }