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);
}