diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4a04665..d409cae 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -185,7 +185,7 @@ public final class Constants { public static final double MAX_ARM_LENGTH = 53; public static final double MIN_ARM_LENGTH = 1; - public static final double MOVE_SPEED = .1; // cm per second + public static final double MOVE_SPEED = 30000; // ticks per second public static final double SHOULDER_RESTING_ANGLE = 0; public static final double ELBOW_RESTING_ANGLE = 0; @@ -208,8 +208,8 @@ public final class Constants { public static final int ELBOW_SLOT_IDX = 0; public static final int ELBOW_PID_LOOP_IDX = 1; - public static final Gains SHOULDER_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains ELBOW_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOULDER_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.2); + public static final Gains ELBOW_GAINS = new Gains(0.5, 0.0, 30.0, 0.0, 0, 1.0); //Prev P 1 d 0.4 public static final int CLIMBER_TIMEOUT_MS = 50; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6f4a969..ce1e39f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -166,9 +166,9 @@ public class RobotContainer { // IK command - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), - // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), + getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( @@ -208,23 +208,24 @@ public class RobotContainer { // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotTurret.setDefaultCommand( - new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - }, m_robotTurret)); + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + // if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + // }, m_robotTurret)); - m_robotHood.setDefaultCommand( - new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); } - }, m_robotHood)); + // m_robotHood.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + // if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); } + // }, m_robotHood)); m_robotClimber.setDefaultCommand( - new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); } - }, m_robotClimber)); + // new RunCommand(() -> { + // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), + getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} + // }, m_robotClimber)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -336,7 +337,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) @@ -349,8 +350,8 @@ public class RobotContainer { // .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); - // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) - // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + .whenPressed(new InstantCommand(() -> this.currentMode = Mode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentMode = Mode.SHOOTER)); // .whenReleased(EnableClimber())); // control turret manual mode diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 07870ff..2300c8e 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,6 +4,7 @@ package frc4388.robot.subsystems; +import java.util.Arrays; import java.util.HashMap; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; @@ -35,6 +36,7 @@ public class Climber extends SubsystemBase { private WPI_Pigeon2 m_gyro; private Point tPoint; + private double[] tJointAngles; private boolean groundRelative; private double shoulderSpeedLimiter; @@ -50,7 +52,9 @@ public class Climber extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - // setClimberGains(); + tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; + + setClimberGains(); // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); // elbowStartPosition = m_elbow.getSelectedSensorPosition(); @@ -144,44 +148,53 @@ public class Climber extends SubsystemBase { } public void setJointAngles(double[] angles) { - System.out.println(angles); + System.out.println(Arrays.toString(angles)); setJointAngles(angles[0], angles[1]); } public void setJointAngles(double shoulderAngle, double elbowAngle) { - shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle; - elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle; + // shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle; + // elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle; - shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle; - elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; + // shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle; + // elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; - // Convert radians to ticks - System.out.println("angles: " + shoulderAngle + ", " + elbowAngle); + // // Convert radians to ticks + SmartDashboard.putString("angles", shoulderAngle + ", " + elbowAngle); - double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; - double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; + // double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; + // double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; - shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO; - elbowPosition *= ClimberConstants.ELBOW_GB_RATIO; + // shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO; + // elbowPosition *= ClimberConstants.ELBOW_GB_RATIO; // shoulderPosition += m_shoulderOffset; // elbowPosition += m_elbowOffset; - m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); - m_elbow.set(TalonFXControlMode.Position, elbowPosition); + m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); + m_elbow.set(TalonFXControlMode.Position, elbowAngle); } - public void controlWithInput(double xInput, double yInput) { + public void controlPointWithInput(double xInput, double yInput) { tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02; tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; } + public void controlJointsWithInput(double shoulderInput, double elbowInput) { + tJointAngles[0] += shoulderInput * ClimberConstants.MOVE_SPEED * .02; + tJointAngles[1] += elbowInput * ClimberConstants.MOVE_SPEED * .02; + } + + int pCount = 0; @Override public void periodic() { SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - // setJointAngles(jointAngles); + if(pCount % 1 == 0) + setJointAngles(tJointAngles); + + pCount ++; // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). double currentElbowPos = this.m_elbow.getSelectedSensorPosition();