From b5345a53cbc55dd44370a9b6f6613b5c720c828f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 6 Mar 2022 12:57:41 -0700 Subject: [PATCH] climber default command not IK --- src/main/java/frc4388/robot/RobotContainer.java | 9 +++++++-- src/main/java/frc4388/robot/subsystems/Climber.java | 12 ++++++++---- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c4ae0a2..865208a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,8 +137,13 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), - getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), getOperatorController().getLeftY()), + m_robotClimber)); + + // IK command + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), + // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Turret default command //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index d952561..23478db 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -22,8 +22,8 @@ import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { - private WPI_TalonFX m_shoulder; - private WPI_TalonFX m_elbow; + public WPI_TalonFX m_shoulder; + public WPI_TalonFX m_elbow; private double m_shoulderOffset; private double m_elbowOffset; @@ -33,8 +33,6 @@ public class Climber extends SubsystemBase { private double m_robotAngle; private double m_robotPosition; - - private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { @@ -142,6 +140,12 @@ public class Climber extends SubsystemBase { angles[1] = elbowAngle; return angles; } + +public void setMotors(double shoulderOutput, double elbowOutput) { + m_shoulder.set(shoulderOutput); + m_elbow.set(elbowOutput); +} + /* Rotation Matrix R = [cos(0) -sin(0) \n sin(0) cos(0)] Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)]