public void autoDrive() {
double speed = 0;
if (distPID.isEnable()) {
speed = distPID.get();
}
double turn = 0;
if (turnPID.isEnable()) {
turn = turnPID.get();
}
if (speed > 0) {
drive.drive(speed, turn);
} else {
if (turn > 0) {
// turn right
tankDrive(turn, -turn);
} else if (turn < 0) {
tankDrive(-turn, turn);
}
}
SmartDashboard.putDouble("left pwm", leftJag.get());
SmartDashboard.putDouble("right pwm", rightJag.get());
SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
SmartDashboard.putDouble("turn error", turnPID.getError());
// System.out.println("driving " + speed + " " + turn);
}
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());
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
// implement manual jog //
double y = oi.getNESStick().getY();
if (Math.abs(y) > 0.1) {
double val = this.camera.getPitch() - y * PITCH_JOG;
if (val > MAX_PITCH) val = MAX_PITCH;
if (val < MIN_PITCH) val = MIN_PITCH;
this.camera.setPitch(val);
}
double x = oi.getNESStick().getX();
if (Math.abs(x) > 0.1) {
double val = this.camera.getYaw() + x * YAW_JOG;
if (val > MAX_YAW) val = MAX_YAW;
if (val < MIN_YAW) val = MIN_YAW;
this.camera.setYaw(val);
}
// preset positions //
if (oi.getNESStick().getAButton().get()) this.setPosition(pFORWARD);
else if (oi.getNESStick().getBButton().get()) this.setPosition(pDOWN);
else if (oi.getNESStick().getStartButton().get()) this.setPosition(pDRIVE);
SmartDashboard.putDouble("camera pitch", this.camera.getPitch());
SmartDashboard.putDouble("camera yaw", this.camera.getYaw());
}
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 tankDrive(double left, double right) {
drive.tankDrive(left, right);
SmartDashboard.putDouble("left pwm", left);
SmartDashboard.putDouble("right pwm", right);
}
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());
}