/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.stuy;
import edu.stuy.commands.Autonomous;
import edu.stuy.commands.CommandBase;
import edu.stuy.commands.TusksRetract;
import edu.stuy.subsystems.Conveyor;
import edu.stuy.subsystems.Flywheel;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class JoeBot extends IterativeRobot {
Command autonomousCommand;
public static final double BALL_STATIONARY_TIME = 0.5;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
public void robotInit() {
// instantiate the command used for the autonomous period
// autonomousCommand = new ExampleCommand();
// Initialize all subsystems
CommandBase.init();
if (!Devmode.DEV_MODE) {
AxisCamera.getInstance();
}
SmartDashboard.putBoolean("SDB auton drive tuning", false);
SmartDashboard.putDouble("Auton left speed", 0.0);
SmartDashboard.putDouble("Auton right speed", 0.0);
SmartDashboard.putDouble("Auton drive time", 0.0);
}
public void disabledPeriodic() {
updateSmartDashboard();
CommandBase.oi.resetBox();
// CameraVision.getInstance().setCamera(false);
}
public void autonomousInit() {
// schedule the autonomous command (example)
new TusksRetract().start();
autonomousCommand = new Autonomous();
autonomousCommand.start();
}
/**
* This function is called periodically during autonomous
*/
public void autonomousPeriodic() {
Scheduler.getInstance().run();
updateSmartDashboard();
Conveyor conv = CommandBase.conveyor;
// Has the ball settled at the top?
conv.curBallAtTop = CommandBase.conveyor.ballAtTop();
if (conv.curBallAtTop && conv.startBallDelayTime < 0) {
conv.startBallDelayTime = Timer.getFPGATimestamp();
}
conv.ballWaitTime = (conv.startBallDelayTime > 0) ? Timer.getFPGATimestamp() - conv.startBallDelayTime : -1;
conv.ballSettled = conv.startBallDelayTime < 0 || conv.ballWaitTime > BALL_STATIONARY_TIME;
}
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (autonomousCommand != null) {
autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control
*/
public void teleopPeriodic() {
Scheduler.getInstance().run();
CommandBase.oi.updateLights();
updateSmartDashboard();
Conveyor conv = CommandBase.conveyor;
// Has the ball settled at the top?
conv.curBallAtTop = CommandBase.conveyor.ballAtTop();
if (conv.curBallAtTop && conv.startBallDelayTime < 0) conv.startBallDelayTime = Timer.getFPGATimestamp();
conv.ballWaitTime = (conv.startBallDelayTime > 0) ? Timer.getFPGATimestamp() - conv.startBallDelayTime : -1;
conv.ballSettled = conv.startBallDelayTime < 0 || conv.ballWaitTime > BALL_STATIONARY_TIME;
}
// We use SmartDashboard to monitor bot information.
// Here, we put things to the SmartDashboard
private void updateSmartDashboard() {
SmartDashboard.putDouble("Button Pressed: ", CommandBase.oi.getDistanceButton());
SmartDashboard.putDouble("Distance: ", CommandBase.oi.getDistanceFromDistanceButton());
SmartDashboard.putBoolean("Acquirer In: ", CommandBase.oi.getDigitalValue(OI.ACQUIRER_IN_SWITCH_CHANNEL));
SmartDashboard.putBoolean("Acquirer Out: ", CommandBase.oi.getDigitalValue(OI.ACQUIRER_OUT_SWITCH_CHANNEL));
SmartDashboard.putBoolean("Shoot Button: ", CommandBase.oi.getDigitalValue(OI.SHOOTER_BUTTON_CHANNEL));
SmartDashboard.putBoolean("Stinger Switch: ", CommandBase.oi.getDigitalValue(OI.STINGER_SWITCH_CHANNEL));
SmartDashboard.putBoolean("Conveyor In: ", CommandBase.oi.getDigitalValue(OI.CONVEYOR_UP_SWITCH_CHANNEL));
SmartDashboard.putBoolean("Conveyor Out: ", CommandBase.oi.getDigitalValue(OI.CONVEYOR_DOWN_SWITCH_CHANNEL));
SmartDashboard.putDouble("Auton Setting Switch: ", CommandBase.oi.getAutonSetting());
SmartDashboard.putDouble("Speed Trim: ", CommandBase.oi.getSpeedPot());
SmartDashboard.putDouble("Delay Pot: ", CommandBase.oi.getDelayPot());
SmartDashboard.putDouble("Delay Time: ", CommandBase.oi.getDelayTime());
SmartDashboard.putDouble("Max Voltage: ", CommandBase.oi.getMaxVoltage());
SmartDashboard.putBoolean("Upper Conveyor Sensor: ", CommandBase.conveyor.ballAtTop());
SmartDashboard.putBoolean("Lower Conveyor Sensor: ", CommandBase.conveyor.ballAtBottom());
SmartDashboard.putDouble("getRPMtop", Flywheel.upperRoller.getRPM());
SmartDashboard.putDouble("getRPMbottom", Flywheel.lowerRoller.getRPM());
SmartDashboard.putDouble("Acquirer speed", CommandBase.acquirer.getRollerSpeed());
//SmartDashboard.putBoolean("Pressure switch", CommandBase.drivetrain.compressor.getPressureSwitchValue());
//SmartDashboard.putDouble("Battery voltage", DriverStation.getInstance().getBatteryVoltage());
SmartDashboard.putBoolean("Ball settled", CommandBase.conveyor.ballSettled);
SmartDashboard.putBoolean("Speed settled", CommandBase.flywheel.isSpeedSettled());
}
}