From 43e3a14c4df980832d5c034369d9624a4e43d400 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 15:37:01 -0600 Subject: [PATCH] button box thing hopefully done --- .../java/frc4388/robot/RobotContainer.java | 50 ++- .../ClimberCommands/RunClimberPath.java | 6 +- .../robot/commands/RunTurretOrClimber.java | 50 +++ .../frc4388/robot/subsystems/Climber.java | 340 ++++++++++++++++-- .../robot/subsystems/ClimberRewrite.java | 340 ------------------ 5 files changed, 402 insertions(+), 384 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunTurretOrClimber.java delete mode 100644 src/main/java/frc4388/robot/subsystems/ClimberRewrite.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 625b1de..6f4a969 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; -import frc4388.robot.subsystems.ClimberRewrite; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; @@ -110,7 +110,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -149,6 +149,8 @@ public class RobotContainer { .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); public static boolean softLimits = true; + public enum Mode {SHOOTER, CLIMBER}; + public Mode currentMode = Mode.SHOOTER; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -202,13 +204,29 @@ public class RobotContainer { new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + 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(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); - // m_robotTurret.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)); + + // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); // continually sends updates to the Blinkin LED controller to keep the lights on @@ -318,18 +336,18 @@ 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))) - .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) + // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) + // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) + // .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) - .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) - .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) - .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); + // .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) + // .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) + // .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)); diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java index 23f37e6..592d72d 100644 --- a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -10,11 +10,11 @@ import edu.wpi.first.wpilibj.drive.Vector2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.subsystems.Claws; -import frc4388.robot.subsystems.ClimberRewrite; +import frc4388.robot.subsystems.Climber; import frc4388.utility.Vector2D; public class RunClimberPath extends CommandBase { - ClimberRewrite climber; + Climber climber; Claws claws; Point[] path; @@ -23,7 +23,7 @@ public class RunClimberPath extends CommandBase { boolean endPath; /** Creates a new RunClimberPath. */ - public RunClimberPath(Point[] _path, ClimberRewrite _climber, Claws _claws) { + public RunClimberPath(Point[] _path, Climber _climber, Claws _claws) { path = _path; endPath = false; diff --git a/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java b/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java new file mode 100644 index 0000000..c610eda --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.RobotContainer; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Turret; + +public class RunTurretOrClimber extends CommandBase { + + Turret turret; + Climber climber; + + /** Creates a new RunTurretOrClimber. */ + public RunTurretOrClimber(Turret turret, Climber climber) { + // Use addRequirements() here to declare subsystem dependencies. + + this.turret = turret; + this.climber = climber; + + addRequirements(this.turret, this.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // if (RobotContainer.currentMode.equals(RobotContainer.Mode.TURRET)) { + // this.turret.runTurretWithInput(getOperatorController().getLeftX()) + // } else if (RobotContainer.currentMode.equals(RobotContainer.Mode.CLIMBER)) { + + // } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 7425346..07870ff 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,47 +4,337 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.revrobotics.CANSparkMax; +import java.util.HashMap; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + +import org.opencv.core.Point; + +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; +import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { + private static double[][] shoulderFeedMap; + private static double[][] elbowFeedMap; - WPI_TalonFX m_climberShoulder; - WPI_TalonFX m_climberElbow; - - /** Creates a new Climber. */ - public Climber(WPI_TalonFX climberShoulder, WPI_TalonFX climberElbow) { - m_climberShoulder = climberShoulder; - m_climberElbow = climberElbow; - - m_climberShoulder.configFactoryDefault(); - m_climberElbow.configFactoryDefault(); - m_climberShoulder.setNeutralMode(NeutralMode.Brake); - m_climberElbow.setNeutralMode(NeutralMode.Brake); + public static void configureFeedMaps() { + } - public void runShoulderWithInput(double input) { - m_climberShoulder.set(input); + private WPI_TalonFX m_shoulder; + private WPI_TalonFX m_elbow; + private WPI_Pigeon2 m_gyro; + + private Point tPoint; + + private boolean groundRelative; + private double shoulderSpeedLimiter; + private double elbowSpeedLimiter; + + /** Creates a new ClimberRewrite. */ + public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) { + m_shoulder = shoulder; + m_shoulder.configFactoryDefault(); + m_shoulder.setNeutralMode(NeutralMode.Brake); + + m_elbow = elbow; + m_elbow.configFactoryDefault(); + m_elbow.setNeutralMode(NeutralMode.Brake); + + // setClimberGains(); + + // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); + // elbowStartPosition = m_elbow.getSelectedSensorPosition(); + // m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); + // m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); + + m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); + m_elbow.configForwardSoftLimitEnable(true); + // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); + // m_elbow.configReverseSoftLimitEnable(true); + + m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); + m_shoulder.configForwardSoftLimitEnable(true); + m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); + m_shoulder.configReverseSoftLimitEnable(false); + + m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + m_elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + m_shoulder.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + + m_shoulder.overrideLimitSwitchesEnable(true); + m_elbow.overrideLimitSwitchesEnable(true); + + tPoint = new Point(); + + if(groundRelative) + m_gyro = gyro; + + groundRelative = _groundRelative; + this.elbowSpeedLimiter = 1.0; } - public void runElbowWithInput(double input){ - m_climberElbow.set(input); + public void setClimberGains() { + m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); + + m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); } - public void runBothMotorsWithInputs(double shoulderInput, double elbowInput) { - m_climberShoulder.set(shoulderInput); - m_climberElbow.set(elbowInput); + public void setClimberFeedForward(double shoulderF, double elbowF) { + m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, shoulderF, ClimberConstants.CLIMBER_TIMEOUT_MS); + + m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, elbowF, ClimberConstants.CLIMBER_TIMEOUT_MS); + } + + public void compensateFeedForArmAngles() { + double shoulderPosition = m_shoulder.getSelectedSensorPosition(); + double elbowPosition = m_elbow.getSelectedSensorPosition(); + + double shoulderFeed = 0; + double elbowFeed = 0; + + for(int i = 1; i < shoulderFeedMap.length; i++) { + if(shoulderFeedMap[i][0] > shoulderPosition) { + double percentDifference = (shoulderPosition - shoulderFeedMap[i-1][0]) / (shoulderFeedMap[i][0] - shoulderFeedMap[i-1][0]); + double feedDifference = shoulderFeedMap[i][1] - shoulderFeedMap[i-1][1]; + shoulderFeed = (percentDifference * feedDifference) + shoulderFeedMap[i-1][1]; + } + } + + for(int i = 1; i < elbowFeedMap.length; i++) { + if(elbowFeedMap[i][0] > elbowPosition) { + double percentDifference = (elbowPosition - elbowFeedMap[i-1][0]) / (elbowFeedMap[i][0] - elbowFeedMap[i-1][0]); + double feedDifference = elbowFeedMap[i][1] - elbowFeedMap[i-1][1]; + elbowFeed = (percentDifference * feedDifference) + elbowFeedMap[i-1][1]; + } + } + + setClimberFeedForward(shoulderFeed, elbowFeed); + } + + public void setMotors(double shoulderOutput, double elbowOutput) { + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); + } + + public double[] getJointAngles() { + return new double[] { + (m_shoulder.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.SHOULDER_GB_RATIO, + (m_elbow.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.ELBOW_GB_RATIO + }; + } + + public void setJointAngles(double[] angles) { + System.out.println(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_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); + + 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 += m_shoulderOffset; + // elbowPosition += m_elbowOffset; + + m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); + m_elbow.set(TalonFXControlMode.Position, elbowPosition); + } + + public void controlWithInput(double xInput, double yInput) { + tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02; + tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; } @Override public void periodic() { - // This method will be called once per scheduler run + SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); + SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); + // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); + // setJointAngles(jointAngles); + + // * 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(); + double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); + double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); + + if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { + this.elbowSpeedLimiter = 1.0; + } + + // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). + double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); + double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); + double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); + + if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { + this.shoulderSpeedLimiter = 1.0; + } } - public double getCurrent() { - return m_climberElbow.getSupplyCurrent(); + /** + * Gets joint angles for climber arm + * {@code targetPoint.x} and {@code targetPoint.y} are set in the xy plane of the climber, accounting for the + * rotation of the robot. Both parameters should be in cm. + * Does not set the motors automatically + *

+ * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 + * + * @param targetPoint Target xy point for the climber arm to go to + * @param tiltAngle The tilt of the robot + * @return [shoulderAngle, elbowAngle] in radians */ + public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) { + double [] angles = new double[2]; + + double mag = Math.hypot(targetPoint.x, targetPoint.y); + if(mag >= ClimberConstants.MAX_ARM_LENGTH) { + targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH; + targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH; + mag = ClimberConstants.MAX_ARM_LENGTH; + } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) { + targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH; + targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH; + mag = ClimberConstants.MIN_ARM_LENGTH; + } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { + targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; + targetPoint.y = 0.d; + mag = ClimberConstants.MIN_ARM_LENGTH; + } + + // The angle to the target point + double theta; + if(targetPoint.x == 0.d) { + theta = Math.PI/2.d; // TODO rename variable + } else { + theta = Math.atan(targetPoint.y / targetPoint.x); + } + theta += tiltAngle; + + if(targetPoint.x < 0.d) + theta += Math.PI; + + + // Correct target position for tilt + targetPoint.x = Math.cos(theta) * mag; + targetPoint.y = Math.sin(theta) * mag; + + // Law and Order: Cosines edition + double shoulderAngle; + if(mag == 0) { + shoulderAngle = 0; + } else { + shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / + (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag)); + } + shoulderAngle = theta - shoulderAngle; + + double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) / + (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); + //elbowAngle = Math.PI - elbowAngle; + // System.out.println("sa1: " + shoulderAngle); + // System.out.println("ea1: " + elbowAngle); + + // If the climber is resting on the robot base, rotate the upper arm in the direction of the target + if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { + shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; + double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; + // System.out.println("xDiff: " + xDiff); + + elbowAngle = Math.atan(-targetPoint.y / xDiff); + // System.out.println("ea2: " + elbowAngle); + if(elbowAngle < 0) { + elbowAngle = Math.PI - Math.abs(elbowAngle); + } + + if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) + elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; + // System.out.println("ea3: " + elbowAngle); + + // Deal with negative wraparound + // if(xDiff >= 0.d) + // elbowAngle += Math.PI; + // System.out.println("ea4: " + elbowAngle); + } + + angles[0] = shoulderAngle; + angles[1] = elbowAngle; + return angles; } + + public static Point getClimberPosition(double shoulderAngle, double elbowAngle) { + Point climberPoint = new Point(0, 0); + + climberPoint.x += Math.sin(shoulderAngle); + climberPoint.y += Math.cos(shoulderAngle); + + climberPoint.x -= Math.sin(elbowAngle - shoulderAngle); + climberPoint.y += Math.cos(elbowAngle - shoulderAngle); + + return climberPoint; + } + + public static Point getClimberPosition(double[] jointAngles) { + return getClimberPosition(jointAngles[0], jointAngles[1]); + } + + public void setClimberSoftLimits(boolean set){ + m_elbow.configForwardSoftLimitEnable(set); + m_shoulder.configForwardSoftLimitEnable(set); + } + + public void setEncoders(double value){ + m_elbow.setSelectedSensorPosition(value); + m_shoulder.setSelectedSensorPosition(value); + } + + public double getCurrent(){ + return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); + } + } diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java deleted file mode 100644 index 78cc123..0000000 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ /dev/null @@ -1,340 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.subsystems; - -import java.util.HashMap; - -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.WPI_Pigeon2; - -import org.opencv.core.Point; - -import edu.wpi.first.wpilibj.motorcontrol.Talon; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; -import frc4388.robot.Constants.ClimberConstants; - -public class ClimberRewrite extends SubsystemBase { - private static double[][] shoulderFeedMap; - private static double[][] elbowFeedMap; - - public static void configureFeedMaps() { - - } - - private WPI_TalonFX m_shoulder; - private WPI_TalonFX m_elbow; - private WPI_Pigeon2 m_gyro; - - private Point tPoint; - - private boolean groundRelative; - private double shoulderSpeedLimiter; - private double elbowSpeedLimiter; - - /** Creates a new ClimberRewrite. */ - public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) { - m_shoulder = shoulder; - m_shoulder.configFactoryDefault(); - m_shoulder.setNeutralMode(NeutralMode.Brake); - - m_elbow = elbow; - m_elbow.configFactoryDefault(); - m_elbow.setNeutralMode(NeutralMode.Brake); - - // setClimberGains(); - - // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); - // elbowStartPosition = m_elbow.getSelectedSensorPosition(); - // m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); - // m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); - - m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); - m_elbow.configForwardSoftLimitEnable(true); - // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); - // m_elbow.configReverseSoftLimitEnable(true); - - m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - m_shoulder.configForwardSoftLimitEnable(true); - m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - m_shoulder.configReverseSoftLimitEnable(false); - - m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - m_elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - m_shoulder.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - - m_shoulder.overrideLimitSwitchesEnable(true); - m_elbow.overrideLimitSwitchesEnable(true); - - tPoint = new Point(); - - if(groundRelative) - m_gyro = gyro; - - groundRelative = _groundRelative; - this.elbowSpeedLimiter = 1.0; - } - - public void setClimberGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - } - - public void setClimberFeedForward(double shoulderF, double elbowF) { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, shoulderF, ClimberConstants.CLIMBER_TIMEOUT_MS); - - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, elbowF, ClimberConstants.CLIMBER_TIMEOUT_MS); - } - - public void compensateFeedForArmAngles() { - double shoulderPosition = m_shoulder.getSelectedSensorPosition(); - double elbowPosition = m_elbow.getSelectedSensorPosition(); - - double shoulderFeed = 0; - double elbowFeed = 0; - - for(int i = 1; i < shoulderFeedMap.length; i++) { - if(shoulderFeedMap[i][0] > shoulderPosition) { - double percentDifference = (shoulderPosition - shoulderFeedMap[i-1][0]) / (shoulderFeedMap[i][0] - shoulderFeedMap[i-1][0]); - double feedDifference = shoulderFeedMap[i][1] - shoulderFeedMap[i-1][1]; - shoulderFeed = (percentDifference * feedDifference) + shoulderFeedMap[i-1][1]; - } - } - - for(int i = 1; i < elbowFeedMap.length; i++) { - if(elbowFeedMap[i][0] > elbowPosition) { - double percentDifference = (elbowPosition - elbowFeedMap[i-1][0]) / (elbowFeedMap[i][0] - elbowFeedMap[i-1][0]); - double feedDifference = elbowFeedMap[i][1] - elbowFeedMap[i-1][1]; - elbowFeed = (percentDifference * feedDifference) + elbowFeedMap[i-1][1]; - } - } - - setClimberFeedForward(shoulderFeed, elbowFeed); - } - - public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); - } - - public double[] getJointAngles() { - return new double[] { - (m_shoulder.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.SHOULDER_GB_RATIO, - (m_elbow.getSelectedSensorPosition() * Math.PI) / (Constants.TICKS_PER_ROTATION_FX/2.d) / ClimberConstants.ELBOW_GB_RATIO - }; - } - - public void setJointAngles(double[] angles) { - System.out.println(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_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); - - 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 += m_shoulderOffset; - // elbowPosition += m_elbowOffset; - - m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); - m_elbow.set(TalonFXControlMode.Position, elbowPosition); - } - - public void controlWithInput(double xInput, double yInput) { - tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02; - tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; - } - - @Override - public void periodic() { - SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); - SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); - // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - // setJointAngles(jointAngles); - - // * 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(); - double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); - double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); - - if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { - this.elbowSpeedLimiter = 1.0; - } - - // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). - double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); - double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - - if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { - this.shoulderSpeedLimiter = 1.0; - } - } - - /** - * Gets joint angles for climber arm - * {@code targetPoint.x} and {@code targetPoint.y} are set in the xy plane of the climber, accounting for the - * rotation of the robot. Both parameters should be in cm. - * Does not set the motors automatically - *

- * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 - * - * @param targetPoint Target xy point for the climber arm to go to - * @param tiltAngle The tilt of the robot - * @return [shoulderAngle, elbowAngle] in radians */ - public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) { - double [] angles = new double[2]; - - double mag = Math.hypot(targetPoint.x, targetPoint.y); - if(mag >= ClimberConstants.MAX_ARM_LENGTH) { - targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH; - targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH; - mag = ClimberConstants.MAX_ARM_LENGTH; - } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) { - targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH; - targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH; - mag = ClimberConstants.MIN_ARM_LENGTH; - } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { - targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; - targetPoint.y = 0.d; - mag = ClimberConstants.MIN_ARM_LENGTH; - } - - // The angle to the target point - double theta; - if(targetPoint.x == 0.d) { - theta = Math.PI/2.d; // TODO rename variable - } else { - theta = Math.atan(targetPoint.y / targetPoint.x); - } - theta += tiltAngle; - - if(targetPoint.x < 0.d) - theta += Math.PI; - - - // Correct target position for tilt - targetPoint.x = Math.cos(theta) * mag; - targetPoint.y = Math.sin(theta) * mag; - - // Law and Order: Cosines edition - double shoulderAngle; - if(mag == 0) { - shoulderAngle = 0; - } else { - shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / - (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag)); - } - shoulderAngle = theta - shoulderAngle; - - double elbowAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2) - Math.pow(mag, 2)) / - (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); - //elbowAngle = Math.PI - elbowAngle; - // System.out.println("sa1: " + shoulderAngle); - // System.out.println("ea1: " + elbowAngle); - - // If the climber is resting on the robot base, rotate the upper arm in the direction of the target - if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { - shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; - double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; - // System.out.println("xDiff: " + xDiff); - - elbowAngle = Math.atan(-targetPoint.y / xDiff); - // System.out.println("ea2: " + elbowAngle); - if(elbowAngle < 0) { - elbowAngle = Math.PI - Math.abs(elbowAngle); - } - - if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) - elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; - // System.out.println("ea3: " + elbowAngle); - - // Deal with negative wraparound - // if(xDiff >= 0.d) - // elbowAngle += Math.PI; - // System.out.println("ea4: " + elbowAngle); - } - - angles[0] = shoulderAngle; - angles[1] = elbowAngle; - return angles; - } - - public static Point getClimberPosition(double shoulderAngle, double elbowAngle) { - Point climberPoint = new Point(0, 0); - - climberPoint.x += Math.sin(shoulderAngle); - climberPoint.y += Math.cos(shoulderAngle); - - climberPoint.x -= Math.sin(elbowAngle - shoulderAngle); - climberPoint.y += Math.cos(elbowAngle - shoulderAngle); - - return climberPoint; - } - - public static Point getClimberPosition(double[] jointAngles) { - return getClimberPosition(jointAngles[0], jointAngles[1]); - } - - public void setClimberSoftLimits(boolean set){ - m_elbow.configForwardSoftLimitEnable(set); - m_shoulder.configForwardSoftLimitEnable(set); - } - - public void setEncoders(double value){ - m_elbow.setSelectedSensorPosition(value); - m_shoulder.setSelectedSensorPosition(value); - } - - public double getCurrent(){ - return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); - } - -}