diff --git a/build/org/usfirst/frc/team3695/robot/Constants$AutonomousConstants.class b/build/org/usfirst/frc/team3695/robot/Constants$AutonomousConstants.class index cdbdeb4..1ac48e1 100644 Binary files a/build/org/usfirst/frc/team3695/robot/Constants$AutonomousConstants.class and b/build/org/usfirst/frc/team3695/robot/Constants$AutonomousConstants.class differ diff --git a/build/org/usfirst/frc/team3695/robot/Constants$TEUFELSKIND.class b/build/org/usfirst/frc/team3695/robot/Constants$TEUFELSKIND.class index 715a27e..e93e793 100644 Binary files a/build/org/usfirst/frc/team3695/robot/Constants$TEUFELSKIND.class and b/build/org/usfirst/frc/team3695/robot/Constants$TEUFELSKIND.class differ diff --git a/build/org/usfirst/frc/team3695/robot/Constants.class b/build/org/usfirst/frc/team3695/robot/Constants.class index 30208ec..2e1d7b6 100644 Binary files a/build/org/usfirst/frc/team3695/robot/Constants.class and b/build/org/usfirst/frc/team3695/robot/Constants.class differ diff --git a/build/org/usfirst/frc/team3695/robot/OI.class b/build/org/usfirst/frc/team3695/robot/OI.class index 2b3cfde..920f0d5 100644 Binary files a/build/org/usfirst/frc/team3695/robot/OI.class and b/build/org/usfirst/frc/team3695/robot/OI.class differ diff --git a/build/org/usfirst/frc/team3695/robot/Robot.class b/build/org/usfirst/frc/team3695/robot/Robot.class index 968ed73..b50b6ef 100644 Binary files a/build/org/usfirst/frc/team3695/robot/Robot.class and b/build/org/usfirst/frc/team3695/robot/Robot.class differ diff --git a/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto$1.class b/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto$1.class index 289a2b2..6017b63 100644 Binary files a/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto$1.class and b/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto$1.class differ diff --git a/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.class b/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.class index e36b0cb..b84e8c0 100644 Binary files a/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.class and b/build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.class b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.class index 8ed246b..65effe4 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.class and b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.class b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.class index c8a6df5..4b39418 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.class and b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.class b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.class index ab96e12..1e21f0f 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.class and b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.class b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.class index 6887c77..67d3408 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.class and b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.class b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.class index cbee9ab..5046d44 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.class and b/build/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.class b/build/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.class index 4a182bd..02e20b1 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.class and b/build/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.class b/build/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.class index 32ac741..6febec7 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.class and b/build/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.class differ diff --git a/build/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.class b/build/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.class index 425cf8e..6742f0a 100644 Binary files a/build/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.class and b/build/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.class differ diff --git a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemClamp.class b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemClamp.class index 6c4bae4..0de4430 100644 Binary files a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemClamp.class and b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemClamp.class differ diff --git a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$1.class b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$1.class deleted file mode 100644 index 4bc11ec..0000000 Binary files a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$1.class and /dev/null differ diff --git a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PID.class b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PID.class deleted file mode 100644 index 4341806..0000000 Binary files a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PID.class and /dev/null differ diff --git a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PIDF.class b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PIDF.class new file mode 100644 index 0000000..0404efa Binary files /dev/null and b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PIDF.class differ diff --git a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.class b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.class index 7d1c948..af86a0e 100644 Binary files a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.class and b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.class differ diff --git a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.class b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.class index 0590dcc..e7b7be2 100644 Binary files a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.class and b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.class differ diff --git a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.class b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.class index 70f4b8a..f8e26f8 100644 Binary files a/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.class and b/build/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.class differ diff --git a/dist/FRCUserProgram.jar b/dist/FRCUserProgram.jar index 345e805..64fec62 100644 Binary files a/dist/FRCUserProgram.jar and b/dist/FRCUserProgram.jar differ diff --git a/src/org/usfirst/frc/team3695/robot/Constants.java b/src/org/usfirst/frc/team3695/robot/Constants.java index c68765f..426148e 100644 --- a/src/org/usfirst/frc/team3695/robot/Constants.java +++ b/src/org/usfirst/frc/team3695/robot/Constants.java @@ -38,9 +38,9 @@ public class Constants { RIGHT_PINION_MOTOR = 4, SCREW_MOTOR = 5, /// PID - RIGHT_PID = 0, + PIDF_LOOP_ID = 0, LEFT_PID = 0, - TIMEOUT_PID = 10000; + PIDF_TIMEOUT = 10000; public static final int /// I2C @@ -53,8 +53,8 @@ public class Constants { public static final int /// MANIPULATOR - OPEN_ARMS = 5, - CLOSE_ARMS = 4, + OPEN_ARMS = 4, + CLOSE_ARMS = 5, /// CANDY CANE RAISE_HOOK = 6, LOWER_HOOK = 7; @@ -89,8 +89,8 @@ public static class OOF { public static class TEUFELSKIND { public static final boolean - LEFT_PINION_MOTOR_INVERT = false, - RIGHT_PINION_MOTOR_INVERT = true, + LEFT_PINION_MOTOR_INVERT = true, + RIGHT_PINION_MOTOR_INVERT = false, SCREW_MOTOR_INVERT = false, LEFT_ARM_MOTOR_INVERT = true, @@ -118,9 +118,9 @@ public static class AutonomousConstants { //Save distances in inches //Distance from the side positions to the middle of the switch on the side - public static final int DIST_TO_SWITCH = 148; + public static final int DIST_TO_SWITCH = 150+5; //Distance rom the side positions to the middle of the scale on the side - public static final int DIST_TO_SCALE = 324; + public static final int DIST_TO_SCALE = (260); //Distance from a side to the enemy switch public static final int DIST_TO_ENEMY_SWITCH = 480; @@ -137,9 +137,9 @@ public static class AutonomousConstants { public static final int DIST_ALLIANCE_WALL_TO_BLOCKS = 98; // the distance to pass the switch before traveling to the foreign home switch - public static final double DIST_PAST_SWITCH = 238.735; + public static final double DIST_PAST_SWITCH = 220.735+2; - public static final double DIST_TO_FOREIGN_SWITCH = 178.75; + public static final double DIST_TO_FOREIGN_SWITCH = 163.75+5; public static final double DIST_PAST_SCALE = 240; public static final double DIST_SCALE_LINEUP = 96; diff --git a/src/org/usfirst/frc/team3695/robot/OI.java b/src/org/usfirst/frc/team3695/robot/OI.java index 80ff026..fc93a77 100644 --- a/src/org/usfirst/frc/team3695/robot/OI.java +++ b/src/org/usfirst/frc/team3695/robot/OI.java @@ -62,7 +62,7 @@ public OI() { /// Cyborg command testers SmartDashboard.putData("Drive Direct", new CyborgCommandDriveDirect(Util.getAndSetDouble("Drive Direct Power", 0))); SmartDashboard.putData("Drive Distance", new CyborgCommandDriveDistance(Util.getAndSetDouble("Drive Distance Inches", 0), (int) Util.getAndSetDouble("Drive Distance Timeout", 5000))); - SmartDashboard.putData("Drive Until Error", new CyborgCommandDriveUntilError(500,2)); + SmartDashboard.putData("Drive Until Error", new CyborgCommandDriveUntilError(500,2,.25, 3)); SmartDashboard.putData("Rotate Degree", new CyborgCommandRotateDegrees(Util.getAndSetDouble("Rotate Degrees", 0), (int) Util.getAndSetDouble("Rotate Timeout", 5000))); } diff --git a/src/org/usfirst/frc/team3695/robot/Robot.java b/src/org/usfirst/frc/team3695/robot/Robot.java index 20b0ff1..05ec0e2 100644 --- a/src/org/usfirst/frc/team3695/robot/Robot.java +++ b/src/org/usfirst/frc/team3695/robot/Robot.java @@ -1,4 +1,3 @@ - package org.usfirst.frc.team3695.robot; import edu.wpi.first.wpilibj.DriverStation; @@ -72,7 +71,7 @@ public void robotInit() { SUB_CLAMP = new SubsystemClamp(); SUB_COMPRESSOR = new SubsystemCompressor(); SUB_DRIVE = new SubsystemDrive(); - + SUB_DRIVE.setInverts(); SUB_HOOK = new SubsystemHook(); SUB_MAST = new SubsystemMast(); vision = new Vision(); @@ -106,8 +105,8 @@ public void robotInit() { /// instantiate third priority chooser thirdPriorityChooser = new SendableChooser<>(); thirdPriorityChooser.addDefault(Goal.SWITCH.toString(), Goal.SWITCH); - goalChooser.addObject(Goal.SCALE.toString(), Goal.SCALE); - goalChooser.addObject(Goal.ENEMY_SWITCH.toString(), Goal.ENEMY_SWITCH); + thirdPriorityChooser.addObject(Goal.SCALE.toString(), Goal.SCALE); + thirdPriorityChooser.addObject(Goal.ENEMY_SWITCH.toString(), Goal.ENEMY_SWITCH); SmartDashboard.putData("Third Priority", thirdPriorityChooser); /// instantiate bot chooser @@ -187,4 +186,3 @@ public void testPeriodic() { LiveWindow.run(); } } - diff --git a/src/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.java b/src/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.java index 5fe566a..d46a1b0 100644 --- a/src/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.java +++ b/src/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.java @@ -33,7 +33,6 @@ public CommandGroupAuto(Position position, Goal goal, Goal thirdPriority) { //Get the state of the switches and scale for each round gameData = DriverStation.getInstance().getGameSpecificMessage(); - // make sure everything is in the right state/position up here Robot.SUB_CLAMP.closeArms(); Robot.SUB_DRIVE.setInverts(); @@ -99,89 +98,95 @@ public CommandGroupAuto(Position position, Goal goal, Goal thirdPriority) { } private void runForIt() { - addSequential(new CyborgCommandDriveUntilError(500, 2)); + addSequential(new CyborgCommandDriveDistance(160, 5000)); } private void leftSwitch() { if (gameData.charAt(0) == 'L') { //When the switch is on the left - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 3500)); - addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250)); + addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 2000)); + addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 5000)); +// addSequential(new CyborgCommandDriveDistance(46, 2000)); + addSequential(new CommandWait(350)); + addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 8500)); addSequential(new CommandWait(250)); - addSequential(new CyborgCommandDriveUntilError(500, 2)); + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3)); addSequential(new CyborgCommandSpit(500)); } else if (gameData.charAt(0) == 'R') { //When the switch is on the right + //CHECK TUNING!!!!!!! addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250)); + addSequential(new CommandWait(350)); + addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 5000)); addSequential(new CommandWait(250)); addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_FOREIGN_SWITCH, 4000)); addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500)); - addSequential(new CommandWait(250)); + addSequential(new CommandWait(350)); addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250)); addSequential(new CommandWait(250)); - addSequential(new CyborgCommandDriveUntilError(500, 2)); + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5,3)); addSequential(new CyborgCommandSpit(500)); - } + } } // TODO right switch terminates early private void rightSwitch() { - if (gameData.charAt(0) == 'R') { //When the switch is on the right - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 3500)); - addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250), 1500); + if (gameData.charAt(0) == 'R') { //When the switch is on the left + addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 2000)); + addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 5000)); +// addSequential(new CyborgCommandDriveDistance(46, 2000)); + addSequential(new CommandWait(500)); + addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 8500)); addSequential(new CommandWait(250)); - addSequential(new CyborgCommandDriveUntilError(500, 2)); - addSequential(new CyborgCommandSpit(500L)); - } else { //When the switch is on the left - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_FOREIGN_SWITCH, 4000)); - addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandDriveUntilError(500, 2)); + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3)); addSequential(new CyborgCommandSpit(500)); + } + else { + addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 5000)); +// addSequential(new CyborgCommandDriveDistance(46, 2000)); } +// else if (gameData.charAt(0) == 'L') { //When the switch is on the right +// //CHECK TUNING!!!!!!! +// addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000)); +// addSequential(new CommandWait(500)); +// addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 5000)); +// addSequential(new CommandWait(250)); +// addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_FOREIGN_SWITCH, 4000)); +// addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500)); +// addSequential(new CommandWait(350)); +// addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250)); +// addSequential(new CommandWait(250)); +// addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 4)); +// addSequential(new CyborgCommandSpit(500)); +// } } private void centerSwitch() { + addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 1500)); addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PASS_PORTAL, 5000)); if (gameData.charAt(0) == 'L') { //When the switch is on the left addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 5000)); addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_CENTER_LINE_SWITCH_ALIGN, 5000)); addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 5000)); - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_ALLIANCE_WALL_TO_BLOCKS - + AutonomousConstants.DIST_BLOCKS_TO_SWITCH - - AutonomousConstants.DIST_PASS_PORTAL, 5000)); - - } else if (gameData.charAt(0) == 'L') { //When the switch is on the right + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3)); + } else if (gameData.charAt(0) == 'R') { //When the switch is on the right addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 5000)); addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_CENTER_LINE_SWITCH_ALIGN, 5000)); addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 5000)); - // addParallel(new CyborgCommandGoToMid()); - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_ALLIANCE_WALL_TO_BLOCKS - + AutonomousConstants.DIST_BLOCKS_TO_SWITCH - - AutonomousConstants.DIST_PASS_PORTAL, 5000)); + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3)); } else { - addSequential(new CyborgCommandDriveUntilError(500, 2)); + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.3, 3)); } + addSequential(new CyborgCommandSpit(500)); } private void leftScale() { if (gameData.charAt(1) == 'L') { //When scale is on the left addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 3000)); - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 4000)); - addSequential(new CommandWait(250)); + addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 6000)); + addSequential(new CommandWait(250)); + addSequential(new CyborgCommandRotateDegrees(45, 4000)); addSequential(new CyborgCommandGrow(Mast.PINION_UP, 3500)); - addSequential(new CommandWait(250)); - addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250)); + addSequential(new CyborgCommandDriveDistance(15, 3000)); addSequential(new CyborgCommandSpit(500)); + addSequential(new CyborgCommandDriveUntilError(500, 500000, -0.15, 3)); } else { //When scale is on the right addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000)); addSequential(new CommandWait(250)); @@ -197,7 +202,7 @@ private void leftScale() { addSequential(new CommandWait(250)); addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250)); addSequential(new CommandWait(250)); - addSequential(new CyborgCommandDriveUntilError(500, 2)); + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.15, 3)); addSequential(new CyborgCommandSpit(500)); } } @@ -205,11 +210,11 @@ private void leftScale() { private void rightScale() { if (gameData.charAt(1) == 'R') { //When scale is on the right addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 3000)); - addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 4000)); + addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 7000)); addSequential(new CommandWait(250)); addSequential(new CyborgCommandGrow(Mast.PINION_UP, 3500)); addSequential(new CommandWait(250)); - addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250)); + addSequential(new CyborgCommandRotateDegrees(45, 1250)); addSequential(new CyborgCommandSpit(500)); } else { //When scale is on the left addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000)); @@ -226,7 +231,7 @@ private void rightScale() { addSequential(new CommandWait(250)); addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250)); addSequential(new CommandWait(250)); - addSequential(new CyborgCommandDriveUntilError(500, 2)); + addSequential(new CyborgCommandDriveUntilError(500, 2, 0.15, 3)); addSequential(new CyborgCommandSpit(500)); } } diff --git a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.java b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.java index be9549c..b8732df 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.java +++ b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.java @@ -20,7 +20,7 @@ public CyborgCommandDriveDirect(double percent) { protected void initialize() { DriverStation.reportWarning("DRIVING BY POWER", false); - Robot.SUB_DRIVE.pid.reset(); + Robot.SUB_DRIVE.pidf.reset(); time = System.currentTimeMillis() + TIME_WAIT; } diff --git a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.java b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.java index 2f2bfe7..a54e80b 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.java +++ b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.java @@ -6,14 +6,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team3695.robot.Robot; -import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PID; +import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PIDF; import org.usfirst.frc.team3695.robot.util.Util; public class CyborgCommandDriveDistance extends Command { public static final long TIME_WAIT = 3000; public double inches; - public static final int ALLOWABLE_ERROR = 1; + public static final int ALLOWABLE_ERROR = 2; Boolean isFinished; long runTime; @@ -22,18 +22,14 @@ public class CyborgCommandDriveDistance extends Command { public CyborgCommandDriveDistance(double inches, int timeoutms) { this.inches = inches; requires(Robot.SUB_DRIVE); - Robot.SUB_DRIVE.pid.reset(); + Robot.SUB_DRIVE.pidf.reset(); runTime = timeoutms; } protected void initialize() { - Robot.SUB_DRIVE.pid.reset(); + Robot.SUB_DRIVE.pidf.reset(); // inches = Util.getAndSetDouble("Drive Distance Inches", 10); // take out in final version - PID.setPIDF(0, - Util.getAndSetDouble("Distance-P", .5), - Util.getAndSetDouble("Distance-I", 0), - Util.getAndSetDouble("Distance-D", 0), - Util.getAndSetDouble("Distance-F", 0)); + PIDF.setPIDF(0); Robot.SUB_DRIVE.driveDistance(inches, inches); startTime = System.currentTimeMillis(); } @@ -41,27 +37,26 @@ protected void initialize() { protected void execute() { DriverStation.reportWarning("DRIVING " + inches + " INCHES", false); - SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches()); - SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches()); - - - SmartDashboard.putNumber("Error", Robot.SUB_DRIVE.pid.getError()); + SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pidf.getLeftInches()); + SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pidf.getRightInches()); + Robot.SUB_DRIVE.pidf.getError(); } protected boolean isFinished() { isFinished = startTime + runTime < System.currentTimeMillis(); boolean leftInRange = - Robot.SUB_DRIVE.pid.getLeftInches() > (inches) - ALLOWABLE_ERROR && - Robot.SUB_DRIVE.pid.getLeftInches() < (inches) + ALLOWABLE_ERROR; + Robot.SUB_DRIVE.pidf.getLeftInches() > (inches) - ALLOWABLE_ERROR && + Robot.SUB_DRIVE.pidf.getLeftInches() < (inches) + ALLOWABLE_ERROR; boolean rightInRange = - Robot.SUB_DRIVE.pid.getRightInches() > inches - ALLOWABLE_ERROR && - Robot.SUB_DRIVE.pid.getRightInches() < inches + ALLOWABLE_ERROR; + Robot.SUB_DRIVE.pidf.getRightInches() > inches - ALLOWABLE_ERROR && + Robot.SUB_DRIVE.pidf.getRightInches() < inches + ALLOWABLE_ERROR; return (leftInRange && rightInRange) || isFinished; } protected void end() { DriverStation.reportWarning("CyborgCommandDriveDistance finished", false); Robot.SUB_DRIVE.driveDirect(0, 0); + DriverStation.reportWarning("LEFT:" + Robot.SUB_DRIVE.pidf.getLeftInches() + ", " + "RIGHT:" + Robot.SUB_DRIVE.pidf.getRightInches(), false); isFinished = false; } diff --git a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.java b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.java index fd137f5..64ce80c 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.java +++ b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.java @@ -6,8 +6,10 @@ import org.usfirst.frc.team3695.robot.util.Util; public class CyborgCommandDriveUntilError extends Command { + long masterError; public long errorTime; public long runTime; + public long startTime; double allowableError = 2; @@ -15,14 +17,18 @@ public class CyborgCommandDriveUntilError extends Command { double currentPosRight; private long time = 0; - - public CyborgCommandDriveUntilError(long errorTime, double allowableError) { + + double speed; + public CyborgCommandDriveUntilError(long errorTime, double allowableError, double speed, long masterError) { this.errorTime = errorTime; this.allowableError = allowableError; + this.masterError = masterError; runTime = System.currentTimeMillis(); + startTime = System.currentTimeMillis(); requires(Robot.SUB_DRIVE); - currentPosLeft = Robot.SUB_DRIVE.pid.getLeftInches(); - currentPosRight = Robot.SUB_DRIVE.pid.getRightInches(); + currentPosLeft = Robot.SUB_DRIVE.pidf.getLeftInches(); + currentPosRight = Robot.SUB_DRIVE.pidf.getRightInches(); + this.speed = speed; } protected void initialize() { @@ -30,19 +36,20 @@ protected void initialize() { } protected void execute() { - double speed = Util.getAndSetDouble("SPEED ERROR: Forward", -0.25); +// double speed = Util.getAndSetDouble("SPEED ERROR: Forward", -0.25); Robot.SUB_DRIVE.driveDirect(speed, speed); } protected boolean isFinished() { - if (!((currentPosLeft + allowableError) > Robot.SUB_DRIVE.pid.getLeftInches() && (currentPosLeft - allowableError) < Robot.SUB_DRIVE.pid.getLeftInches()) - || !((currentPosRight + allowableError) > Robot.SUB_DRIVE.pid.getRightInches() && (currentPosRight - allowableError) < Robot.SUB_DRIVE.pid.getRightInches())){ - currentPosLeft = Robot.SUB_DRIVE.pid.getLeftInches(); - currentPosRight = Robot.SUB_DRIVE.pid.getRightInches(); +// if (masterError + startTime < System.currentTimeMillis()) return true; + if (!((currentPosLeft + allowableError) > Robot.SUB_DRIVE.pidf.getLeftInches() && (currentPosLeft - allowableError) < Robot.SUB_DRIVE.pidf.getLeftInches()) + || !((currentPosRight + allowableError) > Robot.SUB_DRIVE.pidf.getRightInches() && (currentPosRight - allowableError) < Robot.SUB_DRIVE.pidf.getRightInches())){ + currentPosLeft = Robot.SUB_DRIVE.pidf.getLeftInches(); + currentPosRight = Robot.SUB_DRIVE.pidf.getRightInches(); runTime = System.currentTimeMillis(); return false; } - return errorTime + runTime < System.currentTimeMillis(); + return runTime + errorTime < System.currentTimeMillis(); } protected void end() { diff --git a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.java b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.java index 77b06a3..d4b7678 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.java +++ b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.java @@ -37,30 +37,30 @@ protected void execute() { switch (position) { case PINION_UP: if (Robot.SUB_MAST.upperPinionLimit.get()) { - Robot.SUB_MAST.rightPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.rightPinionate(-1)); - Robot.SUB_MAST.leftPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.leftPinionate(-1)); + Robot.SUB_MAST.rightPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.rightPinionate(1)); + Robot.SUB_MAST.leftPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.leftPinionate(1)); } else { isFinished = true; } break; case PINION_DOWN: if (Robot.SUB_MAST.lowerPinionLimit.get()) { - Robot.SUB_MAST.rightPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.rightPinionate(1)); - Robot.SUB_MAST.leftPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.leftPinionate(1)); + Robot.SUB_MAST.rightPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.rightPinionate(-1)); + Robot.SUB_MAST.leftPinion.set(ControlMode.PercentOutput, Robot.SUB_MAST.leftPinionate(-1)); } else { isFinished = true; } break; case SCREW_UP: if (Robot.SUB_MAST.upperScrewLimit.get()){ - Robot.SUB_MAST.screw.set(ControlMode.PercentOutput, Robot.SUB_MAST.screwify(1)); + Robot.SUB_MAST.screw.set(ControlMode.PercentOutput, Robot.SUB_MAST.screwify(-1)); } else { isFinished = true; } break; case SCREW_DOWN: if (Robot.SUB_MAST.lowerScrewLimit.get()){ - Robot.SUB_MAST.screw.set(ControlMode.PercentOutput, Robot.SUB_MAST.screwify(-1)); + Robot.SUB_MAST.screw.set(ControlMode.PercentOutput, Robot.SUB_MAST.screwify(1)); } else { isFinished = true; } diff --git a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.java b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.java index e4eaee7..fce11f6 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.java +++ b/src/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team3695.robot.Constants; import org.usfirst.frc.team3695.robot.Robot; -import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PID; +import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PIDF; import org.usfirst.frc.team3695.robot.util.Util; public class CyborgCommandRotateDegrees extends Command { @@ -22,37 +22,34 @@ public CyborgCommandRotateDegrees(double degrees, long timeout) { inches = degrees * SCALAR; runTime = timeout; requires(Robot.SUB_DRIVE); - Robot.SUB_DRIVE.pid.reset(); + Robot.SUB_DRIVE.pidf.reset(); } protected void initialize() { - Robot.SUB_DRIVE.pid.reset(); - DriverStation.reportWarning("ROTATING " + (inches / SCALAR) + " DEGREES" + ((inches > 0) ? "CW" : "CCW"), false); + Robot.SUB_DRIVE.pidf.reset(); + PIDF.setPIDF(inches > 0 ? 1 : 2); // inches = Util.getAndSetDouble("Rotate Degrees", 0) * SCALAR; // take out in final version - PID.setPIDF(0, - Util.getAndSetDouble("Rotation-P", 0.37), - Util.getAndSetDouble("Rotation-I", 0), - Util.getAndSetDouble("Rotation-D", 0.025), - Util.getAndSetDouble("Rotation-F", 0)); + DriverStation.reportWarning("ROTATING " + (inches / SCALAR) + " DEGREES" + ((inches > 0) ? "CW" : "CCW") + " AND INCHES: " +inches, false); Robot.SUB_DRIVE.driveDistance(inches, -1 * inches); startTime = System.currentTimeMillis(); + DriverStation.reportWarning("Rotate Init Complete", true); } protected void execute() { DriverStation.reportWarning("ROTATING " + (inches / SCALAR) + " DEGREES" + ((inches > 0) ? "CW" : "CCW"), false); - SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches()); - SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches()); - SmartDashboard.putNumber("Error", Robot.SUB_DRIVE.pid.getError()); + SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pidf.getLeftInches()); + SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pidf.getRightInches()); + Robot.SUB_DRIVE.pidf.getError(); } protected boolean isFinished() { isFinished = startTime + runTime < System.currentTimeMillis(); boolean leftInRange = - Robot.SUB_DRIVE.pid.getLeftInches() > (inches) - ALLOWABLE_ERROR && - Robot.SUB_DRIVE.pid.getLeftInches() < (inches) + ALLOWABLE_ERROR; + Robot.SUB_DRIVE.pidf.getLeftInches() > (inches) - ALLOWABLE_ERROR && + Robot.SUB_DRIVE.pidf.getLeftInches() < (inches) + ALLOWABLE_ERROR; boolean rightInRange = - Robot.SUB_DRIVE.pid.getRightInches() > inches - ALLOWABLE_ERROR && - Robot.SUB_DRIVE.pid.getRightInches() < inches + ALLOWABLE_ERROR; + Robot.SUB_DRIVE.pidf.getRightInches() > inches - ALLOWABLE_ERROR && + Robot.SUB_DRIVE.pidf.getRightInches() < inches + ALLOWABLE_ERROR; return (leftInRange && rightInRange) || isFinished; } diff --git a/src/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.java b/src/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.java index ea21c16..294f943 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.java +++ b/src/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.java @@ -26,8 +26,8 @@ protected void execute() { SmartDashboard.putBoolean("Forza Dampener (Docking mode)", Robot.SUB_DRIVE.docking); SmartDashboard.putBoolean("Reversing mode", Robot.SUB_DRIVE.reversing); SmartDashboard.putBoolean("Narrower (Turn inhibitor)", Robot.SUB_DRIVE.narrowing); - SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches()); - SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches()); + SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pidf.getRightInches()); + SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pidf.getLeftInches()); SmartDashboard.putNumber("Match Time", DriverStation.getInstance().getMatchTime()); switch (Robot.SUB_DRIVE.drivetrain) { diff --git a/src/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.java b/src/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.java index b698c78..9e6c0f5 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.java +++ b/src/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.java @@ -16,7 +16,7 @@ public ManualCommandGrow() { protected void initialize() {} protected void execute() { - Robot.SUB_MAST.moveBySpeed(OI.OPERATOR, Util.getAndSetDouble("Screw Inhibitor", 1)); + Robot.SUB_MAST.moveBySpeed(OI.OPERATOR, Util.getAndSetDouble("Screw Inhibitor", -1)); } protected boolean isFinished() { diff --git a/src/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.java b/src/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.java index 8df2986..90de1da 100644 --- a/src/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.java +++ b/src/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.java @@ -2,7 +2,7 @@ import edu.wpi.first.wpilibj.command.Command; import org.usfirst.frc.team3695.robot.Robot; -import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PID; +import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PIDF; import org.usfirst.frc.team3695.robot.util.Util; /** Toggle PID */ @@ -17,20 +17,12 @@ public ToggleCommandKillPID() { protected void initialize() { PID_ENABLED = !PID_ENABLED; + if (PID_ENABLED) { - PID.setPIDF(0, - Util.getAndSetDouble("Distance-P", .5), - Util.getAndSetDouble("Distance-I", 0), - Util.getAndSetDouble("Distance-D", 0), - Util.getAndSetDouble("Distance-F", 0)); - PID.setPIDF(1, - Util.getAndSetDouble("Rotation-P", .5), - Util.getAndSetDouble("Rotation-I", 0), - Util.getAndSetDouble("Rotation-D", 0), - Util.getAndSetDouble("Rotation-F", 0)); + PIDF.setPIDF(0); } else { - PID.setPIDF(0,0,0,0,0); - PID.setPIDF(1,0,0,0,0); + PIDF.setPIDF(0, new double[] {0,0,0,0}, new double[] {0,0,0,0}); + PIDF.setPIDF(1, new double[] {0,0,0,0}, new double[] {0,0,0,0}); } } diff --git a/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.java b/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.java index b5c4aac..f44de3e 100644 --- a/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.java +++ b/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.java @@ -5,6 +5,7 @@ import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.wpilibj.BuiltInAccelerometer; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.interfaces.Accelerometer; @@ -15,6 +16,8 @@ import org.usfirst.frc.team3695.robot.commands.ManualCommandDrive; import org.usfirst.frc.team3695.robot.enumeration.Bot; import org.usfirst.frc.team3695.robot.enumeration.Drivetrain; +import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PIDF; +import org.usfirst.frc.team3695.robot.util.Util; import org.usfirst.frc.team3695.robot.util.Xbox; /** VROOM VROOM */ @@ -38,7 +41,7 @@ public class SubsystemDrive extends Subsystem { private Accelerometer accel; - public PID pid; // instantiate innerclass + public PIDF pidf; // instantiate innerclass /* Allowable tolerance to be considered in range when driving a distance, in rotations */ public static final double DISTANCE_ALLOWABLE_ERROR = 8.0; @@ -50,7 +53,7 @@ public void initDefaultCommand() { /* converts left magnetic encoder's magic units to inches */ public static double leftMag2In(double leftMag) { - return leftMag / 212; // 204 + return leftMag / 204; // 204 } /* converts right magnetic encoder's magic unit to inches */ @@ -60,7 +63,7 @@ public static double rightMag2In(double rightMag) { /* converts left magnetic encoder's magic units to inches */ public static double leftIn2Mag(double leftMag) { - return leftMag * 212; // 204 + return leftMag * 204; // 204 } /* converts right magnetic encoder's magic units to inches */ @@ -89,9 +92,6 @@ public static double in2rot(double in) { return in / Constants.WHEEL_DIAMETER / Math.PI; } - - - /* apply left motor invert */ public static final double leftify(double left) { return left * (docking ? dockInhibitor : 1); @@ -117,13 +117,13 @@ public SubsystemDrive() { docking = false; dockInhibitor = 0.5d; - pid = new PID(); + pidf = new PIDF(); // masters leftMaster = new TalonSRX(Constants.LEFT_MASTER); - leftMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Constants.LEFT_PID, Constants.TIMEOUT_PID); + leftMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Constants.LEFT_PID, Constants.PIDF_TIMEOUT); rightMaster = new TalonSRX(Constants.RIGHT_MASTER); - rightMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Constants.RIGHT_PID, Constants.TIMEOUT_PID); + rightMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Constants.PIDF_LOOP_ID, Constants.PIDF_TIMEOUT); // slaves leftSlave = new TalonSRX(Constants.LEFT_SLAVE); @@ -131,16 +131,7 @@ public SubsystemDrive() { rightSlave = new TalonSRX(Constants.RIGHT_SLAVE); rightSlave.follow(rightMaster); - switch (Robot.bot){ - case TEUFELSKIND: - PID.setPIDF(0, Constants.TEUFELSKIND.P, Constants.TEUFELSKIND.I, Constants.TEUFELSKIND.D, Constants.TEUFELSKIND.F); - PID.setPIDF(1, Constants.TEUFELSKIND.P, Constants.TEUFELSKIND.I, Constants.TEUFELSKIND.D, Constants.TEUFELSKIND.F); - break; - case OOF: - PID.setPIDF(0, Constants.OOF.P, Constants.OOF.I, Constants.OOF.D, Constants.OOF.F); - PID.setPIDF(1, Constants.OOF.P, Constants.OOF.I, Constants.OOF.D, Constants.OOF.F); - break; - } + PIDF.setPIDF(0); } public void setDrivetrain(Drivetrain drivetrain) { @@ -179,17 +170,9 @@ public void driveRLTank(Joystick joy, double ramp, double inhibitor) { left = (left > 1.0 ? 1.0 : (left < -1.0 ? -1.0 : left)); right = (right > 1.0 ? 1.0 : (right < -1.0 ? -1.0 : right)); setRamps(ramp); - -// if (getYAngle() > Constants.TILT_ANGLE ) { -// leftMaster.set(ControlMode.PercentOutput, -1*Constants.RECOVERY_SPEED); -// rightMaster.set(ControlMode.PercentOutput, -1*Constants.RECOVERY_SPEED); -// } else if (getYAngle() < -1*Constants.TILT_ANGLE){ -// leftMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED); -// rightMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED); -// } else { - leftMaster.set(ControlMode.PercentOutput, leftify(left)); - rightMaster.set(ControlMode.PercentOutput, rightify(right)); -// } + + leftMaster.set(ControlMode.PercentOutput, leftify(left)); + rightMaster.set(ControlMode.PercentOutput, rightify(right)); } /** @@ -205,14 +188,7 @@ public void driveForza(Joystick joy, double ramp, double radius, double inhibito double acceleration = Xbox.RT(joy) - Xbox.LT(joy); setRamps(ramp); -// if (getYAngle() > Constants.TILT_ANGLE ) { -// leftMaster.set(ControlMode.PercentOutput, -1 * Constants.RECOVERY_SPEED); -// rightMaster.set(ControlMode.PercentOutput, -1 * Constants.RECOVERY_SPEED); -// } else if (getYAngle() < -1 * Constants.TILT_ANGLE){ -// leftMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED); -// rightMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED); -// } else { - + if (!reversing ? Xbox.LEFT_X(joy) < 0 : Xbox.LEFT_X(joy) > 0) { right = acceleration; left = (acceleration * ((2 * (1 - Math.abs(Xbox.LEFT_X(joy)))) - 1)) / radius; @@ -223,9 +199,10 @@ public void driveForza(Joystick joy, double ramp, double radius, double inhibito left = acceleration; right = acceleration; } -// }][\ + left = (left > 1.0 ? 1.0 : (left < -1.0 ? -1.0 : left)); right = (right > 1.0 ? 1.0 : (right < -1.0 ? -1.0 : right)); + leftMaster.set(ControlMode.PercentOutput, leftify(left) * inhibitor * (reversing ? -1.0 : 1.0)); rightMaster.set(ControlMode.PercentOutput, rightify(right) * inhibitor * (reversing ? -1.0 : 1.0)); } @@ -279,60 +256,103 @@ public void setInverts() { } - public static class PID { + public static class PIDF { Boolean enabled; - public PID() { + public PIDF() { enabled = true; } - public static void setPIDF(int slot, double p, double i, double d, double f) { - //For future reference: Inverts must be applied individually - if (Robot.bot == Bot.OOF) { - setPIDF(Robot.SUB_DRIVE.leftMaster, p, i, d, f, slot); - setPIDF(Robot.SUB_DRIVE.leftSlave, p, i, d, f, slot); - setPIDF(Robot.SUB_DRIVE.rightMaster, p, i, d, f, slot); - setPIDF(Robot.SUB_DRIVE.rightSlave, p, i, d, f, slot); - } else { - setPIDF(Robot.SUB_DRIVE.leftMaster, p, i, d, f, slot); - setPIDF(Robot.SUB_DRIVE.leftSlave, p, i, d, f, slot); - setPIDF(Robot.SUB_DRIVE.rightMaster, p, i, d, f, slot); - setPIDF(Robot.SUB_DRIVE.rightSlave, p, i, d, f, slot); + + public static void setPIDF(int slot) { + double[] leftDistPIDF = { + Util.getAndSetDouble("LeftDistance-P", 0.16), + Util.getAndSetDouble("LeftDistance-I", 0), + Util.getAndSetDouble("LeftDistance-D", 3), + Util.getAndSetDouble("LeftDistance-F", 0.30509)}; + double[] rightDistPIDF = { + Util.getAndSetDouble("RightDistance-P", 0.1655), + Util.getAndSetDouble("RightDistance-I", 0), + Util.getAndSetDouble("RightDistance-D", 1.655), + Util.getAndSetDouble("RightDistance-F", 0.33355)}; + double[] leftCWPIDF = { + Util.getAndSetDouble("LeftCW-P", 1.16013), + Util.getAndSetDouble("LeftCW-I", 0), + Util.getAndSetDouble("LeftCW-D", 1.60125), + Util.getAndSetDouble("LeftCW-F", .30509)}; + double[] rightCWPIDF = { + Util.getAndSetDouble("RightCW-P", 0.17514), + Util.getAndSetDouble("RightCW-I", 0), + Util.getAndSetDouble("RightCW-D", 1.7514), + Util.getAndSetDouble("RightCW-F", 0.33355)}; + double[] leftCCWPIDF = { + Util.getAndSetDouble("LeftCCW-P", 0.176125), + Util.getAndSetDouble("LeftCCW-I", 0), + Util.getAndSetDouble("LeftCCW-D", 1.60125), + Util.getAndSetDouble("LeftCCW-F", .30509)}; + double[] rightCCWPIDF = { + Util.getAndSetDouble("RightCCW-P", 1.19864), + Util.getAndSetDouble("RightCCW-I", 0), + Util.getAndSetDouble("RightCCW-D", 1.7514), + Util.getAndSetDouble("RightCCW-F", 0.33355)}; + + if (slot == 0){ + PIDF.setPIDF(0, leftDistPIDF, rightDistPIDF); + } else if (slot == 1) { + PIDF.setPIDF(1, leftCWPIDF, rightCWPIDF); + } else if (slot == 2) { + PIDF.setPIDF(2, leftCCWPIDF, rightCCWPIDF); } + + } + + public static void setPIDF(int slot, double[] leftPIDF, double[] rightPIDF) { + //For future reference: Inverts must be applied individually + setPIDF(Robot.SUB_DRIVE.leftMaster, leftPIDF, slot, (int) Util.getAndSetDouble("Left Velocity", 0), (int) Util.getAndSetDouble("Left Acceleration", 0)); + setPIDF(Robot.SUB_DRIVE.leftSlave, leftPIDF, slot, (int) Util.getAndSetDouble("Left Velocity", 0), (int) Util.getAndSetDouble("Left Acceleration", 0)); + setPIDF(Robot.SUB_DRIVE.rightMaster, rightPIDF, slot, (int) Util.getAndSetDouble("Right Velocity", 0), (int) Util.getAndSetDouble("Right Acceleration", 0)); + setPIDF(Robot.SUB_DRIVE.rightSlave, rightPIDF, slot, (int) Util.getAndSetDouble("Right Velocity", 0), (int) Util.getAndSetDouble("Right Acceleration", 0)); + //Do not remove. This delay may be necessary to ensure all PID loops have updated before it continues driving + for (int i = 0; i < 4; i++) { + DriverStation.reportWarning(Double.toString(leftPIDF[i]), false); + DriverStation.reportWarning(Double.toString(rightPIDF[i]), false); + } } - public static void setPIDF(TalonSRX _talon, double p, double i, double d, double f, int slot) { + // + public static void setPIDF(TalonSRX _talon, double[] PIDF, int slot, int velocity, int acceleration) { /* first choose the sensor */ _talon.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, - 0, Constants.TIMEOUT_PID); + 0, Constants.PIDF_TIMEOUT); _talon.setSensorPhase(true); /* Set relevant frame periods to be at least as fast as periodic rate*/ _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, - Constants.TIMEOUT_PID); + Constants.PIDF_TIMEOUT); _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, - Constants.TIMEOUT_PID); + Constants.PIDF_TIMEOUT); /* set the peak and nominal outputs */ - _talon.configNominalOutputForward(0, Constants.TIMEOUT_PID); - _talon.configNominalOutputReverse(0, Constants.TIMEOUT_PID); - _talon.configPeakOutputForward(1, Constants.TIMEOUT_PID); - _talon.configPeakOutputReverse(-1, Constants.TIMEOUT_PID); + _talon.configNominalOutputForward(0, Constants.PIDF_TIMEOUT); + _talon.configNominalOutputReverse(0, Constants.PIDF_TIMEOUT); + _talon.configPeakOutputForward(1, Constants.PIDF_TIMEOUT); + _talon.configPeakOutputReverse(-1, Constants.PIDF_TIMEOUT); /* set closed loop gains in slot */ - _talon.selectProfileSlot(slot, Constants.RIGHT_PID); - _talon.config_kP(slot, p, Constants.TIMEOUT_PID); - _talon.config_kI(slot, i, Constants.TIMEOUT_PID); - _talon.config_kD(slot, d, Constants.TIMEOUT_PID); - _talon.config_kF(slot, f, Constants.TIMEOUT_PID); + _talon.selectProfileSlot(slot, Constants.PIDF_LOOP_ID); + _talon.config_kP(slot, PIDF[0], Constants.PIDF_TIMEOUT); + _talon.config_kI(slot, PIDF[1], Constants.PIDF_TIMEOUT); + _talon.config_kD(slot, PIDF[2], Constants.PIDF_TIMEOUT); + _talon.config_kF(slot, PIDF[3], Constants.PIDF_TIMEOUT); /* set acceleration and vcruise velocity - see documentation */ - _talon.configMotionCruiseVelocity(15000, Constants.TIMEOUT_PID); - _talon.configMotionAcceleration(6000, Constants.TIMEOUT_PID); + _talon.configMotionCruiseVelocity(velocity, Constants.PIDF_TIMEOUT); + _talon.configMotionAcceleration(acceleration, Constants.PIDF_TIMEOUT); } - public double getError() { - return (leftify(Robot.SUB_DRIVE.leftMaster.getErrorDerivative(Constants.LEFT_PID)) + rightify(Robot.SUB_DRIVE.rightMaster.getErrorDerivative(Constants.RIGHT_PID))) / 2.0; + public void getError() { + SmartDashboard.putNumber("Right Error:", Robot.SUB_DRIVE.rightMaster.getErrorDerivative(0)); + SmartDashboard.putNumber("Left Error:", Robot.SUB_DRIVE.leftMaster.getErrorDerivative(0)); } public double getRightInches() { - return rightMag2In(Robot.SUB_DRIVE.rightMaster.getSelectedSensorPosition(Constants.RIGHT_PID)); + return rightMag2In(Robot.SUB_DRIVE.rightMaster.getSelectedSensorPosition(Constants.PIDF_LOOP_ID)); } public double getLeftInches() { @@ -340,10 +360,10 @@ public double getLeftInches() { } public void reset() { - Robot.SUB_DRIVE.leftMaster.setSelectedSensorPosition(0, Constants.LEFT_PID, Constants.TIMEOUT_PID); - Robot.SUB_DRIVE.rightMaster.setSelectedSensorPosition(0, Constants.RIGHT_PID, Constants.TIMEOUT_PID); - Robot.SUB_DRIVE.leftMaster.setIntegralAccumulator(0,0, Constants.TIMEOUT_PID); - Robot.SUB_DRIVE.rightMaster.setIntegralAccumulator(0,0, Constants.TIMEOUT_PID); + Robot.SUB_DRIVE.leftMaster.setSelectedSensorPosition(0, Constants.LEFT_PID, Constants.PIDF_TIMEOUT); + Robot.SUB_DRIVE.rightMaster.setSelectedSensorPosition(0, Constants.PIDF_LOOP_ID, Constants.PIDF_TIMEOUT); + Robot.SUB_DRIVE.leftMaster.setIntegralAccumulator(0,0, Constants.PIDF_TIMEOUT); + Robot.SUB_DRIVE.rightMaster.setIntegralAccumulator(0,0, Constants.PIDF_TIMEOUT); } } } diff --git a/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.java b/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.java index 3d89d31..314a650 100644 --- a/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.java +++ b/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.java @@ -46,14 +46,14 @@ public SubsystemManipulator(){ /** eat the power cube */ public void eat() { - armLeft.set(ControlMode.PercentOutput, leftArmify(-1)); - armRight.set(ControlMode.PercentOutput, rightArmify(-1)); + armLeft.set(ControlMode.PercentOutput, leftArmify(1)); + armRight.set(ControlMode.PercentOutput, rightArmify(1)); } /** spit out the power cube */ public void spit() { - armLeft.set(ControlMode.PercentOutput, leftArmify(1)); - armRight.set(ControlMode.PercentOutput, rightArmify(1)); + armLeft.set(ControlMode.PercentOutput, leftArmify(-1)); + armRight.set(ControlMode.PercentOutput, rightArmify(-1)); } public void spinByJoystick(Joystick joy) { diff --git a/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.java b/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.java index 121f551..b72d2fb 100644 --- a/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.java +++ b/src/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.java @@ -38,10 +38,10 @@ public void initDefaultCommand() { public SubsystemMast(){ /** Does it make sense to move the limit switch IDs to Constants? **/ //no because they could be moved due to various electrical reasons-nate - lowerPinionLimit = new DigitalInput(6); + lowerPinionLimit = new DigitalInput(3); upperPinionLimit = new DigitalInput(7); - lowerScrewLimit = new DigitalInput(3); - upperScrewLimit = new DigitalInput(5); + lowerScrewLimit = new DigitalInput(5); + upperScrewLimit = new DigitalInput(4); leftPinion = new TalonSRX(Constants.LEFT_PINION_MOTOR); rightPinion = new TalonSRX(Constants.RIGHT_PINION_MOTOR); @@ -87,8 +87,8 @@ public void moveBySpeed(Joystick joy, double inhibitor) { publishSwitches(); - leftPinion.set(ControlMode.PercentOutput, leftPinionate(pinionSpeed)); - rightPinion.set(ControlMode.PercentOutput, rightPinionate(pinionSpeed)); + leftPinion.set(ControlMode.PercentOutput, leftPinionate(-1 * pinionSpeed)); + rightPinion.set(ControlMode.PercentOutput, rightPinionate(-1 * pinionSpeed)); screw.set(ControlMode.PercentOutput, inhibitor * screwify(screwSpeed)); }