mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
button box thing hopefully done
This commit is contained in:
aarav18
@@ -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));
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
* <p><p>
|
||||
* 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());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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
|
||||
* <p><p>
|
||||
* 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());
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user