/** Operator-controlled drive for Teleop mode. */
public void operatorControl() {
boolean retry = true;
while (retry) {
try {
retry = false;
DriverStation.getInstance().setDigitalOut(2, false);
DriverStation.getInstance().setDigitalOut(5, false);
Robot.compressorPump.start();
teleopMode.init();
try {
while (isOperatorControl() && isEnabled() && teleopMode.step()) {
WorkerManager.work();
Dashboard.render();
}
} catch (Exception ex) {
logException(ex);
retry = true;
}
teleopMode.disable();
Robot.compressorPump.stop();
} catch (Exception ex) {
logException(ex);
retry = true;
}
}
}
public String modeText() {
System.out.println("thing: " + m_ds);
return m_ds == null
? "Not connected"
: m_ds.isDisabled()
? "Disabled"
: m_ds.isAutonomous() ? "Autonomous" : m_ds.isTest() ? "Test" : "Teleop";
}
private void sendActions() {
DriverStation.reportError(recording, false);
try {
// BufferedWriter bw = new BufferedWriter(new OutputStreamWriter((new Socket("LSCHS-ROBOTICS",
// 5800).getOutputStream())));
BufferedWriter bw =
new BufferedWriter(new FileWriter(new File("/var/rcrdng/autonRecording3.rcrdng")));
bw.write(recording);
} catch (Exception e) {
DriverStation.reportError(e.getMessage(), false);
}
}
private byte[] output_voltage() {
String voltage = Double.toString(_ds.getBatteryVoltage());
if (voltage.length() != 4) {
voltage = voltage.substring(0, 4);
}
byte[] output = new byte[10];
byte second_digit_two = CHARS[voltage.charAt(1) - 48][1];
second_digit_two |= (byte) 0b01000000;
output[0] = (byte) (0b0000111100001111);
output[2] = CHARS[31][0]; // V
output[3] = CHARS[31][1]; // V
output[4] = CHARS[voltage.charAt(3) - 48][0];
; // third digit
output[5] = CHARS[voltage.charAt(3) - 48][1];
; // third digit
output[6] = CHARS[voltage.charAt(1) - 48][0]; // second digit of voltage
output[7] = second_digit_two; // second digit of voltage, with decimal point.
output[8] = CHARS[voltage.charAt(0) - 48][0]; // first digit of voltage
output[9] = CHARS[voltage.charAt(0) - 48][1]; // first digit of voltage
return output;
}
/**
* Constructor
*
* @param topJaguarChannel The PWM channel for the Jaguar for the top motor
* @param bottomJaguarChannel The PWM channel for the Jaguar for the bottom motor;
* @param topEncoderA The Digital I/O channel for channel A of the encoder for the top motor
* @param topEncoderB The Digital I/O channel for channel B of the encoder for the top motor
* @param bottomEncoderA The Digital I/O channel for channel A of the encoder for the bottom motor
* @param bottomEncoderB The Digital I/O channel for channel B of the encoder for the bottom motor
* @param limitSwitchChannel The Digital I/O channel for the gripper limit switch
*/
public Gripper(
int topJaguarChannel,
int bottomJaguarChannel,
int topEncoderA,
int topEncoderB,
int bottomEncoderA,
int bottomEncoderB,
int limitSwitchChannel) {
topMotor = new Jaguar(topJaguarChannel);
bottomMotor = new Jaguar(bottomJaguarChannel);
topEncoder = new Encoder(topEncoderA, topEncoderB);
topEncoder.start();
topEncoder.reset();
bottomEncoder = new Encoder(bottomEncoderA, bottomEncoderB);
bottomEncoder.start();
bottomEncoder.reset();
topTarget = 0;
bottomTarget = 0;
limitSwitch = new DigitalInput(limitSwitchChannel);
ds = DriverStation.getInstance();
}
/**
* 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();
}
public void setLEDTeamColour() {
// System.out.println(driverstation.getAlliance().name);
if (driverstation.getAlliance().name.startsWith("B")) {
System.out.println("Blue");
teamColor.set(true);
} else {
teamColor.set(false);
}
}
@Override
protected void initialize() {
String defense = Robot.robot.getSelectedDefense();
DriverStation.reportError("Building autotraverse for defense " + defense, false);
// Traverse the selected defense
Scheduler.getInstance().add(new AutoTraverseOnly(defense));
}
public void logData(
DriverStation ds,
Joystick left,
Joystick right,
Joystick co,
DriveTrain dt,
SuperStructure ss) {
System.out.println(
"Data: B:" + ds.getBatteryVoltage() + " L:" + left.getY() + " R:" + right.getY());
}
/** Handles periodic operations. Should be called periodically. */
public void loop() {
ds.setDigitalOut(8, limitSwitch.get());
final int tolerance =
10; // the number of degrees, in either direction, of tolerance for motor movement
if (!isAccepting) {
if (topTarget > topEncoder.get() && Math.abs(topTarget - topEncoder.get()) > tolerance) {
int error = Math.abs(topTarget - topEncoder.get());
topMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Top Motor");
} else if (topTarget < topEncoder.get()
&& Math.abs(topTarget - topEncoder.get()) > tolerance) {
double error = Math.abs(topTarget - topEncoder.get());
topMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Top Motor");
} else {
topMotor.set(0);
}
if (bottomTarget > bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(-limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Backwards", "Gripper Bottom Motor");
} else if (bottomTarget < bottomEncoder.get()
&& Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
double error = Math.abs(bottomTarget - bottomEncoder.get());
bottomMotor.set(limit(error / ERROR_CONVERSION));
// SmartDashboard.log("Forward", "Gripper Bottom Motor");
} else {
bottomMotor.set(0);
// SmartDashboard.log("Stopped", "Gripper Bottom Motor");
}
}
// if the gripper is in accept mode, it needs to stop when the limit switch is pressed
if (isAccepting && !limitSwitch.get()) {
// and also by simply stopping them
topMotor.set(0);
bottomMotor.set(0);
isAccepting = false;
// stop the motors by making the target positions equal to the current positions
topTarget = topEncoder.get();
bottomTarget = bottomEncoder.get();
} else if (isAccepting && limitSwitch.get()) {
// if it is accepting and the limit switch is not pressed
topMotor.set(1);
bottomMotor.set(-1);
}
}
public void run() {
try {
while (true) {
if (isRecording) {
record();
Thread.sleep(timeStep);
}
}
} catch (Exception e) {
DriverStation.reportError(e.getStackTrace() + "", false);
}
}
/** Automated drive for autonomous mode. */
public void autonomous() {
try {
DriverStation.getInstance().setDigitalOut(2, false);
DriverStation.getInstance().setDigitalOut(5, false);
Robot.compressorPump.start();
hybridMode.init();
try {
while (isAutonomous() && isEnabled() && hybridMode.step()) {
WorkerManager.work();
Dashboard.render();
}
} catch (Exception ex) {
ex.printStackTrace();
}
hybridMode.disable();
Robot.compressorPump.stop();
} catch (Exception ex) {
logException(ex);
}
}
public void updateSmartDashboard() {
if (joystick_left.getRawButton(2) && dev_ < 100) {
dev_++;
} else if (joystick_left.getRawButton(3) && dev_ > 0) {
dev_--;
}
SmartDashboard.putDouble("X", joystick_left.getX());
SmartDashboard.putDouble("Y", joystick_left.getY());
// SmartDashboard.putDouble("Optical Sensor", Optical_Sensor.getVoltage());
SmartDashboard.putDouble("Battery Voltage", DriverStation.getInstance().getBatteryVoltage());
SmartDashboard.putInt("dev_", dev_);
SmartDashboard.putDouble(
"Elevator Sensor Value (.getAverageVoltage())",
this.elevator.ana_chana.getAverageVoltage());
String state = "ROBOT STATE INFORMATION:";
if (this.elevator.state == 0) {
state = state + "\nElevator Stopped.";
}
if (this.elevator.state == 1) {
state = state + "\nElevator Moving.";
}
if (this.elevator.state == 2) {
state = state + "\nElevator E-Stopped.";
}
// state = state+"\nShooter Power Level (Raw):
// "+this.shooter.calculateShooterSpeed();
// state = state+"\nShooter Power Level (Setting): "+this.shooter.shooterSpeedState;
// if (this.integ.globalState==1){
// state = state+"\nMoving Shooter Up To Shoot.";
// }if (this.integ.globalState==2){
// state = state+"\nShooting!";
// }if (this.integ.globalState2LOCK){
// state = state+"\nREALLY SHOOTING!.";
// }
state = state + "\nNumber Of Discs: " + this.elevator.numDiscs;
state =
state
+ "\n Louis + Bobni Are Amazing (Hiding This Message Is ILLEGAL! IT WILL STOP THE ROBOT!)";
SmartDashboard.putString("State", state);
SmartDashboard.putBoolean("Intake", this.intake.toggle.get());
}
private static void openLog(String type) {
long time = System.currentTimeMillis();
if (log != null) log.close();
try {
log =
new PrintWriter(
new FileWriter(
"/" // + time + "_"
+ type
+ "_log.txt"));
log.println(("" + time) + " New log created.");
} catch (IOException e) {
DriverStation.reportError("creating log failed", true);
}
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
oi = new OI();
chooser = new SendableChooser();
chooser.addDefault("Default Auto", new ExampleCommand());
// chooser.addObject("My Auto", new MyAutoCommand());
SmartDashboard.putData("Auto mode", chooser);
try {
/* Communicate w/navX-MXP via the MXP SPI Bus. */
/* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */
/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
ahrs = new AHRS(SPI.Port.kMXP);
} catch (RuntimeException ex) {
DriverStation.reportError("Error instantiating navX-MXP: " + ex.getMessage(), true);
}
}
/**
* Gets value from a button
*
* @param button number of the button
* @return State of the button
*/
public boolean getRawButton(int button) {
if (button == ButtonType.kRTrigger.value) { // Abstracted Buttons from Analog Axis
if (getThrottle() <= -.6) {
return true;
} else {
return false;
}
}
if (button == ButtonType.kLTrigger.value) { // Abstracted Buttons from Analog Axis
if (getThrottle() >= .6) {
return true;
} else {
return false;
}
}
return ((0x1 << (button - 1)) & m_ds.getStickButtons(m_port)) != 0;
}
RobotIO() {
// -------Motors-------
frontLeftMotor = new Talon(RobotMap.frontLeftMotor);
rearLeftMotor = new Talon(RobotMap.rearLeftMotor);
frontRightMotor = new Talon(RobotMap.frontRightMotor);
rearRightMotor = new Talon(RobotMap.rearRightMotor);
shooterArmMotor = new Talon(RobotMap.shooterArmMotor);
shooterLeftWheelMotor = new Talon(RobotMap.shooterLeftWheelMotor);
shooterRightWheelMotor = new Talon(RobotMap.shooterRightWheelMotor);
portcullisArmMotors = new Relay(RobotMap.portcullisArmMotors);
climberLeftMotor = new Victor(RobotMap.climberLeftMotor);
climberRightMotor = new Victor(RobotMap.climberRightMotor);
// ------Solenoids-----
shooterPistonExtend = new Solenoid(RobotMap.shooterPistonExtend);
shooterPistonRetract = new Solenoid(RobotMap.shooterPistonRetract);
portcullisLifterPistonsExtend = new Solenoid(RobotMap.portcullisLifterPistonsExtend);
portcullisLifterPistonsRetract = new Solenoid(RobotMap.portcullisLifterPistonsRetract);
// -------Sensors------
topGyro = new AnalogGyro(RobotMap.topGyro);
bottomGyro = new AnalogGyro(RobotMap.bottomGyro);
leftBallIR = new AnalogInput(RobotMap.leftBallIR);
rightBallIR = new AnalogInput(RobotMap.rightBallIR);
driveEncoderLeft = new Encoder(RobotMap.driveEncoderLeftA, RobotMap.driveEncoderLeftB);
driveEncoderRight = new Encoder(RobotMap.driveEncoderRightA, RobotMap.driveEncoderRightB);
shooterArmEncoder = new Encoder(RobotMap.shooterArmEncoderA, RobotMap.shooterArmEncoderB);
// -----Joysticks-----
driverStick = new Joystick(RobotMap.driverStick);
operatorStick = new Joystick(RobotMap.operatorStick);
// -------Misc--------
driveRobot = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
pdp = new PowerDistributionPanel();
ds = DriverStation.getInstance();
}
public double doRobotdriveScaling(double value) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel)) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel2)) {
driverstation.setDigitalOut(2, true);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(3, false);
if (driverstation.getAnalogIn(Constants.driverstation_scale_channel) < 1) {
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel);
}
driverstation.setDigitalOut(3, true);
return 0;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, true);
driverstation.setDigitalOut(2, false);
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel) * 0.2;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(2, false);
return value * Constants.robotdrive_scale_hi;
}
public class Team3699Robot extends SimpleRobot {
public Joystick joystick_left = new Joystick(Constants.joystick_left_USB);
public Joystick joystick_right = new Joystick(Constants.joystick_right_USB);
public Joystick joystick_elevator = new Joystick(3);
public DriverStationLCD userMessages = DriverStationLCD.getInstance();
public DriverStation driverstation = DriverStation.getInstance();
public NetworkTable server;
/*<<<<<<< HEAD
// public ShooterControl shooter = new ShooterControl();
public Jaguar shooterMotor = new Jaguar(Constants.shooterPWMChannel);
=======*/
public ShooterControl shooter = new ShooterControl();
public Jaguar shooterMotor = new Jaguar(Constants.shooterPWMChannel);
// >>>>>>> pULL dOWN sHOOTER dURING aUTONOMUS, wITH jUST a tIMED sCRIPT
public ArmControl3 arm = new ArmControl3(this);
public Jaguar armMotor = new Jaguar(Constants.armMotorPWM);
public IntegrationControl2 integ = new IntegrationControl2(this);
// public AnalogChannel Optical_Sensor = new AnalogChannel(Constants.OpticalSensorChannel);
// public Jaguar test_CIM_1 = new Jaguar(7);
// public Jaguar test_CIM_2 = new Jaguar(8);
/*
public DigitalInput Disc_Check = new DigitalInput(Constants.Disc_Check_Channel);
public DigitalInput Disc_Top_Check = new DigitalInput(Constants.Disc_Top_Check_Channel);
public DigitalInput Upside_Down_Check = new DigitalInput(Constants.Upside_Down_Check_Channel);
*/
public RobotDrive robotdrive =
new RobotDrive(Constants.robotdrive_left_PWM, Constants.robotdrive_right_PWM);
public Jaguar Intake_motor = new Jaguar(Constants.intake_PWM);
public Jaguar Elevator_motor = new Jaguar(Constants.elevator_PWM);
public Jaguar Elevator_intake_motor = new Jaguar(Constants.elevator_intake_PWM);
public Jaguar pullDownJaguar = new Jaguar(Constants.pulldownPWM);
// public Jaguar Elevator_intake = new Jaguar(Constants.elevator_intake_PWM);
// public Jaguar Elevator_outtake = new Jaguar(Constants.elevator_outtake_PWM);
public ElevatorControl elevator = new ElevatorControl(this);
public IntakeControl intake = new IntakeControl(this);
// public Auto_Integration2 integ2 = new Auto_Integration2(this);
public Shoot3Auto auto_shoot = new Shoot3Auto(this);
// SendableChooser autoChooser;
int dev_ = 0;
public boolean[] errors = new boolean[10];
public AutoPullDown pullDown = new AutoPullDown();
// ^ Yeah, yeah. Way overinitizized
// ERRNO 0: Init error
// ERRNO 1: SmartDashboard error
// ERRNO 2: Teleop error
public boolean allowUnlimitedErrors = true;
// Set to true to allow unlimeted errors
public void error(String message, int errno) {
if (!(this.errors[errno]) || this.allowUnlimitedErrors) {
log(message);
errors[errno] = true;
}
}
public void resetError(int errno) {
errors[errno] = false;
}
public void log(String message) {
System.out.println(message);
}
public void robotInit() {
log("RobotInit called. Initializing NetworkTabes...");
try {
NetworkTable.setIPAddress("10.36.99.2");
NetworkTable.setServerMode();
NetworkTable.setTeam(3699);
NetworkTable.initialize();
} catch (Exception be) {
error("Error in robotInit: \n " + be.getMessage(), 0);
error("Reocourrence Test! You shouldnt see this!", 0);
}
log("Init SmartDashboard...");
this.server = NetworkTable.getTable("SmartDashboard");
log("" + this.server.containsKey("/SmartDashboard/"));
log("" + this.server.containsKey("/SmartDashboard/FPS"));
log("" + NetworkTable.DEFAULT_PORT);
this.server.putString("_PREINIT0", "_PREINIT::0");
this.server.putString("_PREINIT1", "_PREINIT::1");
this.server.putString("_PREINIT2", "_PREINIT::2");
/*LiveWindow.addActuator("Elevator", "Elevator Intake", Elevator_intake);
LiveWindow.addActuator("Elevator", "Elevator Gearbox", Elevator_motor);
LiveWindow.addActuator("Elevator", "Elevator Outtake", Elevator_outtake);
LiveWindow.addActuator("Intake", "Intake", Elevator_motor);*/
log("Disabling RobotDrive Saftey... :D");
robotdrive.setSafetyEnabled(false);
// autoChooser = new SendableChooser();
// autoChooser.addDefault("Default 3 Discs", this.auto_shoot);
// SmartDashboard.putData("Autonomous mode chooser",autoChooser);
}
public void operatorControl() {
log("Teleop Called Once!");
resetError(1);
resetError(2);
this.elevator.resetState();
// try{
showUserMessages("TeleOp");
this.arm.state = 0;
while (isOperatorControl() && isEnabled()) {
// robotdrive.setSafetyEnabled(true); //In case the program were to stop, this stops the
// motors from continuing to run.
if (!Util.checkButton(this, Constants.robotdrive_break_button)) {
// robotdrive.tankDrive(-doRobotdriveScaling(joystick_left.getY()),
// -doRobotdriveScaling(joystick_right.getY()));
robotdrive.arcadeDrive(
doRobotdriveScaling(joystick_left.getY()), doRobotdriveScaling(joystick_left.getX()));
}
// x, y int := 0, 1;
// if (driverstation.getDigitalIn(3)){
// this.test_CIM_1.set(driverstation.getAnalogIn(2));
// }else{
// this.test_CIM_1.set(0-driverstation.getAnalogIn(2));
// }
//
// if (driverstation.getDigitalIn(4)){
// this.test_CIM_2.set(driverstation.getAnalogIn(3));
// }else{
// this.test_CIM_2.set(0-driverstation.getAnalogIn(3));
// }
updateSmartDashboard();
this.shooter.updateStates(this);
this.shooterMotor.set(this.shooter.calculateShooterSpeed());
if (this.driverstation.getDigitalIn(4)) {
this.armMotor.set(this.driverstation.getAnalogIn(3));
} else {
this.armMotor.set(-this.driverstation.getAnalogIn(3));
}
this.arm.update();
if (this.arm.getArmSpeed() != 0D) {
this.armMotor.set(this.arm.getArmSpeed());
}
if (this.integ.doElevatorUpdate()) {
this.elevator.update();
this.Elevator_motor.set(this.elevator.getElevatorSpeed());
}
this.intake.update();
this.Intake_motor.set(this.intake.calculateIntakeSpeed());
if (this.intake.state == 1) {
this.Elevator_intake_motor.set(0.6D);
} else {
this.Elevator_intake_motor.set(0D);
}
if (this.driverstation.getDigitalIn(3)) {
this.pullDownJaguar.set(this.driverstation.getAnalogIn(2));
} else {
this.pullDownJaguar.set(-this.driverstation.getAnalogIn(2));
}
this.integ.update();
SmartDashboardUpdater.updateSmartDashboard(this);
if (Util.checkButton(this, 9)) {
this.elevator.numDiscs = 0;
}
if (Util.checkButton(this, 8)) {
this.elevator.state = 0;
this.integ.state = 0;
this.intake.state = 0;
this.integ.elevator_out_roller.set(0d);
this.arm.state = 0;
}
{
try {
SmartDashboard.putString("DEBUG*isConnected", "" + this.server.isConnected());
SmartDashboard.putString("DEBUG*isServer", "" + this.server.isServer());
SmartDashboard.putDouble("FPS~!", this.server.getNumber("FPS", -1d));
SmartDashboard.putDouble("NewDistance~!", this.server.getNumber("NewDistance", -1d));
// SmartDashboard.putString("NetworkTable Keys", this.server.);
log("W0RK1NG!");
} catch (Exception be) {
error("ERROR UPDATING SmartDashboard dtt STACK TRACE: " + be.getMessage(), 1);
}
}
Timer.delay(0.005); // and make sure we dont overload the cRIO
}
// }catch (Exception be){
// error("ERROR IN TELEOP! STACKTRACE: \n "+be.getMessage(), 2);
// }
}
public void disabled() {
log("Disabled.");
showUserMessages("Disabled");
}
public void autonomous() {
log("Autonomus! (XD I am bad at spelling)");
showUserMessages("Autonomous"); /*
<<<<<<< HEAD
//autoChooser.getSelected();
auto_shoot.update();
// robotdrive.setSafetyEnabled(false);
// robotdrive.tankDrive(doRobotdriveScaling(-0.45),doRobotdriveScaling(-0.45));
// Timer.delay(2.0);
// robotdrive.tankDrive(doRobotdriveScaling(0.0),doRobotdriveScaling(0.0)); // Use to determine ft/s at this power.
=======
this.pullDown.reset();
while (this.isAutonomous()&&isEnabled()){
this.pullDown.pullDown(this);
}
//robotdrive.tankDrive(doRobotdriveScaling(-0.45),doRobotdriveScaling(-0.45));
//Timer.delay(2.0);
//robotdrive.tankDrive(doRobotdriveScaling(0.0),doRobotdriveScaling(0.0)); // Use to determine ft/s at this power.
>>>>>>> pULL dOWN sHOOTER dURING aUTONOMUS, wITH jUST a tIMED sCRIPT
*/
}
public double doRobotdriveScaling(double value) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel)) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel2)) {
driverstation.setDigitalOut(2, true);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(3, false);
if (driverstation.getAnalogIn(Constants.driverstation_scale_channel) < 1) {
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel);
}
driverstation.setDigitalOut(3, true);
return 0;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, true);
driverstation.setDigitalOut(2, false);
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel) * 0.2;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(2, false);
return value * Constants.robotdrive_scale_hi;
}
public void showUserMessages(String status) {
this.userMessages.println(DriverStationLCD.Line.kUser2, 1, "FRC Robotics Team ");
this.userMessages.println(
DriverStationLCD.Line.kUser3, 1, "3699 Robot " + status + " ");
this.userMessages.println(DriverStationLCD.Line.kUser4, 1, Constants.version);
this.userMessages.println(DriverStationLCD.Line.kUser5, 1, "===VENDETTA LIST:===");
this.userMessages.println(DriverStationLCD.Line.kUser6, 1, "(Names Forthcoming) ");
this.userMessages.updateLCD();
}
public void updateSmartDashboard() {
if (joystick_left.getRawButton(2) && dev_ < 100) {
dev_++;
} else if (joystick_left.getRawButton(3) && dev_ > 0) {
dev_--;
}
SmartDashboard.putDouble("X", joystick_left.getX());
SmartDashboard.putDouble("Y", joystick_left.getY());
// SmartDashboard.putDouble("Optical Sensor", Optical_Sensor.getVoltage());
SmartDashboard.putDouble("Battery Voltage", DriverStation.getInstance().getBatteryVoltage());
SmartDashboard.putInt("dev_", dev_);
SmartDashboard.putDouble(
"Elevator Sensor Value (.getAverageVoltage())",
this.elevator.ana_chana.getAverageVoltage());
String state = "ROBOT STATE INFORMATION:";
if (this.elevator.state == 0) {
state = state + "\nElevator Stopped.";
}
if (this.elevator.state == 1) {
state = state + "\nElevator Moving.";
}
if (this.elevator.state == 2) {
state = state + "\nElevator E-Stopped.";
}
// state = state+"\nShooter Power Level (Raw):
// "+this.shooter.calculateShooterSpeed();
// state = state+"\nShooter Power Level (Setting): "+this.shooter.shooterSpeedState;
// if (this.integ.globalState==1){
// state = state+"\nMoving Shooter Up To Shoot.";
// }if (this.integ.globalState==2){
// state = state+"\nShooting!";
// }if (this.integ.globalState2LOCK){
// state = state+"\nREALLY SHOOTING!.";
// }
state = state + "\nNumber Of Discs: " + this.elevator.numDiscs;
state =
state
+ "\n Louis + Bobni Are Amazing (Hiding This Message Is ILLEGAL! IT WILL STOP THE ROBOT!)";
SmartDashboard.putString("State", state);
SmartDashboard.putBoolean("Intake", this.intake.toggle.get());
}
}
public class Digit_Board {
private static class Board_Task implements Runnable {
private Digit_Board _b;
Board_Task(Digit_Board b) {
_b = b;
}
public void run() {
_b.board_task();
}
}
private static Digit_Board instance = new Digit_Board();
private Thread _task_thread;
private boolean _do_things = false;
public static Digit_Board getInstance() {
return Digit_Board.instance;
}
protected Digit_Board() {
_task_thread = new Thread(new Board_Task(this), "1504_Display_Board");
_task_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
DisplayInit();
start();
System.out.println("MXP Leader, standing by.");
}
public void start() {
if (_do_things) return;
_do_things = true;
_task_thread = new Thread(new Board_Task(this));
_task_thread.start();
}
public void stop() {
_do_things = false;
}
private DriverStation _ds = DriverStation.getInstance();
private I2C _display_board;
private DigitalInput _a;
private DigitalInput _b;
// taken from mike
private static final int A_MASK = 0b0000000000000001;
private static final int B_MASK = 0b0000000000000010;
private volatile int _input_mask, _input_mask_rising, _input_mask_rising_last;
private static long _timeout = 2500;
private AnalogInput _pot;
private static enum STATE {
Voltage,
Position,
Obstacle,
Wait
}
private static String[] _positions = {"P 1", "P 2", "P 3", "P 4", "P 5"};
private static String[] _obstacles = {
"LBAR", "PORT", "DRAW", "MOAT", "FRIS", "RAMP", "SALP", "ROCK", "TERR"
};
int pos = 0;
int obs = 0;
private void DisplayInit() {
_display_board = new I2C(I2C.Port.kMXP, 0x70);
_a = new DigitalInput(19);
_b = new DigitalInput(20);
_pot = new AnalogInput(7);
}
public void update() {
int current_mask = get_input_mask();
_input_mask |= current_mask;
_input_mask_rising |= (~_input_mask_rising_last & current_mask);
_input_mask_rising_last = current_mask;
}
private int get_input_mask() {
int mask = 0;
mask |= (getA() ? 1 : 0) << A_MASK;
mask |= (getB() ? 1 : 0) << B_MASK;
return mask;
}
private boolean getRawButtonOnRisingEdge(int button_mask) {
button_mask = button_mask << 1;
// Compute a clearing mask for the button.
int clear_mask = 0b1111111111111111 - button_mask;
// Get the value of the button - 1 or 0
boolean value = (_input_mask_rising & button_mask) != 0;
// Mask this and only this button back to 0
_input_mask_rising &= clear_mask;
return value;
}
private boolean getRawButtonLatch(int button_mask) {
// Compute a clearing mask for the button.
int clear_mask = 0b1111111111111111 - button_mask;
// Get the value of the button - 1 or 0
boolean value = (_input_mask & button_mask) != 0;
// Mask this and only this button back to 0
_input_mask &= clear_mask;
return value;
}
public boolean getA() {
return (!_a.get());
}
public boolean getALatch() {
return getRawButtonLatch(A_MASK);
}
public boolean getAOnRisingEdge() {
return getRawButtonOnRisingEdge(A_MASK);
}
public boolean getB() {
return (!_b.get());
}
public boolean getBLatch() {
return getRawButtonLatch(B_MASK);
}
public boolean getBOnRisingEdge() {
return getRawButtonOnRisingEdge(B_MASK);
}
public double getPot() {
double val = (double) _pot.getAverageValue(); // integer between 3 - 400
double delay = Math.min((val / 400), 10.0); // number between 0 and 10
return delay;
}
private byte[] output_voltage() {
String voltage = Double.toString(_ds.getBatteryVoltage());
if (voltage.length() != 4) {
voltage = voltage.substring(0, 4);
}
byte[] output = new byte[10];
byte second_digit_two = CHARS[voltage.charAt(1) - 48][1];
second_digit_two |= (byte) 0b01000000;
output[0] = (byte) (0b0000111100001111);
output[2] = CHARS[31][0]; // V
output[3] = CHARS[31][1]; // V
output[4] = CHARS[voltage.charAt(3) - 48][0];
; // third digit
output[5] = CHARS[voltage.charAt(3) - 48][1];
; // third digit
output[6] = CHARS[voltage.charAt(1) - 48][0]; // second digit of voltage
output[7] = second_digit_two; // second digit of voltage, with decimal point.
output[8] = CHARS[voltage.charAt(0) - 48][0]; // first digit of voltage
output[9] = CHARS[voltage.charAt(0) - 48][1]; // first digit of voltage
return output;
}
private byte[] output_pos(String input) {
byte[] output = new byte[10];
output[0] = (byte) (0b0000111100001111);
output[2] = CHARS[input.charAt(3) - 48][0];
output[3] = CHARS[input.charAt(3) - 48][1];
output[4] = output[5] = output[6] = output[7] = (byte) 0b00000000;
output[8] = CHARS[input.charAt(0) - 55][0];
output[9] = CHARS[input.charAt(0) - 55][1];
return output;
}
private byte[] output_obs(String input) {
byte[] output = new byte[10];
output[0] = (byte) (0b0000111100001111);
for (int i = 0; i < input.length(); i++) {
output[(2 * i) + 2] = CHARS[input.charAt(3 - i) - 55][0];
output[(2 * i) + 3] = CHARS[input.charAt(3 - i) - 55][1];
}
// output[2] = CHARS[input.charAt(3) - 55][0];
// output[3] = CHARS[input.charAt(3) - 55][1];
// output[4] = CHARS[input.charAt(2) - 55][0];
// output[5] = CHARS[input.charAt(2) - 55][1];
// output[6] = CHARS[input.charAt(1) - 55][0];
// output[7] = CHARS[input.charAt(1) - 55][1];
// output[8] = CHARS[input.charAt(0) - 55][0];
// output[9] = CHARS[input.charAt(0) - 55][1];
return output;
}
private void board_task() {
byte[] osc = new byte[1];
byte[] blink = new byte[1];
byte[] bright = new byte[1];
osc[0] = (byte) 0x21;
blink[0] = (byte) 0x81;
bright[0] = (byte) 0xEF;
_display_board.writeBulk(osc);
_display_board.writeBulk(bright);
_display_board.writeBulk(blink);
STATE mode = STATE.Voltage;
long refresh = 0;
while (_do_things) {
update();
if ((System.currentTimeMillis() - refresh) > _timeout) {
mode = STATE.Voltage;
}
boolean update_refresh = true;
if (getAOnRisingEdge()) {
if (mode == STATE.Position) {
pos = (pos + 1) % _positions.length; // just display current position on first press
}
mode = STATE.Position;
// System.out.println("pos");
} else if (getBOnRisingEdge()) {
if (mode == STATE.Obstacle) {
obs = (obs + 1) % _obstacles.length;
}
mode = STATE.Obstacle; // display current obstacle on first press
// System.out.println("obs");
} else {
update_refresh = false;
}
if (pos == 0) {
obs = 0;
}
if (pos > 0 && obs == 0) {
obs++;
}
if (update_refresh) {
refresh = System.currentTimeMillis();
}
if (mode == STATE.Position) {
_display_board.writeBulk(output_pos(_positions[pos]));
} else if (mode == STATE.Obstacle) {
_display_board.writeBulk(output_obs(_obstacles[obs]));
} else {
_display_board.writeBulk(output_voltage());
}
try {
Thread.sleep(40); // wait a while because people can't read that
// fast
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
// Thanks @Team 1493
private static final byte[][] CHARS = {
{(byte) 0b00111111, (byte) 0b00000000}, // 0; 0
{(byte) 0b00000110, (byte) 0b00000000}, // 1; 1
{(byte) 0b11011011, (byte) 0b00000000}, // 2; 2
{(byte) 0b11001111, (byte) 0b00000000}, // 3; 3
{(byte) 0b11100110, (byte) 0b00000000}, // 4; 4
{(byte) 0b11101101, (byte) 0b00000000}, // 5; 5
{(byte) 0b11111101, (byte) 0b00000000}, // 6; 6
{(byte) 0b00000111, (byte) 0b00000000}, // 7; 7
{(byte) 0b11111111, (byte) 0b00000000}, // 8; 8
{(byte) 0b11101111, (byte) 0b00000000}, // 9; 9
{(byte) 0b11110111, (byte) 0b00000000}, // A; 10
{(byte) 0b10001111, (byte) 0b00010010}, // B; 11
{(byte) 0b00111001, (byte) 0b00000000}, // C; 12
{(byte) 0b00001111, (byte) 0b00010010}, // D; 13
{(byte) 0b11111001, (byte) 0b00000000}, // E; 14
{(byte) 0b11110001, (byte) 0b00000000}, // F; 15
{(byte) 0b10111101, (byte) 0b00000000}, // G; 16
{(byte) 0b11110110, (byte) 0b00000000}, // H; 17
{(byte) 0b00001001, (byte) 0b00010010}, // I; 18
{(byte) 0b00011110, (byte) 0b00000000}, // J; 19
{(byte) 0b01110000, (byte) 0b00001100}, // K; 20
{(byte) 0b00111000, (byte) 0b00000000}, // L; 21
{(byte) 0b00110110, (byte) 0b00000101}, // M; 22
{(byte) 0b00110110, (byte) 0b00001001}, // N; 23
{(byte) 0b00111111, (byte) 0b00000000}, // O; 24
{(byte) 0b11110011, (byte) 0b00000000}, // P; 25
{(byte) 0b00111111, (byte) 0b00001000}, // Q; 26
{(byte) 0b11110011, (byte) 0b00001000}, // R; 27
{(byte) 0b10001101, (byte) 0b00000001}, // S; 28
{(byte) 0b00000001, (byte) 0b00010010}, // T; 29
{(byte) 0b00111110, (byte) 0b00000000}, // U; 30
{(byte) 0b00110000, (byte) 0b00100100}, // V; 31
{(byte) 0b00110110, (byte) 0b00101000}, // W; 32
{(byte) 0b00000000, (byte) 0b00101101}, // X; 33
{(byte) 0b00000000, (byte) 0b00010101}, // Y; 34
{(byte) 0b00001001, (byte) 0b00100100}, // Z; 35
};
}
public void run() {
m_ds.task();
}
public OI() {
ds = DriverStation.getInstance().getEnhancedIO();
shooterFeedbackEnableButton.whileHeld(new ShootingWithFeedback());
}
public OI() {
enhancedIO = DriverStation.getInstance().getEnhancedIO();
leftStick = new Joystick(RobotMap.LEFT_JOYSTICK_PORT);
rightStick = new Joystick(RobotMap.RIGHT_JOYSTICK_PORT);
shooterStick = new Joystick(RobotMap.SHOOTER_JOYSTICK_PORT);
debugBox = new Joystick(RobotMap.DEBUG_BOX_PORT);
distanceButton = DISTANCE_BUTTON_STOP;
distanceInches = Flywheel.distances[Flywheel.STOP_INDEX];
try {
if (!Devmode.DEV_MODE) {
enhancedIO.setDigitalConfig(
BIT_1_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
BIT_2_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
BIT_3_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
ACQUIRER_IN_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
ACQUIRER_OUT_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
CONVEYOR_UP_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
CONVEYOR_DOWN_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
SHOOTER_BUTTON_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
STINGER_SWITCH_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kInputPullUp);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_KEY_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FAR_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FENDER_2PT_LIGHT_CHANNEL,
DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FENDER_NARROW_LIGHT_CHANNEL,
DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_REVERSE_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_FENDER_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
enhancedIO.setDigitalConfig(
DISTANCE_BUTTON_STOP_LIGHT_CHANNEL, DriverStationEnhancedIO.tDigitalConfig.kOutput);
}
} catch (EnhancedIOException e) {
}
if (!Devmode.DEV_MODE) {
new JoystickButton(leftStick, 1).whenPressed(new DrivetrainSetGear(false));
new JoystickButton(leftStick, 2).whenPressed(new DrivetrainSetGear(true));
new JoystickButton(rightStick, 1).whenPressed(new TusksExtend());
new JoystickButton(rightStick, 2).whenPressed(new TusksRetract());
// OI box switches
new InverseDigitalIOButton(ACQUIRER_IN_SWITCH_CHANNEL).whileHeld(new AcquirerAcquire());
new InverseDigitalIOButton(ACQUIRER_OUT_SWITCH_CHANNEL).whileHeld(new AcquirerReverse());
new InverseDigitalIOButton(CONVEYOR_UP_SWITCH_CHANNEL).whileHeld(new ConveyManual());
new InverseDigitalIOButton(CONVEYOR_DOWN_SWITCH_CHANNEL).whileHeld(new ConveyReverseManual());
new InverseDigitalIOButton(SHOOTER_BUTTON_CHANNEL).whileHeld(new ConveyAutomatic());
new InverseDigitalIOButton(STINGER_SWITCH_CHANNEL).whileHeld(new StingerExtend());
new JoystickButton(shooterStick, 1).whileHeld(new ConveyManual());
new JoystickButton(shooterStick, 4).whenPressed(new FlywheelStop());
new JoystickButton(shooterStick, 5).whileHeld(new AcquirerReverse());
new JoystickButton(shooterStick, 6).whileHeld(new ConveyReverseManual());
new JoystickButton(shooterStick, 7).whileHeld(new AcquirerAcquire());
new JoystickButton(shooterStick, 8).whileHeld(new ConveyAutomatic());
// see getDistanceButton()
// Debug box switches
new JoystickButton(debugBox, 1)
.whileHeld(
new FlywheelRun(Flywheel.distances[Flywheel.FENDER_INDEX], Flywheel.speedsTopHoop));
new JoystickButton(debugBox, 2).whileHeld(new AcquirerAcquire());
new JoystickButton(debugBox, 3).whileHeld(new ConveyAutomatic());
new JoystickButton(debugBox, 4).whileHeld(new ConveyManual());
// Debug box buttons
new JoystickButton(debugBox, 5).whileHeld(new DrivetrainSetGear(false)); // low gear
new JoystickButton(debugBox, 6).whileHeld(new DrivetrainSetGear(true)); // high gear
new JoystickButton(debugBox, 9).whileHeld(new TusksExtend());
new JoystickButton(debugBox, 10).whileHeld(new TusksRetract());
}
}
/**
* Constructor
*
* @param port USB Port on DriverStation
*/
public XboxController(int port) {
super();
m_port = port;
m_ds = DriverStation.getInstance();
}
/**
* Get Value from an Axis
*
* @param axis Axis Number
* @return Value from Axis (-1 to 1)
*/
public double getRawAxis(int axis) {
return m_ds.getStickAxis(m_port, axis);
}
@Override
public void execute() {
System.out.println("Starting execution");
try {
if (distance > 0) {
while (DriverStation.getInstance().isEnabled()
&& TKOHardware.getDriveTalon(0).getSetpoint() < distance) {
TKOHardware.getDriveTalon(0)
.set(TKOHardware.getDriveTalon(0).getSetpoint() + incrementer);
TKOHardware.getDriveTalon(2)
.set(TKOHardware.getDriveTalon(2).getSetpoint() + incrementer);
System.out.println(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
TKOLogger.getInstance()
.addMessage(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
Timer.delay(0.001);
}
} else {
while (DriverStation.getInstance().isEnabled()
&& TKOHardware.getDriveTalon(0).getSetpoint() > distance) {
TKOHardware.getDriveTalon(0)
.set(TKOHardware.getDriveTalon(0).getSetpoint() - incrementer);
TKOHardware.getDriveTalon(2)
.set(TKOHardware.getDriveTalon(2).getSetpoint() - incrementer);
System.out.println(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
TKOLogger.getInstance()
.addMessage(
"Ncoder Left: "
+ TKOHardware.getDriveTalon(0).getPosition()
+ "\t Ncoder Rgith: "
+ TKOHardware.getDriveTalon(2).getPosition()
+ "\t Left Setpoint: "
+ TKOHardware.getDriveTalon(0).getSetpoint());
Timer.delay(0.001);
}
}
TKOHardware.getDriveTalon(0).set(distance);
TKOHardware.getDriveTalon(2).set(distance);
while (Math.abs(TKOHardware.getLeftDrive().getPosition() - distance) > threshold
&& DriverStation.getInstance().isEnabled()) {
// not close enough doe; actually gets stuck here
TKOLogger.getInstance()
.addMessage(
"NOT CLOSE ENOUGH TO TARGET DIST: "
+ (TKOHardware.getLeftDrive().getPosition() - distance));
Timer.delay(0.001);
}
} catch (TKOException e1) {
e1.printStackTrace();
System.out.println("Error at another expected spot, I would assume....");
}
System.out.println("Done executing");
}
/**
* Sends data back to LabVIEW dashboard on computer. For camera targeting data, sensor feedback,
* etc.
*
* @author Ziv
*/
public class Dashboards {
private DriverStation ds = DriverStation.getInstance();
private Dashboard lowDash = ds.getDashboardPackerLow();
private static Dashboards instance = null;
private Dashboards() {}
public static Dashboards getInstance() {
if (instance == null) {
instance = new Dashboards();
}
return instance;
}
public void sendContinuousData() {
// LabVIEW stuff.
lowDash.addCluster(); // top
lowDash.addCluster(); // target1 tracking
lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxCornerX);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxCornerY);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxWidth);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget1().rawBboxHeight);
lowDash.finalizeCluster();
lowDash.addCluster(); // target2 tracking
lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxCornerX);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxCornerY);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxWidth);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget2().rawBboxHeight);
lowDash.finalizeCluster();
lowDash.addCluster(); // target3 tracking
lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxCornerX);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxCornerY);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxWidth);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget3().rawBboxHeight);
lowDash.finalizeCluster();
lowDash.addCluster(); // target4 tracking
lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxCornerX);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxCornerY);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxWidth);
lowDash.addInt((int) CommandBase.cam.tracker.getTarget4().rawBboxHeight);
lowDash.finalizeCluster();
lowDash.addDouble(CommandBase.cam.tracker.getBestTarget().centerX);
// Shooter.
lowDash.addDouble(CommandBase.shooter.getSetpoint());
lowDash.addDouble(CommandBase.shooter.getRate());
// Elevator.
lowDash.addDouble(CommandBase.shooter.getPower());
lowDash.finalizeCluster();
lowDash.commit();
}
public void sendPeriodicData() {
// SmartDashboard output stuff.
SmartDashboard.putDouble("Force Sensor Value", CommandBase.elev.getPressure());
SmartDashboard.putDouble("Yaw position", CommandBase.dt.yawGyro.getAngle());
SmartDashboard.putDouble("Target1 X", CommandBase.cam.tracker.pidGet());
SmartDashboard.putDouble("Cam servo", CommandBase.cam.getPos());
SmartDashboard.putDouble("Shooter rate", CommandBase.shooter.getRate());
SmartDashboard.putDouble("DT left", CommandBase.dt.getLeftPos());
SmartDashboard.putDouble("DT right", CommandBase.dt.getRightPos());
SmartDashboard.putDouble("Cam pos", CommandBase.cam.getPos());
SmartDashboard.putDouble("Shooter power", CommandBase.shooter.getPower());
SmartDashboard.putDouble("Throttle", CommandBase.oi.getDriveThrottle());
SmartDashboard.putDouble("Wheel", CommandBase.oi.getDriveWheel());
SmartDashboard.putDouble("Cam delta", CommandBase.oi.getCamDelta());
}
}