From 600a1a1c0e90c03a198c6036de8e5157b7f96af3 Mon Sep 17 00:00:00 2001 From: Kevin Zhang Date: Wed, 17 Feb 2016 15:47:24 -0800 Subject: [PATCH] Move all constants in DeadReckoning to Auton class because it makes more sense --- .../usfirst/frc/team3501/robot/Constants.java | 45 +++++++++---------- .../robot/commands/auton/AlignToScore.java | 10 ++--- .../robot/commands/auton/PassLowBar.java | 8 ++-- .../robot/commands/auton/PassMoat.java | 8 ++-- .../robot/commands/auton/PassRampart.java | 8 ++-- .../robot/commands/auton/PassRockWall.java | 8 ++-- .../commands/auton/PassRoughTerrain.java | 8 ++-- .../robot/commands/driving/TurnForTime.java | 4 +- .../team3501/robot/subsystems/DriveTrain.java | 12 ++++- 9 files changed, 60 insertions(+), 51 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 2d3cd43d..7245d5fe 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -124,29 +124,6 @@ public class Constants { } } - public static class DeadReckoning { - public static final double DEFAULT_SPEED = 0.5; - public static boolean isUsingTimeToPassDefense = true; - - // dead reckoning time and speed constants for driving through defenses - public static double passRockWallTime = 0; - public static double passRockWallSpeed = 0; - public static double passRockWallDistance = 0; - public static double passLowBarTime = 0; - public static double passLowBarSpeed = 0; - public static double passLowBarDistance = 0; - public static double passMoatTime = 0; - public static double passMoatSpeed = 0; - public static double passMoatDistance = 0; - public static double passRampartTime = 0; - public static double passRampartSpeed = 0; - public static double passRampartDistance = 0; - public static double passRoughTerrainTime = 0; - public static double passRoughTerrainSpeed = 0; - public static double passRoughTerrainDistance = 0; - - } - public static class IntakeArm { public static final int ROLLER_PORT = 0; public static final int ARM_PORT = 1; @@ -249,6 +226,28 @@ public class Constants { public static final double POS5_TURN1_TIME = 60; public static final double POS5_TURN_MAXSPEED = 0.5; public static final double POS5_DIST2_TIME = 0; + + // Passing Defenses Constants + + public static final double DEFAULT_SPEED = 0.5; + public static boolean isUsingTimeToPassDefense = true; + + // dead reckoning time and speed constants for driving through defenses + public static double passRockWallTime = 0; + public static double passRockWallSpeed = 0; + public static double passRockWallDistance = 0; + public static double passLowBarTime = 0; + public static double passLowBarSpeed = 0; + public static double passLowBarDistance = 0; + public static double passMoatTime = 0; + public static double passMoatSpeed = 0; + public static double passMoatDistance = 0; + public static double passRampartTime = 0; + public static double passRampartSpeed = 0; + public static double passRampartDistance = 0; + public static double passRoughTerrainTime = 0; + public static double passRoughTerrainSpeed = 0; + public static double passRoughTerrainDistance = 0; } public enum Direction { diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java index f2c577ce..6d0b9ede 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java @@ -39,7 +39,7 @@ public class AlignToScore extends CommandGroup { public double horizontalDistToGoal; public AlignToScore(int position) { - if (Constants.DeadReckoning.isUsingTimeToPassDefense) { + if (Constants.Auton.isUsingTimeToPassDefense) { if (position == 1) { addSequential(new DriveForTime(Constants.Auton.POS1_DIST1_TIME, Constants.Auton.POS1_DRIVE_MAXSPEED)); @@ -146,7 +146,7 @@ public class AlignToScore extends CommandGroup { * double horizontalDistToGoal) { * double leftDist = Robot.driveTrain.getLeftLidarDistance(); * double rightDist = Robot.driveTrain.getRightLidarDistance(); - * + * * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2); * double distToTower; * // TODO: figure out if we do want to shoot into the side goal if we are @@ -156,7 +156,7 @@ public class AlignToScore extends CommandGroup { * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) * - DIST_CASTLE_WALL_TO_SIDE_GOAL; * } - * + * * // TODO: figure out if we do want to shoot into the font goal if we are * // in position 3, 4, 5, or if we want to change that * else { @@ -164,9 +164,9 @@ public class AlignToScore extends CommandGroup { * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) * - DIST_CASTLE_WALL_TO_SIDE_GOAL; * } - * + * * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal); - * + * * return angleToTurn; * } */ diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java index 49b085d0..8f35a25c 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java @@ -25,13 +25,13 @@ import edu.wpi.first.wpilibj.command.CommandGroup; public class PassLowBar extends CommandGroup { public PassLowBar() { - if (Constants.DeadReckoning.isUsingTimeToPassDefense) { - addSequential(new DriveForTime(Constants.DeadReckoning.passLowBarTime, - Constants.DeadReckoning.passLowBarSpeed)); + if (Constants.Auton.isUsingTimeToPassDefense) { + addSequential(new DriveForTime(Constants.Auton.passLowBarTime, + Constants.Auton.passLowBarSpeed)); } else { addSequential(new DriveDistance( - Constants.DeadReckoning.passLowBarDistance, + Constants.Auton.passLowBarDistance, Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java index f9331077..3539d970 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassMoat.java @@ -28,12 +28,12 @@ import edu.wpi.first.wpilibj.command.CommandGroup; public class PassMoat extends CommandGroup { public PassMoat() { - if (Constants.DeadReckoning.isUsingTimeToPassDefense) { - addSequential(new DriveForTime(Constants.DeadReckoning.passMoatTime, - Constants.DeadReckoning.passMoatSpeed)); + if (Constants.Auton.isUsingTimeToPassDefense) { + addSequential(new DriveForTime(Constants.Auton.passMoatTime, + Constants.Auton.passMoatSpeed)); } else { - addSequential(new DriveDistance(Constants.DeadReckoning.passMoatDistance, + addSequential(new DriveDistance(Constants.Auton.passMoatDistance, Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); } diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java index 21c12895..30051460 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java @@ -26,13 +26,13 @@ public class PassRampart extends CommandGroup { public PassRampart() { - if (Constants.DeadReckoning.isUsingTimeToPassDefense) { - addSequential(new DriveForTime(Constants.DeadReckoning.passRockWallTime, - Constants.DeadReckoning.passRockWallSpeed)); + if (Constants.Auton.isUsingTimeToPassDefense) { + addSequential(new DriveForTime(Constants.Auton.passRockWallTime, + Constants.Auton.passRockWallSpeed)); } else { addSequential(new DriveDistance( - Constants.DeadReckoning.passRampartDistance, + Constants.Auton.passRampartDistance, Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); } diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java index ea87ca29..a0b6690c 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRockWall.java @@ -25,13 +25,13 @@ import edu.wpi.first.wpilibj.command.CommandGroup; public class PassRockWall extends CommandGroup { public PassRockWall() { - if (Constants.DeadReckoning.isUsingTimeToPassDefense) { - addSequential(new DriveForTime(Constants.DeadReckoning.passRockWallTime, - Constants.DeadReckoning.passRockWallSpeed)); + if (Constants.Auton.isUsingTimeToPassDefense) { + addSequential(new DriveForTime(Constants.Auton.passRockWallTime, + Constants.Auton.passRockWallSpeed)); } else { addSequential(new DriveDistance( - Constants.DeadReckoning.passRockWallDistance, + Constants.Auton.passRockWallDistance, Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); } diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java index 2ddddf43..e3d64dde 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java @@ -26,13 +26,13 @@ public class PassRoughTerrain extends CommandGroup { public PassRoughTerrain() { - if (Constants.DeadReckoning.isUsingTimeToPassDefense) { - addSequential(new DriveForTime(Constants.DeadReckoning.passRockWallTime, - Constants.DeadReckoning.passRockWallSpeed)); + if (Constants.Auton.isUsingTimeToPassDefense) { + addSequential(new DriveForTime(Constants.Auton.passRockWallTime, + Constants.Auton.passRockWallSpeed)); } else { addSequential(new DriveDistance( - Constants.DeadReckoning.passRoughTerrainDistance, + Constants.Auton.passRoughTerrainDistance, Constants.DriveTrain.PASS_DEFENSE_TIMEOUT)); } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java index 21f200c8..368d1045 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForTime.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team3501.robot.commands.driving; -import org.usfirst.frc.team3501.robot.Constants.DeadReckoning; +import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.Constants.Direction; import org.usfirst.frc.team3501.robot.Robot; @@ -36,7 +36,7 @@ public class TurnForTime extends Command { } public TurnForTime(double seconds, Direction direction) { - this(seconds, direction, DeadReckoning.DEFAULT_SPEED); + this(seconds, direction, Constants.Auton.DEFAULT_SPEED); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 96f1d115..9852848e 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -227,10 +227,16 @@ public class DriveTrain extends PIDSubsystem { } /* - * Checks the drive mode + * Checks the drive mode <<<<<<< 9728080f491e9fb09795494349dba1297f447c0f * * @return the current state of the robot in each state Average distance from * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE + * ======= + * + * @return the current state of the robot in each state Average distance from + * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE + * >>>>>>> Move all constants in DeadReckoning to Auton class because it makes + * more sense */ private double sensorFeedback() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) @@ -360,4 +366,8 @@ public class DriveTrain extends PIDSubsystem { leftGearPiston.set(gear); rightGearPiston.set(gear); } + + public void toggleTimeDeadReckoning() { + Constants.Auton.isUsingTimeToPassDefense = !Constants.Auton.isUsingTimeToPassDefense; + } } -- 2.30.2