diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 0c6f5f9..8e9b828 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -500,14 +500,14 @@ public static class WristConstants { public static final double kV = 0; public static final double kG = 0.22; - public static final double GEAR_RATIO = 3.0; + public static final double GEAR_RATIO = 90; public static final double ROTATION_TO_DEGREES = 360; public static final double ANGLE_OFFSET = 0; public static final int MIN_ANGLE = -90; - public static final int MAX_ANGLE = 70; + public static final int MAX_ANGLE = 90; public static final double WRIST_TOLERANCE = 0.5; @@ -519,11 +519,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.15; - public static final double STALL_POWER_CUBE = 0.0; + public static final double STALL_POWER_CUBE = 0.90; - public static final double CURRENT_THRESHOLD_CONE = 13; - public static final double CURRENT_THRESHOLD_CUBE = 9; + public static final double CURRENT_THRESHOLD_CONE = 15; + public static final double CURRENT_THRESHOLD_CUBE = 12; } public static class ElevatorConstants { @@ -538,7 +538,7 @@ public static class ElevatorConstants { public static final double kV = 0; public static final double kG = 0.625; - public static final double MIN_DIST = 5; //Ask Charlie + 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; diff --git a/src/main/java/frc/team3128/PositionConstants.java b/src/main/java/frc/team3128/PositionConstants.java index 080b4df..5a002b0 100644 --- a/src/main/java/frc/team3128/PositionConstants.java +++ b/src/main/java/frc/team3128/PositionConstants.java @@ -2,19 +2,19 @@ public class PositionConstants { public static enum Position { - HIGH_CONE(10, 45, true), - HIGH_CUBE(10, 45, false), + HIGH_CONE(53, -10, true), + HIGH_CUBE(50, 15, false), MID_CONE(10, 45, true), - MID_CUBE(10, 45, false), - LOW(10, 45, true), + MID_CUBE(25, 25, false), + LOW(3, 45, true), SHELF_CONE(10, 45, true), SHELF_CUBE(10, 45, false), - CHUTE_CONE(10, 45, true), + CHUTE_CONE(10, 55, true), CHUTE_CUBE(10, 45, false), GROUND_CONE(10, 45, true), - GROUND_CUBE(10, 45, false), + GROUND_CUBE(3, 0, false), NEUTRAL(5, 45, false); diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index 21a29a2..ff5f715 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -110,6 +110,14 @@ private void configureButtonBindings() { rightStick.getButton(3).onTrue(moveElv(-0.4)).onFalse(moveElv(0)); rightStick.getButton(4).onTrue(moveElevator(30)); rightStick.getButton(5).onTrue(resetElevator()); + rightStick.getButton(6).onTrue(moveWri(0.4)).onFalse(moveWri(0)); + rightStick.getButton(7).onTrue(moveWri(-0.4)).onFalse(moveWri(0)); + rightStick.getButton(8).onTrue(moveWrist(30)); + rightStick.getButton(9).onTrue(resetWrist()); + rightStick.getButton(10).onTrue(intake(true)); + rightStick.getButton(11).onTrue(intake(false)); + rightStick.getButton(12).onTrue(outtake()); + rightStick.getButton(13).onTrue(stopManip()); buttonPad.getButton(5).onTrue( diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index 3d42133..5a84f29 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -48,7 +48,7 @@ public void execute() { translation = translation.rotateBy(Rotation2d.fromDegrees(-90)); } - rotation = zAxis.getAsDouble() * maxAngularVelocity * swerve.throttle; + rotation = -zAxis.getAsDouble() * maxAngularVelocity * swerve.throttle; Rotation2d driveAngle = translation.getAngle(); double slowedDist = accelLimiter.calculate(translation.getNorm()); diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java index cf156a9..575c1d5 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.kBrake); m_wrist.setSmartCurrentLimit(40); - resetEncoder(); + // resetEncoder(); } @Override @@ -61,6 +61,7 @@ public void resetEncoder() { } public void set(double power) { + disable(); m_wrist.set(power); }