From 28375996e4fbd5a1b06b8e0a6007f3fa46872bc6 Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Sun, 8 Oct 2023 13:19:43 -0700 Subject: [PATCH 1/4] Workshop 10/7 --- src/main/java/frc/team3128/Constants.java | 34 +++++++++---------- .../java/frc/team3128/PositionConstants.java | 2 +- .../java/frc/team3128/RobotContainer.java | 8 ++--- .../frc/team3128/commands/CmdManager.java | 4 +-- .../frc/team3128/commands/CmdSwerveDrive.java | 6 ++-- .../frc/team3128/commands/CmdTrajectory.java | 11 +++--- 6 files changed, 30 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 5156769..3f7a16a 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -53,15 +53,15 @@ public static class TrajectoryConstants { public static final double CONDITION_3 = 2.5; public static final Pose2d[] END_POINTS = new Pose2d[]{ - new Pose2d(1.75,0.5,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,0.95,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,1.55,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,2.05,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,2.65,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,3.2,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,3.75,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,4.35,Rotation2d.fromDegrees(0)), - new Pose2d(1.75,4.79,Rotation2d.fromDegrees(0)) + new Pose2d(1.75,0.5,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,0.95,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,1.55,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,2.05,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,2.65,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,3.2,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,3.75,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,4.35,Rotation2d.fromDegrees(180)), + new Pose2d(1.75,4.79,Rotation2d.fromDegrees(180)) }; } @@ -185,13 +185,11 @@ public static class SwerveConstants { public static final double rotationKP = 2; public static final double rotationKI = 0; public static final double rotationKD = 0; - public static final double rotationKS = 0; /* Turning PID Values */ - public static final double turnKP = 0.1; + public static final double turnKP = 5; public static final double turnKI = 0; public static final double turnKD = 0; - public static final double turnKF = 0.1; /* Angle Motor PID Values */ // switched 364 pid values to SDS pid values @@ -522,18 +520,18 @@ public static class ElevatorConstants { public static final int ELV1_ID = 11; public static final int ELV2_ID = 12; - public static final double kP = 3; + public static final double kP = 1; public static final double kI = 0; public static final double kD = 0; - public static final double kS = 0.475; + public static final double kS = 0.975; public static final double kV = 0; - public static final double kG = 0.625; + public static final double kG = 1.05; public static final double MIN_DIST = 2; //Ask Charlie public static final double MAX_DIST = 55; //Ask Charlie - public static final double GEAR_RATIO = 12.5; + public static final double GEAR_RATIO = 10; public static final double SPOOL_CIRCUMFERENCE = 3 * Math.PI; public static final double FRAME_LENGTH = 15; @@ -552,12 +550,12 @@ public static class BalanceConstants{ } public static class LedConstants{ - public static final int CANDLE_ID = 37; + public static final int CANDLE_ID = 52; public static final int WHITE_VALUE = 0; //leds used don't have a white value public static final int STARTING_ID = 8; - public static final int PIVOT_COUNT = 100; + public static final int PIVOT_COUNT = 200; public static final int PIVOT_COUNT_FRONT = 50; //change public static final int PIVOT_COUNT_BACK = 50; //change diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 9440a66..32c54be 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -14,7 +14,7 @@ public static enum Position { CHUTE_CUBE(10, 45, false), GROUND_CONE(10, -18, true), - GROUND_CUBE(0, 0, false), + GROUND_CUBE(2.5, 0, false), NEUTRAL(3, 80, false); diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 7852094..edbed91 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -98,21 +98,21 @@ private void configureButtonBindings() { controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)); controller.getUpPOVButton().onTrue(runOnce(()-> { - CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 0 : 180; + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; CmdSwerveDrive.enabled = true; })); controller.getDownPOVButton().onTrue(runOnce(()-> { - CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 0 : 180; CmdSwerveDrive.enabled = true; })); controller.getRightPOVButton().onTrue(runOnce(()-> { - CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 270 : 90; + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 90 : 270; CmdSwerveDrive.enabled = true; })); controller.getLeftPOVButton().onTrue(runOnce(()-> { - CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 90 : 270; + CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 270 : 90; CmdSwerveDrive.enabled = true; })); diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index c2a919a..b1138d2 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -32,8 +32,8 @@ private CmdManager() {} private static CommandBase score(Position position, int xPos, boolean runImmediately) { return sequence( runOnce(()-> ENABLE = runImmediately), - // waitUntil(()-> ENABLE), - // either(none(), new CmdTrajectory(xPos + Vision.SELECTED_GRID * 3), ()-> runImmediately), + waitUntil(()-> ENABLE), + either(none(), new CmdTrajectory(xPos), ()-> runImmediately), waitUntil(()-> ENABLE), extend(position), waitUntil(()-> !ENABLE), diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index 6136b49..5a3fc96 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -28,7 +28,6 @@ public class CmdSwerveDrive extends CommandBase { private final SlewRateLimiter accelLimiter; private final PIDController rController; - private final DoubleSupplier kS; public static boolean enabled = false; public static double rSetpoint; @@ -41,10 +40,10 @@ public CmdSwerveDrive(DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier this.zAxis = zAxis; accelLimiter = new SlewRateLimiter(maxAcceleration); - rController = new PIDController(rotationKP, 0, 0); + rController = new PIDController(turnKP, 0, 0); rController.enableContinuousInput(0, 360); rController.setTolerance(0.5); - kS = NAR_Shuffleboard.debug("Drivetrain", "kS", rotationKS, 5, 4); + swerve.fieldRelative = fieldRelative; } @@ -67,7 +66,6 @@ public void execute() { } if (enabled) { rotation = Units.degreesToRadians(rController.calculate(swerve.getGyroRotation2d().getDegrees(), rSetpoint)); - rotation += Math.copySign(kS.getAsDouble(), rotation); if (rController.atSetpoint()) { rotation = 0; } diff --git a/src/main/java/frc/team3128/commands/CmdTrajectory.java b/src/main/java/frc/team3128/commands/CmdTrajectory.java index 7f50a2d..3ac0b4b 100644 --- a/src/main/java/frc/team3128/commands/CmdTrajectory.java +++ b/src/main/java/frc/team3128/commands/CmdTrajectory.java @@ -59,7 +59,7 @@ private boolean isPastPoint(Translation2d start, double condition) { private ArrayList generatePoses() { final ArrayList pathPoints = new ArrayList(); final Translation2d start = swerve.getPose().getTranslation(); - final Rotation2d holonomicAngle = allianceFlip(END_POINTS[index].getRotation()); + final Rotation2d holonomicAngle = END_POINTS[index].getRotation(); final PathPoint startPoint = new PathPoint(start, allianceFlip(HEADING), swerve.getGyroRotation2d()); final boolean topPath = start.getY() >= (POINT_2A.getY() + POINT_2B.getY()) / 2; final boolean skipLastPoint = (topPath && index == 8) || (!topPath && index == 0); @@ -85,10 +85,9 @@ private CommandBase generateAuto() { @Override public void initialize() { - if (Vision.AUTO_ENABLED) { - trajCommand = generateAuto(); - trajCommand.schedule(); - } + trajCommand = generateAuto(); + trajCommand.schedule(); + CmdSwerveDrive.enabled = false; } @Override @@ -106,6 +105,6 @@ public void execute() { @Override public boolean isFinished(){ - return swerve.getPose().minus(END_POINTS[index]).getTranslation().getNorm() < 0.5 || !Vision.AUTO_ENABLED; + return swerve.getPose().minus(END_POINTS[index]).getTranslation().getNorm() < 0.5; } } From 3d245bb4a9a9e25ffeca228a802df6ef50d6bb45 Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Sun, 8 Oct 2023 18:15:03 -0700 Subject: [PATCH 2/4] SDA 10/8 --- src/main/java/frc/team3128/Constants.java | 24 +++++++++---------- .../java/frc/team3128/PositionConstants.java | 4 ++-- .../java/frc/team3128/RobotContainer.java | 3 ++- .../frc/team3128/commands/CmdManager.java | 4 +++- .../frc/team3128/commands/CmdTrajectory.java | 4 +++- .../java/frc/team3128/subsystems/Swerve.java | 20 ++++++++++++++++ 6 files changed, 42 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 3f7a16a..0b88cb9 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -216,7 +216,7 @@ public static class SwerveConstants { // For safety, use less than theoretical and real values public static final double maxSpeed = 4.5; //meters per second - 16.3 ft/sec public static final double bumpSpeed = 2.5; - public static final double maxAcceleration = 2.3; + public static final double maxAcceleration = 2.5; public static final double maxAngularVelocity = 5; //3; //11.5; // citrus: 10 public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(maxSpeed, maxAcceleration); @@ -313,15 +313,15 @@ public static class VisionConstants { public static final Matrix SVR_VISION_MEASUREMENT_STD = VecBuilder.fill(1,1,Units.degreesToRadians(10)); public static final Pose2d[] SCORES = new Pose2d[]{ - new Pose2d(1.90,0.5,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,1.05,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,1.65,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,2.15,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,2.75,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,3.3,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,3.85,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,4.45,Rotation2d.fromDegrees(180)), - new Pose2d(1.90,4.89,Rotation2d.fromDegrees(180)) + new Pose2d(1.90,0.55,Rotation2d.fromDegrees(180)), //0.5 + new Pose2d(1.90,1.1,Rotation2d.fromDegrees(180)), //1.05 + new Pose2d(1.90,1.7,Rotation2d.fromDegrees(180)), //1.65 + new Pose2d(1.90,2.2,Rotation2d.fromDegrees(180)), //2.15 + new Pose2d(1.90,2.8,Rotation2d.fromDegrees(180)), //2.75 + new Pose2d(1.90,3.35,Rotation2d.fromDegrees(180)), //3.3 + new Pose2d(1.90,3.90,Rotation2d.fromDegrees(180)), //3.85 + new Pose2d(1.90,4.50,Rotation2d.fromDegrees(180)), //4.45 + new Pose2d(1.90,4.94,Rotation2d.fromDegrees(180)) //4.89 }; public static final Pose2d[][] SCORES_GRID = new Pose2d[][] { @@ -509,11 +509,11 @@ public static class ManipulatorConstants{ public static final int ROLLER_MOTOR_ID = 31; public static final double ROLLER_POWER = 0.9; public static final double STALL_POWER_CONE = 0.05; - public static final double STALL_POWER_CUBE = 0; + public static final double STALL_POWER_CUBE = 0.1; public static final double CURRENT_THRESHOLD_CONE = 25; - public static final double CURRENT_THRESHOLD_CUBE = 15; + public static final double CURRENT_THRESHOLD_CUBE = 20; } public static class ElevatorConstants { diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 32c54be..062c9d9 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -3,8 +3,8 @@ public class PositionConstants { public static enum Position { HIGH_CONE(47, 0, true), - HIGH_CUBE(50, 20, false), - MID_CONE(22.5, 28, true), + HIGH_CUBE(48.5, 20, false), + MID_CONE(25.5, 28, true), MID_CUBE(25, 30, false), LOW(3, 45, true), diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index edbed91..2683c83 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -22,6 +22,7 @@ import frc.team3128.common.utility.Log; import frc.team3128.subsystems.Elevator; import frc.team3128.subsystems.Leds; +import frc.team3128.subsystems.Manipulator; import frc.team3128.common.utility.NAR_Shuffleboard; import frc.team3128.subsystems.Swerve; import frc.team3128.subsystems.Vision; @@ -145,7 +146,7 @@ private void configureButtonBindings() { buttonPad.getButton(13).onTrue(runOnce(()-> SINGLE_STATION = !SINGLE_STATION)); - buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL)); + buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL).beforeStarting(stopManip())); buttonPad.getButton(16).onTrue( HPpickup(Position.CHUTE_CONE, Position.SHELF_CONE) diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index b1138d2..7757841 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.team3128.RobotContainer; +import frc.team3128.Constants.LedConstants.Colors; import frc.team3128.PositionConstants.Position; import frc.team3128.common.hardware.input.NAR_XboxController; @@ -58,6 +59,7 @@ public static CommandBase HPpickup(Position position1, Position position2) { public static CommandBase pickup(Position position, boolean runImmediately) { return sequence( + runOnce(()->leds.setElevatorLeds(position.cone ? Colors.CONE : Colors.CUBE)), runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), extend(position), @@ -104,7 +106,7 @@ public static CommandBase intake(Boolean cone) { runOnce(()-> manipulator.intake(cone), manipulator), waitSeconds(0.4), waitUntil(()-> manipulator.hasObjectPresent()), - waitSeconds(cone ? 0.15 : 0), + waitSeconds(cone ? 0.15 : 0.15), runOnce(()-> manipulator.stallPower(), manipulator) ); } diff --git a/src/main/java/frc/team3128/commands/CmdTrajectory.java b/src/main/java/frc/team3128/commands/CmdTrajectory.java index 3ac0b4b..a19ab11 100644 --- a/src/main/java/frc/team3128/commands/CmdTrajectory.java +++ b/src/main/java/frc/team3128/commands/CmdTrajectory.java @@ -29,6 +29,7 @@ public class CmdTrajectory extends CommandBase { private final Swerve swerve; private final int xPos; + private Pose2d endPoint; private int index; private CommandBase trajCommand; @@ -88,6 +89,7 @@ public void initialize() { trajCommand = generateAuto(); trajCommand.schedule(); CmdSwerveDrive.enabled = false; + endPoint = flip(END_POINTS[index]); } @Override @@ -105,6 +107,6 @@ public void execute() { @Override public boolean isFinished(){ - return swerve.getPose().minus(END_POINTS[index]).getTranslation().getNorm() < 0.5; + return swerve.getPose().minus(endPoint).getTranslation().getNorm() < 0.5; } } diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java index fc4cc79..c94fce1 100644 --- a/src/main/java/frc/team3128/subsystems/Swerve.java +++ b/src/main/java/frc/team3128/subsystems/Swerve.java @@ -40,6 +40,10 @@ public class Swerve extends SubsystemBase { public boolean fieldRelative; public double throttle = 0.8; + public double speed = 0; + private Translation2d prevTrans = new Translation2d(); + public double acceleration = 0; + private double prevSpeed = 0; private double initialRoll, initialPitch; @@ -56,6 +60,7 @@ public Swerve() { fieldRelative = true; estimatedPose = new Pose2d(); zeroAxis(); + prevTrans = new Translation2d(); modules = new SwerveModule[] { new SwerveModule(0, Mod0.constants), @@ -105,6 +110,8 @@ public void initShuffleboard() { NAR_Shuffleboard.addComplex("Drivetrain","Drivetrain", this,0,0); NAR_Shuffleboard.addData("Drivetrain", "ENABLE", ()-> CmdManager.ENABLE, 0, 1); NAR_Shuffleboard.addData("Drivetrain", "Single Station", ()-> CmdManager.SINGLE_STATION, 0, 3); + NAR_Shuffleboard.addData("Drivetrain", "Speed", ()-> speed, 2, 1); + NAR_Shuffleboard.addData("Drivetrain", "Acceleration", ()-> acceleration, 3, 1); } public Pose2d getPose() { @@ -164,6 +171,19 @@ public void periodic() { for (SwerveModule module : modules) { SmartDashboard.putNumber("module " + module.moduleNumber, module.getCanCoder().getDegrees()); } + updateSpeed(); + updateAcceleration(); + } + + public void updateSpeed() { + Translation2d translation = getPose().getTranslation(); + speed = translation.getDistance(prevTrans) / 0.02; + prevTrans = translation; + } + + public void updateAcceleration(){ + acceleration = speed - prevSpeed / 0.02; + prevSpeed = speed; } public void resetAll() { From 190860e3552ebf398802d3a3a35734a59888816b Mon Sep 17 00:00:00 2001 From: Mason-Lam <97353903+Mason-Lam@users.noreply.github.com> Date: Fri, 13 Oct 2023 17:40:56 -0700 Subject: [PATCH 3/4] Workshop 10/13 --- src/main/java/frc/team3128/Constants.java | 9 +++++++-- src/main/java/frc/team3128/PositionConstants.java | 4 ++-- src/main/java/frc/team3128/Robot.java | 2 +- src/main/java/frc/team3128/RobotContainer.java | 4 ++-- .../frc/team3128/autonomous/AutoPrograms.java | 15 +++++++++++++-- .../java/frc/team3128/commands/CmdManager.java | 7 +++++++ .../java/frc/team3128/commands/CmdTrajectory.java | 2 +- .../java/frc/team3128/subsystems/Elevator.java | 1 + .../java/frc/team3128/subsystems/Manipulator.java | 2 +- src/main/java/frc/team3128/subsystems/Vision.java | 1 + src/main/java/frc/team3128/subsystems/Wrist.java | 4 ++-- 11 files changed, 38 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 0b88cb9..1a13c2a 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -288,6 +288,11 @@ public static class VisionConstants { new Translation2d(Units.inchesToMeters(5.43), Units.inchesToMeters(11.7)), Rotation2d.fromDegrees(0))); + public static final Camera BACK_RIGHT = new Camera("BACK_RIGHT", true, 0, 0, 0, + new Transform2d( + new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(-11.7)), + Rotation2d.fromDegrees(180))); + public static final PIDController xController = new PIDController(1, 0, 0); public static final PIDController yController = new PIDController(1, 0, 0); public static final PIDController rController = new PIDController(1, 0, 0); @@ -512,8 +517,8 @@ public static class ManipulatorConstants{ public static final double STALL_POWER_CUBE = 0.1; - public static final double CURRENT_THRESHOLD_CONE = 25; - public static final double CURRENT_THRESHOLD_CUBE = 20; + public static final double CURRENT_THRESHOLD_CONE = 30; + public static final double CURRENT_THRESHOLD_CUBE = 25; } public static class ElevatorConstants { diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 062c9d9..8407db5 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -6,14 +6,14 @@ public static enum Position { HIGH_CUBE(48.5, 20, false), MID_CONE(25.5, 28, true), MID_CUBE(25, 30, false), - LOW(3, 45, true), + LOW(3, 0, true), SHELF_CONE(53, -30, true), SHELF_CUBE(37, 22, false), CHUTE_CONE(10, 55, true), CHUTE_CUBE(10, 45, false), - GROUND_CONE(10, -18, true), + GROUND_CONE(8, -18, true), GROUND_CUBE(2.5, 0, false), NEUTRAL(3, 80, false); diff --git a/src/main/java/frc/team3128/Robot.java b/src/main/java/frc/team3128/Robot.java index 1976dff..ad6c9cd 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -59,7 +59,7 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { CommandScheduler.getInstance().run(); - if (timer.hasElapsed(14.75) && Math.abs(Swerve.getInstance().getPitch()) > 6) { + if (timer.hasElapsed(14.75)) { new RunCommand(()->Swerve.getInstance().xlock(), Swerve.getInstance()).schedule(); } } diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 2683c83..9b5cfdf 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -95,8 +95,8 @@ private void configureButtonBindings() { controller.getButton("B").onTrue(new InstantCommand(()-> swerve.resetEncoders())); controller.getButton("Y").onTrue(runOnce(()-> swerve.throttle = 1)).onFalse(runOnce(()-> swerve.throttle = 0.8)); controller.getButton("Start").onTrue(resetSwerve()); - controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)); - controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)); + controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL)); + controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL)); controller.getUpPOVButton().onTrue(runOnce(()-> { CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index 88de510..3100486 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -4,8 +4,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import static edu.wpi.first.wpilibj2.command.Commands.*; +import frc.team3128.PositionConstants.Position; import frc.team3128.common.narwhaldashboard.NarwhalDashboard; import frc.team3128.subsystems.Swerve; +import static frc.team3128.commands.CmdManager.*; /** * Class to store information about autonomous routines. @@ -50,10 +54,17 @@ public Command getAutonomousCommand() { String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard SmartDashboard.putString(selectedAutoName, selectedAutoName); if (selectedAutoName == null) { - return null; + return score(Position.HIGH_CONE, true).beforeStarting(resetAuto()); } - return Trajectories.get(selectedAutoName,selectedAutoName.contains("Climb")); + return Trajectories.get(selectedAutoName,selectedAutoName.contains("Climb")).beforeStarting(resetAuto()); + } + + public CommandBase resetAuto() { + return sequence( + resetAll(), + retract(Position.NEUTRAL) + ); } // /** diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 7757841..846a704 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -156,4 +156,11 @@ public static CommandBase resetWrist() { public static CommandBase resetSwerve() { return runOnce(()-> swerve.zeroGyro()); } + + public static CommandBase resetAll() { + return sequence( + resetWrist(), + resetElevator() + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdTrajectory.java b/src/main/java/frc/team3128/commands/CmdTrajectory.java index a19ab11..534740d 100644 --- a/src/main/java/frc/team3128/commands/CmdTrajectory.java +++ b/src/main/java/frc/team3128/commands/CmdTrajectory.java @@ -89,7 +89,7 @@ public void initialize() { trajCommand = generateAuto(); trajCommand.schedule(); CmdSwerveDrive.enabled = false; - endPoint = flip(END_POINTS[index]); + endPoint = allianceFlip(END_POINTS[index]); } @Override diff --git a/src/main/java/frc/team3128/subsystems/Elevator.java b/src/main/java/frc/team3128/subsystems/Elevator.java index 50b38de..a05cb42 100644 --- a/src/main/java/frc/team3128/subsystems/Elevator.java +++ b/src/main/java/frc/team3128/subsystems/Elevator.java @@ -28,6 +28,7 @@ public Elevator() { configMotors(); initShuffleboard(kS, kV, kG); m_controller.setTolerance(ELV_TOLERANCE); + resetEncoder(); } public void startPID(Position position) { diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index 3b9aeb9..61e6365 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -31,7 +31,7 @@ public Manipulator(){ private void configMotor(){ m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); - m_roller.setInverted(true); + m_roller.setInverted(false); m_roller.setNeutralMode(NeutralMode.Brake); } diff --git a/src/main/java/frc/team3128/subsystems/Vision.java b/src/main/java/frc/team3128/subsystems/Vision.java index 186be5f..8934b7b 100644 --- a/src/main/java/frc/team3128/subsystems/Vision.java +++ b/src/main/java/frc/team3128/subsystems/Vision.java @@ -37,6 +37,7 @@ public Vision() { cameras = new HashMap(); cameras.put(FRONT_LEFT.hostname, new NAR_Camera(FRONT_LEFT)); cameras.put(FRONT_RIGHT.hostname, new NAR_Camera(FRONT_RIGHT)); + cameras.put(BACK_RIGHT.hostname, new NAR_Camera(BACK_RIGHT)); } public Pose2d targetPos(String name, Pose2d robotPos) { diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index a8f8a2f..e7b2473 100644 --- a/src/main/java/frc/team3128/subsystems/Wrist.java +++ b/src/main/java/frc/team3128/subsystems/Wrist.java @@ -43,7 +43,7 @@ private void configMotor() { m_wrist.setInverted(false); m_wrist.setIdleMode(IdleMode.kCoast); m_wrist.setSmartCurrentLimit(40); - // resetEncoder(); + resetEncoder(); } @Override @@ -57,7 +57,7 @@ public double getMeasurement() { } public void resetEncoder() { - m_wrist.setSelectedSensorPosition(0); + m_wrist.setSelectedSensorPosition(90 * GEAR_RATIO / ROTATION_TO_DEGREES); } public void set(double power) { From 10bff794433550591ec0824e7ecbac91c4e90b7d Mon Sep 17 00:00:00 2001 From: "Team 3128: Controls" Date: Fri, 13 Oct 2023 18:21:20 -0700 Subject: [PATCH 4/4] Workshop 10/13 --- src/main/java/frc/team3128/RobotContainer.java | 4 ++-- src/main/java/frc/team3128/autonomous/AutoPrograms.java | 4 ++-- src/main/java/frc/team3128/commands/CmdManager.java | 7 ++++--- src/main/java/frc/team3128/subsystems/Manipulator.java | 3 +-- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 9b5cfdf..0e2cc06 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -95,8 +95,8 @@ private void configureButtonBindings() { controller.getButton("B").onTrue(new InstantCommand(()-> swerve.resetEncoders())); controller.getButton("Y").onTrue(runOnce(()-> swerve.throttle = 1)).onFalse(runOnce(()-> swerve.throttle = 0.8)); controller.getButton("Start").onTrue(resetSwerve()); - controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL)); - controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL)); + controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL).andThen(stopManip())); + controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL).andThen(stopManip())); controller.getUpPOVButton().onTrue(runOnce(()-> { CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0; diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index 3100486..ed5bc9c 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -50,8 +50,8 @@ private void initAutoSelector() { } public Command getAutonomousCommand() { - //String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); - String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard + String selectedAutoName = NarwhalDashboard.getSelectedAutoName(); + // String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard SmartDashboard.putString(selectedAutoName, selectedAutoName); if (selectedAutoName == null) { return score(Position.HIGH_CONE, true).beforeStarting(resetAuto()); diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 846a704..ebd6339 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -34,8 +34,9 @@ private static CommandBase score(Position position, int xPos, boolean runImmedia return sequence( runOnce(()-> ENABLE = runImmediately), waitUntil(()-> ENABLE), - either(none(), new CmdTrajectory(xPos), ()-> runImmediately), - waitUntil(()-> ENABLE), + + //either(none(), new CmdTrajectory(xPos), ()-> runImmediately), + //waitUntil(()-> ENABLE), extend(position), waitUntil(()-> !ENABLE), outtake(), @@ -106,7 +107,7 @@ public static CommandBase intake(Boolean cone) { runOnce(()-> manipulator.intake(cone), manipulator), waitSeconds(0.4), waitUntil(()-> manipulator.hasObjectPresent()), - waitSeconds(cone ? 0.15 : 0.15), + waitSeconds(cone ? 0.15 : 0), runOnce(()-> manipulator.stallPower(), manipulator) ); } diff --git a/src/main/java/frc/team3128/subsystems/Manipulator.java b/src/main/java/frc/team3128/subsystems/Manipulator.java index 61e6365..a57d7cd 100644 --- a/src/main/java/frc/team3128/subsystems/Manipulator.java +++ b/src/main/java/frc/team3128/subsystems/Manipulator.java @@ -28,10 +28,9 @@ public Manipulator(){ configMotor(); initShuffleboard(); } - private void configMotor(){ m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); - m_roller.setInverted(false); + m_roller.setInverted(true); m_roller.setNeutralMode(NeutralMode.Brake); }