/** This function is called periodically during operator control */
public void teleopPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putNumber("Encoder-Value", drivetrain.encoderGet(0));
// SmartDashboard.putNumber("Timer", System.currentTimeMillis() - time);
elevatorPID.safetyCheck();
claw0PID.safetyCheck();
elevatorPID.showPosition();
SmartDashboard.putNumber("Encoder-Value", drivetrain.encoderGet(0));
SmartDashboard.putNumber("Selected Floor", Robot.elevatorPID.getSelectedFloor());
SmartDashboard.putBoolean("Compensation On?", Robot.elevatorPID.isCompSelected());
// camera stuff
// camera.getImage(frame);
// CameraServer.getInstance().setImage(frame);
}
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.
openLog("tele");
if (autonomousCommand != null) autonomousCommand.cancel();
elevatorPID.brakeOn();
print("starting teleop...");
}
public void disabledInit() {
if (log != null) log.close();
elevatorPID.brakeOn();
setupChooser();
}