diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index c918e31..aa1019e 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -20,7 +20,8 @@ public class Robot extends IterativeRobot { cameraServer = CameraServer.getInstance(); cameraServer.setQuality(50); cameraServer.startAutomaticCapture("cam0"); - chooser.addDefault("Time based low bar", new RoutineDriveTime()); + chooser.addDefault("Nothing", new RoutineNothing()); + chooser.addObject("Time based low bar", new RoutineDriveTime()); chooser.addObject("Time based defense", new RoutineDefenseTime()); SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController()); diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java index c2b1ed5..5d47777 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoResetLower.java @@ -22,7 +22,7 @@ public class AutoResetLower extends CommandBase { // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { // Stop when bottom limit switch is hit - return mainArm.getBottomPressed(); + return mainArm.getBottomPressed(); } // Called once after isFinished returns true protected void end() { diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java index 315f4d4..6efc6c3 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoSetArmPosition.java @@ -10,6 +10,7 @@ public class AutoSetArmPosition extends CommandBase { public AutoSetArmPosition(double p) { requires(mainArm); pos = p; + setTimeout(3); } // Called just before this Command runs the first time protected void initialize() { @@ -27,7 +28,7 @@ public class AutoSetArmPosition extends CommandBase { System.out.println("AutoSetArmPosition.isFinished(): " + (Math.abs(pos - mainArm.getDegrees()) <= 1)); System.out.println(" pos : " + pos); System.out.println(" mainArm.getDegrees() : " + mainArm.getDegrees()); - return Math.abs(pos - mainArm.getDegrees()) <= 1; + return (Math.abs(pos - mainArm.getDegrees()) <= 1) || isTimedOut(); } // Called once after isFinished returns true protected void end() { diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java index cc0833d..5d0daa4 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java @@ -5,7 +5,7 @@ import org.usfirst.frc.team2059.robot.Robot; public class RoutineDefenseTime extends CommandGroup { public RoutineDefenseTime() { addSequential(new AutoSetArmStopState(false)); - addSequential(new AutoSetArmPosition(10)); - addSequential(new AutoDriveTime(2, .5)); + addSequential(new AutoSetArmPosition(6)); + addSequential(new AutoDriveTime(3, 1)); } } diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java index 99711d6..b4e96c7 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java @@ -6,6 +6,6 @@ public class RoutineDriveTime extends CommandGroup { public RoutineDriveTime() { addSequential(new AutoSetArmStopState(true)); addSequential(new AutoResetLower(-1)); - addSequential(new AutoDriveTime(2, .5)); + addSequential(new AutoDriveTime(3, 0.75)); } } diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineNothing.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineNothing.java new file mode 100644 index 0000000..bd34766 --- /dev/null +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineNothing.java @@ -0,0 +1,8 @@ +package org.usfirst.frc.team2059.robot.commands.autonomous; +import org.usfirst.frc.team2059.robot.commands.CommandBase; +import edu.wpi.first.wpilibj.command.CommandGroup; +import org.usfirst.frc.team2059.robot.Robot; +public class RoutineNothing extends CommandGroup { + public RoutineNothing() { + } +} diff --git a/src/org/usfirst/frc/team2059/robot/commands/drivetrain/Drive.java b/src/org/usfirst/frc/team2059/robot/commands/drivetrain/Drive.java index df609d1..a21ed5d 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/drivetrain/Drive.java +++ b/src/org/usfirst/frc/team2059/robot/commands/drivetrain/Drive.java @@ -20,7 +20,7 @@ public class Drive extends CommandBase { if (Robot.oi.getJoysticks()[0].getRawButton(2) || Robot.oi.getJoysticks()[0].getRawButton(11)) { sensitivity = 1; } else { - sensitivity = 0.5; + sensitivity = 0.7; } driveBase.driveArcade(x, y, z, sensitivity); } diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/MoveArm.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/MoveArm.java index 4107269..4bb64be 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/MoveArm.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/MoveArm.java @@ -23,11 +23,6 @@ public class MoveArm extends CommandBase { // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { //TODO: I don't know if this should go here - if (SmartDashboard.getBoolean("UseLimitSwitches")) { - if (mainArm.getTopPressed() || mainArm.getBottomPressed()) { - return true; - } - } return false; } // Called once after isFinished returns true diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 83b9bd0..04c03a3 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -15,7 +15,7 @@ public class MainArm extends PIDSubsystem { private double min = RobotMap.zeroDegrees; private double max = RobotMap.ninetyDegrees; public MainArm() { - super("MainArm", 0.07, 0.0, 0.00); + super("MainArm", 0.3, 0.0, 0.4); getPIDController().setContinuous(false); setSetpoint(70); enable();