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] 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) {