diff --git a/src/main/java/org/usfirst/frc/team1701/robot/OI.java b/src/main/java/org/usfirst/frc/team1701/robot/OI.java index bc8aead..af8acd3 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/OI.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/OI.java @@ -33,7 +33,6 @@ public class OI { public static JoystickButton manualWristUp; public static JoystickButton manualWristDown; public static JoystickButton gear; - public static JoystickButton manualWristReset; public static Joystick operation; @@ -75,13 +74,13 @@ public OI() { climbPosition = new JoystickButton(operation, 6); climbPosition.whenPressed(new ClimbPosition()); climb = new JoystickButton(operation, 5); - climb.whenPressed(new Climb()); + climb.whileHeld(new Climb()); climb.whenReleased(new StopClimb()); manualWinchUp = new JoystickButton(operation, 3); - manualWinchUp.whenPressed(new WinchDown()); + manualWinchUp.whileHeld(new WinchUp()); manualWinchUp.whenReleased(new WinchStop()); manualWinchDown = new JoystickButton(operation, 10); - manualWinchDown.whenPressed(new WinchUp()); + manualWinchDown.whileHeld(new WinchDown()); manualWinchDown.whenReleased(new WinchStop()); manualWristUp = new JoystickButton(operation, 7); manualWristUp.whenPressed(new WristDown()); @@ -90,9 +89,10 @@ public OI() { manualWristDown.whenPressed(new WristUp()); manualWristDown.whenReleased(new WristStop()); gear = new JoystickButton(drive_FB,3); - gear.whenPressed(new HighGear()); + gear.whenPressed(new AutoGear()); gear.whenReleased(new LowGear()); + manualWristReset = new JoystickButton(operation,15); manualWristReset.whenPressed(new ResetWrist()); } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/Robot.java b/src/main/java/org/usfirst/frc/team1701/robot/Robot.java index 7dafa32..9ae79bf 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/Robot.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/Robot.java @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import org.usfirst.frc.team1701.robot.commands.AutoCommandGroup; +import org.usfirst.frc.team1701.robot.commands.*; import org.usfirst.frc.team1701.robot.subsystems.*; /* @@ -39,44 +39,28 @@ public class Robot extends IterativeRobot { */ private DriverStation ds = DriverStation.getInstance(); private String gameCode = ""; - public static SendableChooser<Number> autonomousLocation; - public static SendableChooser<Number> action; public static OI oi; public static DriveTrain driveTrain; public static LiftArm liftArm; public static Vision vision; public static Position position; + public static Shuffleboard shuffleboard; /* * This function is run when the robot is first started up. */ public void robotInit() { RobotMap.init(); // Initialize our RobotMap. + Shuffleboard.init(); driveTrain = new DriveTrain(); vision = new Vision(); liftArm = new LiftArm(); position = new Position(); oi = new OI(); - autonomousLocation = new SendableChooser<>(); - autonomousLocation.addObject("Left",1); - autonomousLocation.addObject("Left-Switch",4); - autonomousLocation.addDefault("Middle",2); - autonomousLocation.addObject("Right-Switch",5); - autonomousLocation.addObject("Right",3); - SmartDashboard.putData("Autonomous Location", autonomousLocation); - action = new SendableChooser<>(); - action.addDefault("Defualt Autonomous", 1); - action.addObject("Forward Autonomous", 2); - SmartDashboard.putData("Autonomous Chooser",action); - SmartDashboard.putBoolean("Reversed", false); - SmartDashboard.putString("Current Gear",""); - SmartDashboard.putNumber("Arm",0); - SmartDashboard.putBoolean("Arm Down", false); - SmartDashboard.putNumber("D", 0); vision.setPIPMode(2); driveTrain.resetEncoders(); - - + driveTrain.setBrakeMode(); + driveTrain.setLowGear(); } /* * This function is called when the robot has been disabled. @@ -99,7 +83,7 @@ public void disabledPeriodic() { */ public void autonomousInit() { if(gameCode.length() == 3) { - CommandGroup autonomousCommand = new AutoCommandGroup(gameCode,action.getSelected(),autonomousLocation.getSelected()); + CommandGroup autonomousCommand = new AutoCommandGroup(gameCode, Shuffleboard.action.getSelected(), Shuffleboard.autonomousLocation.getSelected()); autonomousCommand.start(); } } @@ -128,4 +112,6 @@ public void teleopPeriodic() { * This function is called periodically during test mode. */ public void testPeriodic() {} + + } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java b/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java index 39ab210..d03ed03 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/RobotMap.java @@ -10,15 +10,13 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.drive.DifferentialDrive; -/*import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger;*/ + public class RobotMap { /* * Create all static variables, most (exception of logger) filled * by the init() method below. */ - //private static final Logger logger = LogManager.getLogger(); public static WPI_TalonSRX _leftFrontMotor; public static WPI_TalonSRX _leftBackMotor; public static WPI_TalonSRX _rightFrontMotor; @@ -36,18 +34,18 @@ public class RobotMap { public static DoubleSolenoid winchBrake; public static DoubleSolenoid wristBrake; public static AnalogInput liftArmEncoder; + public static AnalogInput wristEncoder; public static DigitalInput cubeSensor; public static DigitalInput armSensor; - public static Spark _led0; - public static Spark _led1; public static AHRS _navx; public static int encPidIdx; - + public static double armSpeed = 1.0; + /** * Initialize the public values above. */ public static void init() { - _leftFrontMotor = new WPI_TalonSRX(0); //0 + _leftFrontMotor = new WPI_TalonSRX(3); //0 _leftBackMotor = new WPI_TalonSRX(1); //1 _rightFrontMotor = new WPI_TalonSRX(15); //15 _rightBackMotor = new WPI_TalonSRX(14); //14 @@ -60,6 +58,7 @@ public static void init() { * Initialize all analog and digital objects */ liftArmEncoder = new AnalogInput(0); + wristEncoder = new AnalogInput(1); cubeSensor = new DigitalInput(0); armSensor = new DigitalInput(1); @@ -88,12 +87,6 @@ public static void init() { * Instantiate NavX. */ _navx = new AHRS(SerialPort.Port.kUSB1); - /** - * LED controllers, follows Spark motor controller pattern. - * @see https://www.revrobotics.com/content/docs/REV-11-1105-UM.pdf, page 7 - */ - _led0 = new Spark(0); - _led1 = new Spark(1); - } + } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/CrossPlatformZone.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/CrossPlatformZone.java deleted file mode 100644 index 21a318d..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/CrossPlatformZone.java +++ /dev/null @@ -1,34 +0,0 @@ -/** - * CrossPlatformZone.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class CrossPlatformZone extends Command{ - boolean isFinished = false; - - public CrossPlatformZone() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.crossPlatformZone) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java index 5b405cd..85387e2 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Drive.java @@ -1,45 +1,56 @@ package org.usfirst.frc.team1701.robot.commands.Auto; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team1701.robot.Robot; public class Drive extends Command { boolean isFinished = false; double distance = 0; double currentSpeed; + double startAngle; boolean setHighGear = false; public Drive(double distance,double speed) { + Robot.driveTrain.driveSpeed = 0; + requires(Robot.driveTrain); this.distance = distance; this.currentSpeed = speed; } public Drive(double distance, double speed, boolean setHighGear) { - this.distance = distance; - this.currentSpeed = speed; + this(distance,speed); this.setHighGear = setHighGear; - } protected void initialize() { + startAngle = Robot.driveTrain.getNavxAngle(); Robot.driveTrain.resetEncoders(); } protected void execute() { - if(setHighGear) { + if(!setHighGear) { Robot.driveTrain.setLowGear(); } else { Robot.driveTrain.setHighGear(); } + SmartDashboard.putNumber("Drive Train", Robot.driveTrain.getRightDistance()); + if(Robot.driveTrain.getRightDistance() < distance) { - Robot.driveTrain.teleopControl(-currentSpeed,0); + Robot.driveTrain.driveSpeed = -currentSpeed; + Robot.driveTrain.setAngle(startAngle); + Robot.driveTrain.startPID(); } else { + Robot.driveTrain.stopPID(); + Robot.driveTrain.stopMotors(); isFinished = true; } } protected boolean isFinished() { return isFinished; } - protected void end() {} + protected void end() { + Robot.driveTrain.stopPID(); + } protected void interrupted() {} } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/DriveForwardCommand.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/DriveForwardCommand.java deleted file mode 100644 index e2f03ee..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/DriveForwardCommand.java +++ /dev/null @@ -1,44 +0,0 @@ -/** - * commands/DriveForwardCommand.java - * - * @author Nicholas Hubbard - * @since 2018-01-31 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class DriveForwardCommand extends Command { - //distance to switch - length of robot / wheel circ - //private double forwardDistance = (168 - 38) / (4 * Math.PI); - private double forwardDistance = 11; - - private boolean isFinished = false; - - public DriveForwardCommand() { - - } - protected void initialize() { - } - protected void execute() { - - if(Robot.driveTrain.getRightDistance() < forwardDistance) { - Robot.driveTrain.setLowGear(); - Robot.driveTrain.leftDriveControl(-0.6); - Robot.driveTrain.rightDriveControl(0.6); - } else { - Robot.driveTrain.stopMotors(); - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() { - Robot.driveTrain.stopMotors(); - } - protected void interrupted() { - Robot.driveTrain.stopMotors(); - } -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/MiddleToSwitchTurn.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/MiddleToSwitchTurn.java deleted file mode 100644 index 943b68b..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/MiddleToSwitchTurn.java +++ /dev/null @@ -1,39 +0,0 @@ -/** - * MiddleToSwitchTurn.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class MiddleToSwitchTurn extends Command { - boolean isFinished = false; - int count; - - public MiddleToSwitchTurn() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - Robot.driveTrain.stopMotors(); - count = 0; - } - protected void execute() { - isFinished = false; - if(Robot.driveTrain.getRightDistance() <= Robot.position.centerToSide && count < 20) { - Robot.driveTrain.teleopControl(-1,0); - count++; - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/PlatformToScale.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/PlatformToScale.java deleted file mode 100644 index a903cd2..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/PlatformToScale.java +++ /dev/null @@ -1,34 +0,0 @@ -/** - * PlatformToScale.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class PlatformToScale extends Command { - boolean isFinished = false; - - public PlatformToScale() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.platformToScale) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/ReverseScale.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/ReverseScale.java deleted file mode 100644 index 0651c27..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/ReverseScale.java +++ /dev/null @@ -1,35 +0,0 @@ -/** - * ReverseScale.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class ReverseScale extends Command { - boolean isFinished = false; - - public ReverseScale() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.scaleReverse) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} - -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightLeft.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightLeft.java deleted file mode 100644 index 3882403..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightLeft.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class SlightLeft extends Command { - boolean isFinished = false; - public double navxStart; - - public SlightLeft() { - requires(Robot.driveTrain); - } - protected void initialize() { - navxStart = Robot.driveTrain.getNavxAngle(); - } - protected void execute() { - if(Robot.driveTrain.getNavxAngle() > navxStart + Robot.position.slightLeftAngle) { - Robot.driveTrain.teleopControl(0,0.75); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() { - Robot.driveTrain.resetEncoders(); - } - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightRight.java deleted file mode 100644 index 5e355ef..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/SlightRight.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class SlightRight extends Command{ - boolean isFinished = false; - public double navxStart; - - public SlightRight() { - requires(Robot.driveTrain); - } - protected void initialize() { - navxStart = Robot.driveTrain.getNavxAngle(); - } - protected void execute() { - if(Robot.driveTrain.getNavxAngle() > navxStart - Robot.position.slightRightAngle) { - Robot.driveTrain.teleopControl(0,-0.75); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() { - Robot.driveTrain.resetEncoders(); - } - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleLeft.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Turn.java similarity index 53% rename from src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleLeft.java rename to src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Turn.java index efdc71e..5e79c03 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleLeft.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/Turn.java @@ -1,32 +1,29 @@ -/** - * WallToMiddleRight.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ package org.usfirst.frc.team1701.robot.commands.Auto; import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; -public class WallToMiddleLeft extends Command { +public class Turn extends Command { + double startAngle = 0; boolean isFinished = false; - public WallToMiddleLeft() { + public Turn(double angle) { requires(Robot.driveTrain); + this.startAngle = angle + Robot.driveTrain.getNavxAngle(); } protected void initialize() { - Robot.driveTrain.resetEncoders(); } protected void execute() { - isFinished = false; - if(Robot.driveTrain.getRightDistance() < Robot.position.wallToCubeLeft) { - Robot.driveTrain.teleopControl(-1,0); - } else { + Robot.driveTrain.driveSpeed = 0; + Robot.driveTrain.setAngle(startAngle); + Robot.driveTrain.startPID(); + if(Robot.driveTrain.onTarget()) { + Robot.driveTrain.stopPID(); + Robot.driveTrain.stopMotors(); isFinished = true; } + } protected boolean isFinished() { return isFinished; diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnLeft.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnLeft.java deleted file mode 100644 index 9f2c722..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnLeft.java +++ /dev/null @@ -1,38 +0,0 @@ -/** - * TurnLeft.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class TurnLeft extends Command { - boolean isFinished = false; - - public double navxStart; - - public TurnLeft() { - requires(Robot.driveTrain); - } - protected void initialize() { - navxStart = Robot.driveTrain.getNavxAngle(); - } - protected void execute() { - if(Robot.driveTrain.getNavxAngle() < navxStart + Robot.position.leftAngle) { - Robot.driveTrain.teleopControl(0,0.75); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() { - Robot.driveTrain.resetEncoders(); - } - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnRight.java deleted file mode 100644 index 6e09011..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnRight.java +++ /dev/null @@ -1,37 +0,0 @@ -/** - * TurnRight.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class TurnRight extends Command { - boolean isFinished = false; - public double navxStart; - - public TurnRight() { - requires(Robot.driveTrain); - } - protected void initialize() { - navxStart = Robot.driveTrain.getNavxAngle(); - } - protected void execute() { - if(Robot.driveTrain.getNavxAngle() > navxStart - Robot.position.rightAngle) { - Robot.driveTrain.teleopControl(0,-0.75); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() { - Robot.driveTrain.resetEncoders(); - } - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitch.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitch.java deleted file mode 100644 index 16dbf87..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitch.java +++ /dev/null @@ -1,34 +0,0 @@ -/** - * TurnToSwitch.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class TurnToSwitch extends Command { - boolean isFinished = false; - - public TurnToSwitch() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.turnToSwitch) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitchRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitchRight.java deleted file mode 100644 index 9ef4c32..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/TurnToSwitchRight.java +++ /dev/null @@ -1,34 +0,0 @@ -/** - * TurnToSwitch.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class TurnToSwitchRight extends Command { - boolean isFinished = false; - - public TurnToSwitchRight() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.turnToSwitchRight) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleRight.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleRight.java deleted file mode 100644 index fc12d36..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToMiddleRight.java +++ /dev/null @@ -1,36 +0,0 @@ -/** - * WallToMiddleRight.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class WallToMiddleRight extends Command { - - boolean isFinished = false; - - public WallToMiddleRight() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - isFinished = false; - if(Robot.driveTrain.getRightDistance() < Robot.position.wallToCubeRight) { - Robot.driveTrain.teleopControl(-1,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToPlatformZone.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToPlatformZone.java deleted file mode 100644 index 526e1a4..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToPlatformZone.java +++ /dev/null @@ -1,34 +0,0 @@ -/** - * WallToPlatformZone.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class WallToPlatformZone extends Command { - boolean isFinished = false; - - public WallToPlatformZone() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.wallToPlatformZone) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToScale.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToScale.java deleted file mode 100644 index 56bf4dd..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToScale.java +++ /dev/null @@ -1,34 +0,0 @@ -/** - * WallToScale.java - * - * @author Noah Husby - * @since 2/22/18 - * @license BSD-3-Clause - */ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class WallToScale extends Command { - boolean isFinished = false; - - public WallToScale() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.wallToScale) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToStraightSwitch.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToStraightSwitch.java deleted file mode 100644 index f1f8f98..0000000 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/Auto/WallToStraightSwitch.java +++ /dev/null @@ -1,28 +0,0 @@ -package org.usfirst.frc.team1701.robot.commands.Auto; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team1701.robot.Robot; - -public class WallToStraightSwitch extends Command { - boolean isFinished = false; - - public WallToStraightSwitch() { - requires(Robot.driveTrain); - } - protected void initialize() { - Robot.driveTrain.resetEncoders(); - } - protected void execute() { - if(Robot.driveTrain.getRightDistance() < Robot.position.wallToStraightSwitch) { - Robot.driveTrain.teleopControl(-Robot.position.autonomousSpeed,0); - } else { - isFinished = true; - } - } - protected boolean isFinished() { - return isFinished; - } - protected void end() {} - protected void interrupted() {} - -} diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java index 62125bd..5cd483c 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/AutoCommandGroup.java @@ -33,11 +33,12 @@ public AutoCommandGroup(String gameCode, Number action, Number autoLocation) { switch ((int) action) { case 1: - placeCubeAutonomous((int) Robot.autonomousLocation.getSelected()); + placeCubeAutonomous((int) Robot.shuffleboard.autonomousLocation.getSelected()); + SmartDashboard.putString("GameCode", gameCode); break; case 2: - addSequential(new DriveForwardCommand()); + addSequential(new Drive(11,0.6)); break; } @@ -45,58 +46,66 @@ public AutoCommandGroup(String gameCode, Number action, Number autoLocation) { private void placeCubeAutonomous(int autoLocation) { if (autoLocation == 1) { + //Left switch (scalePosition) { case 'L': - addSequential(new WallToScale()); - addSequential(new StowPosition()); - addSequential(new TurnLeft()); + Robot.driveTrain.setAutoGear(false); + Robot.driveTrain.setLowGear(); + addSequential(new Drive(20,0.82)); + addSequential(new Turn(135)); addSequential(new ScalePosition()); - addSequential(new ReverseScale()); addSequential(new ReleaseAndPunch()); break; case 'R': - addSequential(new WallToPlatformZone()); - addParallel(new StowPosition()); - addSequential(new TurnRight()); - addSequential(new CrossPlatformZone()); - addSequential(new TurnLeft()); - addParallel(new ScalePosition()); - addSequential(new PlatformToScale()); - addSequential(new ReleaseAndPunch()); + addSequential(new Drive(10,0.8)); break; } } else if (autoLocation == 2) { + //Middle switch (switchPosition) { case 'L': - addSequential(new WallToMiddleLeft()); - addSequential(new StowPosition()); - addSequential(new SlightLeft()); addSequential(new SwitchPosition()); - addSequential(new TurnToSwitch()); + addSequential(new Drive(1,0.8)); + addSequential(new Turn(29)); + addSequential(new Drive(7,.9)); addSequential(new ReleaseAndPunch()); break; case 'R': - addSequential(new WallToMiddleRight()); - addSequential(new StowPosition()); - addSequential(new SlightRight()); addSequential(new SwitchPosition()); - addSequential(new TurnToSwitchRight()); + addSequential(new Drive(1,0.8)); + addSequential(new Turn(-27)); + addSequential(new Drive(7,.9)); addSequential(new ReleaseAndPunch()); + break; } } else if (autoLocation == 3) { + //Right + switch (scalePosition) { + case 'L': + addSequential(new Drive(10,0.8)); + break; + case 'R': + Robot.driveTrain.setAutoGear(false); + Robot.driveTrain.setLowGear(); + addSequential(new Drive(20,0.82)); + addSequential(new Turn(-135)); + addSequential(new ScalePosition()); + addSequential(new ReleaseAndPunch()); + break; + } } else if (autoLocation == 4) { //Left-Switch switch (switchPosition) { case 'L': addSequential(new SwitchPosition()); - addSequential(new WallToStraightSwitch()); + addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed)); addSequential(new ReleaseCube()); break; case 'R': addSequential(new SwitchPosition()); - addSequential(new WallToStraightSwitch()); + addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed)); break; } } else if (autoLocation == 5) { @@ -104,14 +113,16 @@ private void placeCubeAutonomous(int autoLocation) { switch (switchPosition) { case 'L': addSequential(new SwitchPosition()); - addSequential(new WallToStraightSwitch()); + addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed)); break; case 'R': addSequential(new SwitchPosition()); - addSequential(new WallToStraightSwitch()); + addSequential(new Drive(Robot.position.wallToStraightSwitch,Robot.position.autonomousSpeed)); addSequential(new ReleaseCube()); break; } + } else if (autoLocation == 6) { + addSequential(new Turn(-90)); } } } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java index 9e9381a..3c671e5 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ClimbPositionArm.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class ClimbPositionArm extends Command { public ClimbPositionArm() { @@ -9,8 +10,6 @@ public ClimbPositionArm() { } private boolean isFinished; - - protected void initialize() {} protected void execute() { @@ -20,16 +19,16 @@ protected void execute() { Robot.liftArm.setGrabber(true); if(Robot.liftArm.getArmAngle()> Robot.position.armClimb + 100) { - Robot.liftArm.setLiftArm(0.90); } + Robot.liftArm.setLiftArm(RobotMap.armSpeed); } else if(Robot.liftArm.getArmAngle()< Robot.position.armClimb - 100) { - Robot.liftArm.setLiftArm(-0.90); + Robot.liftArm.setLiftArm(-RobotMap.armSpeed); } else { Robot.liftArm.stopLiftArm(); Robot.liftArm.disableWristBrake(); - if(Robot.liftArm.getWristAngle() > Robot.position.wristClimb + 50) { + if(Robot.liftArm.getWristAngle() > Robot.position.wristClimb + 15) { Robot.liftArm.setWrist(0.50); } - else if(Robot.liftArm.getWristAngle() < Robot.position.wristClimb - 50) { + else if(Robot.liftArm.getWristAngle() < Robot.position.wristClimb - 15) { Robot.liftArm.setWrist(-0.50); } else { diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java index 93144db..573491b 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabCube.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class GrabCube extends Command{ @@ -19,11 +20,14 @@ protected void initialize() { } protected void execute() { + //RobotMap.cancelAll = true; Robot.liftArm.setGrabber(true); } protected boolean isFinished() { return true; } - protected void end() {} + protected void end() { + + } protected void interrupted() {} } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java index 14a3cd3..69d6ada 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPosition.java @@ -8,6 +8,7 @@ package org.usfirst.frc.team1701.robot.commands; import edu.wpi.first.wpilibj.command.CommandGroup; + public class GrabPosition extends CommandGroup { public GrabPosition() { diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java index 0e654c6..8dde2a0 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/GrabPositionArm.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class GrabPositionArm extends Command { public GrabPositionArm() { @@ -18,27 +19,27 @@ public GrabPositionArm() { private boolean isFinished; - protected void initialize() {} protected void execute() { + Robot.liftArm.disableWristBrake(); Robot.liftArm.winchHighGear(); isFinished = false; Robot.liftArm.setGrabber(true); - if(Robot.liftArm.getArmAngle()> Robot.position.armGrab + 100) { - Robot.liftArm.setLiftArm(0.90); } - else if(Robot.liftArm.getArmAngle()< Robot.position.armGrab - 100) { - Robot.liftArm.setLiftArm(-0.90); + if(Robot.liftArm.getArmAngle()> Robot.position.armGrab + 8) { + Robot.liftArm.setLiftArm(RobotMap.armSpeed); } + else if(Robot.liftArm.getArmAngle()< Robot.position.armGrab - 8) { + Robot.liftArm.setLiftArm(-RobotMap.armSpeed); } else { Robot.liftArm.stopLiftArm(); Robot.liftArm.disableWristBrake(); - if(Robot.liftArm.getWristAngle() > Robot.position.wristGrab + 50) + if(Robot.liftArm.getWristAngle() > Robot.position.wristGrab + 15) { Robot.liftArm.setWrist(0.50); } - else if(Robot.liftArm.getWristAngle() < Robot.position.wristGrab - 50) + else if(Robot.liftArm.getWristAngle() < Robot.position.wristGrab - 15) { Robot.liftArm.setWrist(-0.50); } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java index aa5ccdc..0dcd130 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseAndPunch.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class ReleaseAndPunch extends Command { @@ -20,14 +21,18 @@ protected void initialize() { } protected void execute() { + //RobotMap.cancelAll = true; Robot.liftArm.setGrabber(false); Robot.liftArm.extendPuncher(); Timer.delay(0.5); Robot.liftArm.retractPuncher(); + } protected boolean isFinished() { return true; } - protected void end() {} + protected void end() { + + } protected void interrupted() {} } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java index 508350a..0923197 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ReleaseCube.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class ReleaseCube extends Command{ @@ -20,10 +21,12 @@ protected void initialize() { protected void execute() { Robot.position.isReleasePressed = true; Robot.liftArm.setGrabber(false); + } protected boolean isFinished() { return true; } - protected void end() {} + protected void end() { + } protected void interrupted() {} } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java index 11a7d94..b2187c8 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/ScalePositionArm.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class ScalePositionArm extends Command { public ScalePositionArm() { @@ -17,6 +18,9 @@ public ScalePositionArm() { } private boolean isFinished; + + + private void stowWrist() { Robot.liftArm.setGrabber(true); @@ -30,9 +34,9 @@ private void stowWrist() { } } private void grabWrist() { - if(Robot.liftArm.getWristAngle() > Robot.position.wristScale + 50) { + if(Robot.liftArm.getWristAngle() > Robot.position.wristScale + 15) { Robot.liftArm.setWrist(0.50); - } else if(Robot.liftArm.getWristAngle() < Robot.position.wristScale - 50) { + } else if(Robot.liftArm.getWristAngle() < Robot.position.wristScale - 15) { Robot.liftArm.setWrist(-0.50); } else { Robot.liftArm.stopWrist(); @@ -47,10 +51,10 @@ protected void execute() { Robot.liftArm.setGrabber(true); if(Robot.liftArm.getArmAngle()> Robot.position.armScale + 100) { - Robot.liftArm.setLiftArm(0.80); + Robot.liftArm.setLiftArm(RobotMap.armSpeed); stowWrist(); } else if(Robot.liftArm.getArmAngle()< Robot.position.armScale - 100) { - Robot.liftArm.setLiftArm(-0.80); + Robot.liftArm.setLiftArm(-RobotMap.armSpeed); stowWrist(); } else { Robot.liftArm.stopLiftArm(); diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java index c586058..9d79eec 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/StowPosition.java @@ -15,15 +15,20 @@ public StowPosition() { requires(Robot.liftArm); } private boolean isFinished; + + public void finish() { + isFinished = true; + } + protected void initialize() {} protected void execute() { Robot.liftArm.disableWristBrake(); isFinished = false; Robot.liftArm.setGrabber(true); - if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 50) { + if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 15) { Robot.liftArm.setWrist(0.75); - } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 50) { + } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 15) { Robot.liftArm.setWrist(-0.75); } else { isFinished = true; diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java index 4be8e97..08c5483 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchPositionArm.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class SwitchPositionArm extends Command { public SwitchPositionArm() { @@ -17,22 +18,11 @@ public SwitchPositionArm() { private boolean isFinished; - private void stowWrist() { - Robot.liftArm.setGrabber(true); - - if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 50) { - Robot.liftArm.setWrist(0.50); - } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 50) { - Robot.liftArm.setWrist(-0.50); - } else { - Robot.liftArm.stopWrist(); - } - } private void grabWrist() { - if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitch + 50) { + if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitch + 15) { Robot.liftArm.setWrist(0.50); - } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitch - 50) { + } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitch - 15) { Robot.liftArm.setWrist(-0.50); } else { Robot.liftArm.stopWrist(); @@ -47,11 +37,9 @@ protected void execute() { Robot.liftArm.setGrabber(true); if(Robot.liftArm.getArmAngle()> Robot.position.armSwitch + 40) { - Robot.liftArm.setLiftArm(0.90); - stowWrist(); + Robot.liftArm.setLiftArm(RobotMap.armSpeed); } else if(Robot.liftArm.getArmAngle()< Robot.position.armSwitch - 40) { - Robot.liftArm.setLiftArm(-0.90); - stowWrist(); + Robot.liftArm.setLiftArm(-RobotMap.armSpeed); } else { Robot.liftArm.stopLiftArm(); grabWrist(); diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java index 95e1c73..2533a4e 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/SwitchShootPositionArm.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; public class SwitchShootPositionArm extends Command { @@ -18,22 +19,11 @@ public SwitchShootPositionArm() { private boolean isFinished; - private void stowWrist() { - Robot.liftArm.setGrabber(true); - - if(Robot.liftArm.getWristAngle() > Robot.position.wristStow + 50) { - Robot.liftArm.setWrist(0.50); - } else if(Robot.liftArm.getWristAngle() < Robot.position.wristStow - 50) { - Robot.liftArm.setWrist(-0.50); - } else { - Robot.liftArm.stopWrist(); - } - } private void grabWrist() { - if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitchShoot + 50) { + if(Robot.liftArm.getWristAngle() > Robot.position.wristSwitchShoot + 15) { Robot.liftArm.setWrist(0.50); - } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitchShoot - 50) { + } else if(Robot.liftArm.getWristAngle() < Robot.position.wristSwitchShoot - 15) { Robot.liftArm.setWrist(-0.50); } else { Robot.liftArm.stopWrist(); @@ -42,17 +32,16 @@ private void grabWrist() { } protected void initialize() {} protected void execute() { + Robot.liftArm.winchHighGear(); Robot.liftArm.disableWristBrake(); isFinished = false; Robot.liftArm.setGrabber(true); if(Robot.liftArm.getArmAngle()> Robot.position.armSwitchShoot + 40) { - Robot.liftArm.setLiftArm(0.70); - stowWrist(); + Robot.liftArm.setLiftArm(RobotMap.armSpeed); } else if(Robot.liftArm.getArmAngle()< Robot.position.armSwitchShoot - 40) { - Robot.liftArm.setLiftArm(-0.70); - stowWrist(); + Robot.liftArm.setLiftArm(-RobotMap.armSpeed); } else { Robot.liftArm.stopLiftArm(); grabWrist(); diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java index 521c155..162acd7 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/TeleopDrive.java @@ -19,26 +19,23 @@ public TeleopDrive() { requires(Robot.driveTrain); } protected void initialize() { + Robot.driveTrain.stopPID(); + //Robot.driveTrain.setLowGear(); + Robot.driveTrain.setCoastMode(); + } protected void execute() { - SmartDashboard.putBoolean("Reversed", Robot.driveTrain.getReverse()); - SmartDashboard.putNumber("Arm",Robot.liftArm.getArmAngle()); - SmartDashboard.putNumber("Wrist", Robot.liftArm.getWristAngle()); - SmartDashboard.putBoolean("Arm Down", RobotMap.armSensor.get()); - SmartDashboard.putNumber("Drive Train", Robot.driveTrain.getRightDistance()); - - if(Robot.liftArm.getCubeSensor() && !Robot.position.isReleasePressed) { Robot.liftArm.setGrabber(true); } - + Robot.shuffleboard.updateDashboard(); double deadConst = .10; - double fBInput = checkDeadZone(OI.drive_FB.getY(), deadConst); - double tInput = -1 * checkDeadZone(OI.drive_T.getX(), deadConst); + double fBInput = 2 * checkDeadZone(OI.drive_FB.getY(), deadConst); + double tInput = -1 * 2 * checkDeadZone(OI.drive_T.getX(), deadConst); Robot.driveTrain.teleopControl(fBInput, tInput); - Robot.driveTrain.autoGear(OI.drive_FB.getY(),deadConst,Robot.driveTrain.getRightDistance(),7); + Robot.driveTrain.autoGear(OI.drive_FB.getY(),deadConst,Robot.driveTrain.getRightDistance(),3); } protected boolean isFinished() { diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java index 72d7fef..cb661df 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchDown.java @@ -13,7 +13,7 @@ public class WinchDown extends Command { protected void execute() { Robot.liftArm.winchLowGear(); - Robot.liftArm.setLiftArm(-1); + Robot.liftArm.setLiftArm(1); } protected boolean isFinished() { return true; diff --git a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java index 0e86550..a617396 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/commands/WinchUp.java @@ -13,7 +13,7 @@ public class WinchUp extends Command { protected void execute() { Robot.liftArm.winchLowGear(); - Robot.liftArm.setLiftArm(1); + Robot.liftArm.setLiftArm(-1); } protected boolean isFinished() { return true; diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java index 219909a..3cd6fcb 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/DriveTrain.java @@ -6,14 +6,18 @@ * @license BSD-3-Clause */ package org.usfirst.frc.team1701.robot.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.command.PIDCommand; +import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.command.Subsystem; -import org.usfirst.frc.team1701.robot.Robot; import org.usfirst.frc.team1701.robot.RobotMap; import org.usfirst.frc.team1701.robot.commands.TeleopDrive; import com.kauailabs.navx.frc.AHRS; -public class DriveTrain extends Subsystem { +public class DriveTrain extends PIDSubsystem { /* * Set of motors. */ @@ -31,13 +35,14 @@ public class DriveTrain extends Subsystem { */ private final DoubleSolenoid driveShift = RobotMap.driveShift; /* - * NavX. + * NavX. */ private final AHRS navx = RobotMap._navx; /* * Special math stuffs. */ private final int encPidIdx = RobotMap.encPidIdx; + private final int rawToRotation = 22000; /* @@ -46,13 +51,41 @@ public class DriveTrain extends Subsystem { private boolean reversed = false; private boolean deadStick = false; public boolean autoGear = false; + public double driveSpeed = 0; + + public DriveTrain() { + super(1.1,0,0); + this.setInputRange(-360,360); + this.setOutputRange(-0.9,0.9); + this.setAbsoluteTolerance(.5); + this.getPIDController().setContinuous(true); + } + + /** + * Sets angle for robot to turn to using PIDSubsystem + * @param startAngle angle to turn to + */ + public void setAngle(double startAngle) { + this.setSetpoint(startAngle); + } + + public void startPID() { + this.getPIDController().enable(); + } + + public void stopPID() { + this.getPIDController().disable(); + } + + + /** * Return left distance. * @return double of left side distance. */ public double getLeftDistance() { - return -leftEncTalon.getSelectedSensorPosition(encPidIdx) / 22000; + return -leftEncTalon.getSelectedSensorPosition(encPidIdx) / rawToRotation; } /** * Reset left side encoder. @@ -75,7 +108,7 @@ public void stopMotors() * @return double of right side distance. */ public double getRightDistance() { - return rightEncTalon.getSelectedSensorPosition(encPidIdx) / 22000; + return rightEncTalon.getSelectedSensorPosition(encPidIdx) / rawToRotation; } /** * Reset right side encoder. @@ -114,7 +147,6 @@ public double getNavxAngle() { return -navx.getAngle(); } - /** * Checks current reverse boolean * @return State of reversed @@ -122,12 +154,6 @@ public double getNavxAngle() public boolean getReverse() { return reversed; } - /** - * Set status of reverse mode. - */ - public void toggleReverse() { - this.reversed = !this.reversed; - } /** * Initialize teleoperated control. * @param forwardsBackwardsAxis See DifferentialDrive.arcadeDrive. @@ -168,17 +194,29 @@ public void setLowGear() { driveShift.set(DoubleSolenoid.Value.kReverse); } + /** + * Sets the drive train's thrust to move in the opposite direction + * @param reversedValue Enables/Disables reversed status + */ public void setReverse(boolean reversedValue) { this.reversed = reversedValue; } /** - * Set state of autoGear - * @param value Set Boolean + * Enables/Disables auto gear. This is helpful for a manual switch where you can turn auto gear on and off + * @param value Sets state of auto gear */ public void setAutoGear(boolean value) { this.autoGear = value; } + + /** + * Automatically shifts the gear after a set distance from when the drive stick last went to dead stick + * @param tInput Forward/Backwards Speed from Joystick + * @param deadConst The value that sets where and when dead stick should enable. BY DEFAULT: 0.1 or -10 % to 10% + * @param encoderValue The raw input that will deliver the current distance + * @param distanceTrigger The value that determines the distance that high gear is set + */ public void autoGear(double tInput, double deadConst, double encoderValue, double distanceTrigger) { if(autoGear) { @@ -201,15 +239,44 @@ public void autoGear(double tInput, double deadConst, double encoderValue, doubl } } - if(encoderValue > distanceTrigger) { + if(Math.abs(encoderValue) > distanceTrigger) { setHighGear(); } - } } + /** + * Checks current deadStick boolean that is determine by autoGear() + * @return Checks weather the thrust joystick is moving or not + */ public boolean getDeadStick() { return this.deadStick; } + + + + @Override + protected double returnPIDInput() { + return -navx.getAngle(); + } + + @Override + protected void usePIDOutput(double output) { + teleopControl(driveSpeed,output); + } + + public void setBrakeMode() { + left_1.setNeutralMode(NeutralMode.Brake); + left_2.setNeutralMode(NeutralMode.Brake); + right_1.setNeutralMode(NeutralMode.Brake); + right_2.setNeutralMode(NeutralMode.Brake); + } + + public void setCoastMode() { + left_1.setNeutralMode(NeutralMode.Coast); + left_2.setNeutralMode(NeutralMode.Coast); + right_1.setNeutralMode(NeutralMode.Coast); + right_2.setNeutralMode(NeutralMode.Coast); + } } diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java index e713fef..29c7571 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/LiftArm.java @@ -83,7 +83,8 @@ public double getArmAngle() { * @return wrist angle as double. */ public double getWristAngle() { - return wristEncoder.getSelectedSensorPosition(encPidIdx); + //return wristEncoder.getSelectedSensorPosition(encPidIdx); + return RobotMap.wristEncoder.getValue(); } /** * Set the clamp position. @@ -103,8 +104,8 @@ public void setGrabber(boolean State) { */ public void setLiftArm(double input) { disableWinchBrake(); - winch1.set(input); - winch2.set(input); + winch1.set(checkArmZone(input)); + winch2.set(checkArmZone(input)); } /** @@ -153,11 +154,7 @@ public boolean getCubeSensor() return RobotMap.cubeSensor.get(); } public double checkArmZone(double speed) { - if(Robot.position.armSafetyMin >= Robot.liftArm.getArmAngle()) { - return 0; - } - - if(Robot.position.armSafetyMax <= Robot.liftArm.getArmAngle()) { + if(RobotMap.armSensor.get() && speed > 0) { return 0; } return speed; diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java index 8eafb41..6253b64 100644 --- a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java +++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Position.java @@ -4,51 +4,31 @@ public class Position extends Subsystem { - /* - * Minimum and maximum traveling distance for wrist and arm - */ - public double armSafetyMin = 98; - public double armSafetyMax = 2202; - public double wristSafetyMin = 0; - public double wristSafetyMax = 0; - /* * Wrist values for positions */ - public double wristStow = -201; - public double wristGrab = 1119; - public double wristScale = 911; - public double wristClimb = 1289; - public double wristSwitch = -1512; - public double wristSwitchShoot = 461; + public double wristStow = -44; + public double wristGrab = 1980; + public double wristScale = 1958; + public double wristClimb = 2007; + public double wristSwitch = 1723; + public double wristSwitchShoot = 1911; + public double startPosition = 1868; /* * Arm value for positions */ - public double armGrab = 355; - public double armScale = 2400; - public double armClimb = 2396; - public double armSwitch = 1373; - public double armSwitchShoot = 563; + public double armGrab = 48; + public double armScale = 1994; + public double armClimb = 1645; + public double armSwitch = 1185; + public double armSwitchShoot = 224; /* * Values for autonomous */ - public double wallToCubeRight = 0.1; - public double wallToCubeLeft = 0.1; - public double centerToSide = 5.00; - public double turnToSwitch = 4.2; - public double turnToSwitchRight = 4.4; - public double wallToScale = 22.75; - public double wallToPlatformZone = 15.69; - public double scaleReverse = 0.31; + public double autonomousSpeed = 0.90; - public double crossPlatformZone = 13.93; - public double platformToScale = 2.1; - public double leftAngle = 81; - public double rightAngle = 81; - public double slightLeftAngle = 45; - public double slightRightAngle = 30; public double wallToStraightSwitch = 7; public boolean isReleasePressed = false; diff --git a/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Shuffleboard.java b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Shuffleboard.java new file mode 100644 index 0000000..9feb692 --- /dev/null +++ b/src/main/java/org/usfirst/frc/team1701/robot/subsystems/Shuffleboard.java @@ -0,0 +1,76 @@ +/** + * Shuffleboard.java + * + * @author Noah Husby + * @since 3/8/18 + * @license BSD-3-Clause + */ +package org.usfirst.frc.team1701.robot.subsystems; + +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.usfirst.frc.team1701.robot.Robot; +import org.usfirst.frc.team1701.robot.RobotMap; + +public class Shuffleboard { + + public static SendableChooser<Number> autonomousLocation; + public static SendableChooser<Number> action; + + /** + * Creates our shuffleboard objects + */ + public static void init() { + /* + * Initialize Autonomous Location Chooser; + * Allows operator/technician to select robot start position before match + */ + autonomousLocation = new SendableChooser<>(); + autonomousLocation.addObject("Left",1); + autonomousLocation.addObject("Left-Switch",4); + autonomousLocation.addDefault("Middle",2); + autonomousLocation.addObject("Right-Switch",5); + autonomousLocation.addObject("Right",3); + autonomousLocation.addObject("Magic",6); + SmartDashboard.putData("Autonomous Location", autonomousLocation); + /* + * Initialize Autonomous Chooser + * Allows operator/technician to select type of autonomous to be run + */ + action = new SendableChooser<>(); + action.addDefault("Defualt Autonomous", 1); + action.addObject("Forward Autonomous", 2); + SmartDashboard.putData("Autonomous Chooser",action); + + /* + * Shuffleboard elements to be updated during teleop period + */ + SmartDashboard.putBoolean("Reversed", false); + SmartDashboard.putString("Current Gear",""); + SmartDashboard.putNumber("Arm",0); + SmartDashboard.putBoolean("Arm Trigger", false); + SmartDashboard.putNumber("Drive Train", 0); + SmartDashboard.putString("GameCode", ""); + SmartDashboard.putNumber("NewEnc",0); + + } + + /** + * Method called during teleop to update shuffleboard elements live + */ + public static void updateDashboard() { + SmartDashboard.putBoolean("Reversed", Robot.driveTrain.getReverse()); + SmartDashboard.putNumber("Arm",Robot.liftArm.getArmAngle()); + SmartDashboard.putNumber("Wrist", Robot.liftArm.getWristAngle()); + SmartDashboard.putBoolean("Arm Trigger", RobotMap.armSensor.get()); + SmartDashboard.putNumber("Drive Train", Robot.driveTrain.getRightDistance()); + SmartDashboard.putNumber("NewEnc",RobotMap.wristEncoder.getValue()); + + } + + + + + +}