/** Update normal driver control */
private void updateDriveControl() {
// Set speed to twist because the robot should turn in place w/ button 2
if (driverstation.JoystickDriverButton3) {
speed = driverstation.JoystickDriverTwist;
} else {
speed =
(driverstation.getStickDistance(
driverstation.JoystickDriverX, driverstation.JoystickDriverY));
}
// sets the drive speed so it will not drive below a minimum speed
if (Math.abs(speed) < MINUMUM_DRIVE_SPEED) {
speed = 0.0;
}
// Decide drive mode
if (driverstation.JoystickDriverButton3) {
if (driverstation.JoystickDriverTrigger) {
speed = speed / 2;
}
// Rotate in place if button 3 is pressed
drive.turnDrive(-speed);
} else {
// Normally use crab drive
drive.crabDrive(
driverstation.getStickAngle(driverstation.JoystickDriverX, driverstation.JoystickDriverY),
speed);
}
}
/** This is called constantly called by the task manager. */
@Override
public void execute() {
drive.setPower(power);
drive.execute();
if (timer.get() > endTime) {
isFinished = true;
}
}
/** Runs drive code */
private void joyDrive() {
double[] rTheta = robotCore.joy.getRTheta();
drive.move(rTheta[0], rTheta[1]);
if (robotCore.joy.getDpadUp()) {
drive.setReverseMode(true);
}
if (robotCore.joy.getDpadDown()) {
drive.setReverseMode(false);
}
// System.out.println(rTheta[0] + "\t" + (rTheta[1] * 180/Math.PI));
}
/** 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);
}
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 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 run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
// Make robot objects
driverstation = Driverstation.getInstance();
drive = Drive.getInstance();
shooter = Shooter.getInstance();
// lifterobject = LifterObject.getInstance();
// compressor = Compressor467.getInstance();
Calibration.init();
Autonomous.init();
TableHandler.init();
// AxisCamera.getInstance();
}
/** Update steering calibration control */
private void updateCalibrateControl() {
double stickAngle =
driverstation.getStickAngle(driverstation.JoystickDriverX, driverstation.JoystickDriverY);
// Branch into motor being calibrated
if (driverstation.getStickDistance(driverstation.JoystickDriverX, driverstation.JoystickDriverY)
> 0.5) {
if (stickAngle < 0) {
if (stickAngle < -0.5) {
motorId = RobotMap.BACK_LEFT;
} else {
motorId = RobotMap.FRONT_LEFT;
}
} else {
if (stickAngle > 0) {
if (stickAngle > 0.5) {
motorId = RobotMap.BACK_RIGHT;
} else {
motorId = RobotMap.FRONT_RIGHT;
}
}
}
}
// Prints selected motor to the driverstation
printSelectedMotor();
// Determine calibration mode
if (driverstation.JoystickDriverButton3) {
Calibration.stopWheelCalibrate();
steerMode = true;
}
if (driverstation.JoystickDriverButton4 && button4Debounce) {
Calibration.toggleWheelCalibrate();
steerMode = false;
button4Debounce = false;
}
if (!driverstation.JoystickDriverButton4) {
button4Debounce = true;
}
// Branch into type of calibration
if (steerMode) {
driverstation.println("Steering Calibrate", 3);
Calibration.updateSteeringCalibrate(motorId);
System.out.println(drive.getSteeringAngle(motorId));
} else {
driverstation.println("Wheel Calibrate", 3);
Calibration.updateWheelCalibrate(motorId);
}
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
// Class teleop functions
tankDrive.teleopPeriodic();
driveShift.teleopPeriodic();
arm.teleopPeriod();
// Shooter functions
shooter.intake();
shooter.armPivot();
shooter.armPistons();
// Runs duel cameras
cameraFeeds.run();
}
public static void main(String[] args) {
try {
UIManager.setLookAndFeel(UIManager.getSystemLookAndFeelClassName());
} catch (Exception e) {
JOptionPane.showMessageDialog(null, "Fatal Error Occurred!");
System.exit(1);
}
/*View.SplashWindow s = new View.SplashWindow();
s.setVisible(true);*/
String fPath = args.length == 0 ? JOptionPane.showInputDialog("Enter source path") : args[0];
File f = new File(fPath);
if (!f.exists()) {
System.out.println("Path does not exist!");
System.exit(1);
}
String fDrive = fPath.substring(0, 3);
File dFile = new File(fDrive);
File[] dirs = Drive.getAllDrives();
ArrayList<Drive> drives = new ArrayList<Drive>();
for (int i = 0; i < dirs.length; i++) {
if (!dirs[i].getPath().equals(dFile.getPath())) {
System.out.println("Loading Drive " + dirs[i].getPath() + "...");
drives.add(new Drive(dirs[i]));
}
}
System.out.println("Done! Loading Interface...");
// s.setVisible(false);
Drive[] dr = new Drive[drives.size()];
for (int i = 0; i < dr.length; i++) {
dr[i] = drives.get(i);
}
new View(dr, f);
}
/** 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();
}
/**
* This autonomous (along with the chooser code above) shows how to select between different
* autonomous modes using the dashboard. The sendable chooser code works with the Java
* SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
* uncomment the getString line to get the auto name from the text box below the Gyro
*
* <p>You can add additional auto modes by adding additional comparisons to the switch structure
* below with additional strings. If using the SendableChooser make sure to add them to the
* chooser code above as well.
*/
public void autonomousInit() {
autoSelected = (String) chooser.getSelected();
// autoSelected = SmartDashboard.getString("Auto Selector", defaultAuto);
System.out.println("Auto selected: " + autoSelected);
switch (autoSelected) {
case touch:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 2 sec
Timer.delay(2);
tankDrive.auto(0, 0); // Stop robot
break;
case lowBarCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.60); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 4.5 sec
Timer.delay(4.5);
tankDrive.auto(0, 0); // Stop robot
break;
case rockWallCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.85); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.78, 0.85); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(4.5);
tankDrive.auto(0, 0); // Stop robot
break;
case roughTerrainCross:
Timer.delay(0.5); // Wait 0.5 sec
// shooter.armAuto(0.50); // Lower arm - run motors at 50% speed for 0.5 sec
// Timer.delay(0.5);
// shooter.armAuto(0);
// Timer.delay(1.5);
tankDrive.auto(0.0, 0.80); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(.1);
tankDrive.auto(0.73, 0.80); // Move forward - run motors at 80% speed for 4 sec
Timer.delay(4);
tankDrive.auto(0, 0); // Stop robot
break;
default:
// Nothing ...
break;
}
}
public void disabledInit() {
drive.setRun(false);
loadAndShoot.resetBooleans();
loadAndShoot.setRun(false);
}
/** Called when the task has completed. */
@Override
public void end() {
drive.setPower(0);
drive.execute();
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
lights.set(Relay.Value.kForward);
// tracking.run();
/*try {
readString = serial.readString();
readString = readString.substring(readString.indexOf("data{"), readString.indexOf("}"));
xTurn = readString.substring(readString.indexOf("x("), readString.indexOf(")"));
xTurnVal = Integer.parseInt(xTurn);
} catch (VisaException ex) {
System.out.println("Error Reading string - " + ex);
}*/
y = drive_control.getRawAxis(1);
x = drive_control.getRawAxis(2);
t = drive_control.getRawAxis(3);
if (y > 1) y = 1;
else if (y < -1) y = -1;
if (x > 1) x = 1;
else if (x < -1) x = -1;
drive.setDriveJ(x, y, t);
/*if(drive_control.getRawButton(12))
//wheelsOn = true;
else if(drive_control.getRawButton(11));
wheelsOn = false;*/
/*try{
launcher.setX((drive_control.getRawAxis(4)-1)/2);
}catch(Exception e){}*/
// System.out.println((drive_control.getRawAxis(4)-1)*-1350);
if (!autoWheels) {
csc.setSetpoint((drive_control.getRawAxis(4) - 1) * -1500);
try {
// launcher.setX((drive_control.getRawAxis(4)+1)*2000);
System.out.println(
-launcher.getSpeed()
+ " -- "
+ csc.tmpResult
+ " -- "
+ (drive_control.getRawAxis(4) - 1) * -1500);
} catch (Exception e) {
System.out.println(e);
}
if (drive_control.getRawButton(11)) autoWheels = true;
} else {
csc.setSetpoint(2300);
if (drive_control.getRawButton(11)) autoWheels = false;
}
/*if(wheelsOn)
shoot(1);
else
shoot(0);*/
if (drive_control.getRawButton(5)) {
wedge.moveDown();
// servo1.set(1);
// servo2.set(0);
// wedge.set(Relay.Value.kForward);
} else if (drive_control.getRawButton(6)) {
wedge.moveUp();
// servo1.set(0);
// servo2.set(1);
// wedge.set(Relay.Value.kReverse);
} else {
// servo1.set(.5);
// servo2.set(.5);
// wedge.set(Relay.Value.kOff);
}
if (drive_control.getRawButton(3)) turret.set(.5);
else if (drive_control.getRawButton(4)) turret.set(-.5);
else turret.set(0);
if (drive_control.getRawButton(2)) driveMainBelt(true, true);
else if (drive_control.getRawButton(7)) driveMainBelt(false, true);
else driveMainBelt(false, false);
if (drive_control.getTrigger()) {
try {
belt2.setX(-1);
} catch (Exception e) {
}
} else if (drive_control.getRawButton(8)) {
try {
belt2.setX(1);
} catch (Exception e) {
}
} else {
try {
belt2.setX(0);
} catch (Exception e) {
}
}
if (control.getRawButton(3)) {
turret.set(.25);
} else if (control.getRawButton(4)) {
turret.set(-.25);
} else {
turret.set(0);
}
try {
station.println(
DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(-launcher.getSpeed()));
} catch (CANTimeoutException ex) {
}
/*try{
if(tracking.foundTarget()){
station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
if(x>0)
station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
else if(x<0)
station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
else
station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
}
else{
station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
station.println(DriverStationLCD.Line.kUser3, 1, "");
station.println(DriverStationLCD.Line.kUser4, 1, "");
}
}catch(Exception e){}*/
station.updateLCD();
}