diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 524e8f4..ecaca29 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -35,11 +35,14 @@ public class OI { // Print log when button 1 pressed //joystickButtons[0][0].whenPressed(new LogEncoder()); joystickButtons[0][0].whileHeld(new SetShooterState(true)); - joystickButtons[0][3].whileHeld(new SpinRollers(1)); + joystickButtons[0][2].whileHeld(new SpinRollers(-0.5,false)); + joystickButtons[0][3].whileHeld(new SpinRollers(1,false)); + joystickButtons[0][6].whileHeld(new SpinRollers(1,true)); // joystickButtons[0][2].whileHeld(new PIDDrive(400)); joystickButtons[1][0].whileHeld(new MoveArm(1)); joystickButtons[1][1].whileHeld(new MoveArm(-1)); - joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect)); + //joystickButtons[1][2].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCollect)); + joystickButtons[1][2].whileHeld(new ResetLower(-1)); joystickButtons[1][3].whileHeld(new SetArmPosition(RobotMap.mainArmPresetTraverse)); joystickButtons[1][4].whileHeld(new SetArmPosition(RobotMap.mainArmPresetCloseShot)); joystickButtons[1][5].whileHeld(new SetArmPosition(RobotMap.mainArmPresetFarShot)); diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index ec33e89..c16ad14 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -2,6 +2,7 @@ package org.usfirst.frc.team2059.robot; import org.usfirst.frc.team2059.robot.commands.CommandBase; import org.usfirst.frc.team2059.robot.commands.autonomous.*; import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.livewindow.LiveWindow; @@ -11,18 +12,22 @@ public class Robot extends IterativeRobot { public static OI oi; Command autonomousCommand; SendableChooser chooser; + CameraServer cameraServer; public void robotInit() { CommandBase.init(); oi = new OI(); chooser = new SendableChooser(); - chooser.addDefault("Time based drive", new RoutineDriveTime()); + cameraServer = CameraServer.getInstance(); + cameraServer.setQuality(50); + cameraServer.startAutomaticCapture("cam0"); + chooser.addDefault("Time based low bar", new RoutineDriveTime()); chooser.addObject("Time based defense", new RoutineDefenseTime()); SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putData("MainArm", CommandBase.mainArm.getPIDController()); SmartDashboard.putData("LeftEncoderController", CommandBase.driveBase.getLeftController()); SmartDashboard.putBoolean("CompressorEnabled", true); //Automatically determine if rolling in or rolling out - SmartDashboard.putBoolean("SmartRollers", true); + SmartDashboard.putBoolean("SmartRollers", false); //Use the limit swithces on the shooter SmartDashboard.putBoolean("UseLimitSwitches", true); } diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index b1791cb..2acb72d 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -28,7 +28,7 @@ public class RobotMap { public static int armStopSolenoidTwo = 5; //Misc public static int mainArmPresetCollect = -5; - public static int mainArmPresetTraverse = 11; + public static int mainArmPresetTraverse = 10; public static int mainArmPresetCloseShot = 90; public static int mainArmPresetFarShot = 70; } diff --git a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java index 6323183..ec8ce4a 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/AutoDriveTime.java @@ -11,7 +11,7 @@ public class AutoDriveTime extends CommandBase { protected void initialize() { } protected void execute() { - driveBase.driveArcade(0, -power, 0, 0); + driveBase.driveArcade(0, -power, 0, 1); } protected void end() { driveBase.stopDriving(); 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 6d65c8e..cc0833d 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDefenseTime.java @@ -4,11 +4,8 @@ import edu.wpi.first.wpilibj.command.CommandGroup; import org.usfirst.frc.team2059.robot.Robot; public class RoutineDefenseTime extends CommandGroup { public RoutineDefenseTime() { - addSequential(new AutoSetArmStopState(true)); - addSequential(new AutoResetLower(-1)); - addSequential(new AutoSetArmPosition(15)); addSequential(new AutoSetArmStopState(false)); - addSequential(new AutoSetArmPosition(11)); -// addSequential(new AutoDriveTime(2,.5)); + addSequential(new AutoSetArmPosition(10)); + addSequential(new AutoDriveTime(2, .5)); } } 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 54ac0aa..99711d6 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java +++ b/src/org/usfirst/frc/team2059/robot/commands/autonomous/RoutineDriveTime.java @@ -4,8 +4,8 @@ import edu.wpi.first.wpilibj.command.CommandGroup; import org.usfirst.frc.team2059.robot.Robot; public class RoutineDriveTime extends CommandGroup { public RoutineDriveTime() { - addSequential(new AutoResetLower(-1)); addSequential(new AutoSetArmStopState(true)); + addSequential(new AutoResetLower(-1)); addSequential(new AutoDriveTime(2, .5)); } } 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 1988660..df609d1 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/drivetrain/Drive.java +++ b/src/org/usfirst/frc/team2059/robot/commands/drivetrain/Drive.java @@ -17,7 +17,7 @@ public class Drive extends CommandBase { double x = Robot.oi.getJoysticks()[0].getRawAxis(0); double y = Robot.oi.getJoysticks()[0].getRawAxis(1); double z = Robot.oi.getJoysticks()[0].getRawAxis(2); - if (Robot.oi.getJoysticks()[0].getRawButton(2)) { + if (Robot.oi.getJoysticks()[0].getRawButton(2) || Robot.oi.getJoysticks()[0].getRawButton(11)) { sensitivity = 1; } else { sensitivity = 0.5; diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java index e6a982e..3029439 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/ResetLower.java @@ -15,7 +15,6 @@ public class ResetLower extends CommandBase { } // Called repeatedly when this Command is scheduled to run protected void execute() { - System.out.println("test"); mainArm.disable(); mainArm.resetLower(speed); } diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java index 7a1e386..294365a 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/SetArmPosition.java @@ -17,11 +17,6 @@ public class SetArmPosition extends CommandBase { // Called repeatedly when this Command is scheduled to run protected void execute() { //Move the arm stop - if (pos == RobotMap.mainArmPresetCollect) { - pneumatics.setArmStopState(true); - } else if ((pos != RobotMap.mainArmPresetCollect) && (mainArm.getDegrees() > 2)) { - pneumatics.setArmStopState(false); - } mainArm.enable(); mainArm.setSetpoint(pos); } diff --git a/src/org/usfirst/frc/team2059/robot/commands/shooter/SpinRollers.java b/src/org/usfirst/frc/team2059/robot/commands/shooter/SpinRollers.java index 5731589..0d275c1 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/shooter/SpinRollers.java +++ b/src/org/usfirst/frc/team2059/robot/commands/shooter/SpinRollers.java @@ -7,15 +7,17 @@ import org.usfirst.frc.team2059.robot.Robot; */ public class SpinRollers extends CommandBase { double speed; - public SpinRollers(double s) { + boolean smartrollers; + public SpinRollers(double s, boolean sr) { speed = s; + smartrollers = sr; } // Called just before this Command runs the first time protected void initialize() { } // Called repeatedly when this Command is scheduled to run protected void execute() { - if (SmartDashboard.getBoolean("SmartRollers") && mainArm.getDegrees() < 5) { + if (smartrollers && (mainArm.getDegrees() < 5 || mainArm.getBottomPressed())) { shooter.shootAtSpeed(-.5); } else { shooter.shootAtSpeed(speed); diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 3a7ae12..04c03a3 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -17,6 +17,7 @@ public class MainArm extends PIDSubsystem { public MainArm() { super("MainArm", 0.3, 0.0, 0.4); getPIDController().setContinuous(false); + setSetpoint(70); enable(); } public void initDefaultCommand() {