From f2b8b915dc92679bde47e4d75868a82748f50750 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Mon, 14 Mar 2022 20:11:07 -0600 Subject: [PATCH] Started rewrite --- .../frc4388/robot/subsystems/Climber.java | 45 ++++----- .../robot/subsystems/ClimberRewrite.java | 96 +++++++++++++++++++ 2 files changed, 119 insertions(+), 22 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/ClimberRewrite.java diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 9ad1f97..42b5886 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -15,6 +15,8 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import org.opencv.core.Point; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -34,7 +36,7 @@ public class Climber extends SubsystemBase { private double m_robotAngle; private double m_robotPosition; - private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; + private Point m_position = new Point(ClimberConstants.MIN_ARM_LENGTH, 0.d); public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { m_shoulder = shoulder; @@ -68,46 +70,46 @@ public class Climber extends SubsystemBase { /* getJointAngles * Gets joint angles for climber arm - * xTarget and yTarget are set in the xy plane of the climber, accounting for the + * targetPoint.x and targetPoint.y are set in the xy plane of the climber, accounting for the * rotation of the robot. Both parameters should be in cm. * returns [shoulderAngle, elbowAngle] in radians * Does not set the motors automatically * * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */ - public static double[] getJointAngles(double xTarget, double yTarget, double tiltAngle) { + public static double[] getJointAngles(Point targetPoint, double tiltAngle) { double [] angles = new double[2]; - double mag = Math.hypot(xTarget, yTarget); + double mag = Math.hypot(targetPoint.x, targetPoint.y); if(mag >= ClimberConstants.MAX_ARM_LENGTH) { - xTarget = (xTarget / mag) * ClimberConstants.MAX_ARM_LENGTH; - yTarget = (yTarget / 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) { - xTarget = (xTarget / mag) * ClimberConstants.MIN_ARM_LENGTH; - yTarget = (yTarget / mag) * ClimberConstants.MIN_ARM_LENGTH; + 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) { - xTarget = ClimberConstants.MIN_ARM_LENGTH; - yTarget = 0.d; + targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; + targetPoint.y = 0.d; mag = ClimberConstants.MIN_ARM_LENGTH; } // The angle to the target point double theta; - if(xTarget == 0.d) { + if(targetPoint.x == 0.d) { theta = Math.PI/2.d; // TODO rename variable } else { - theta = Math.atan(yTarget / xTarget); + theta = Math.atan(targetPoint.y / targetPoint.x); } theta += tiltAngle; - if(xTarget < 0.d) + if(targetPoint.x < 0.d) theta += Math.PI; // Correct target position for tilt - xTarget = Math.cos(theta) * mag; - yTarget = Math.sin(theta) * mag; + targetPoint.x = Math.cos(theta) * mag; + targetPoint.y = Math.sin(theta) * mag; // Law and Order: Cosines edition double shoulderAngle; @@ -128,10 +130,10 @@ public class Climber extends SubsystemBase { // 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 = xTarget - ClimberConstants.LOWER_ARM_LENGTH; + double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; // System.out.println("xDiff: " + xDiff); - elbowAngle = Math.atan(-yTarget / xDiff); + elbowAngle = Math.atan(-targetPoint.y / xDiff); // System.out.println("ea2: " + elbowAngle); if(elbowAngle < 0) { elbowAngle = Math.PI - Math.abs(elbowAngle); @@ -234,10 +236,10 @@ public void setMotors(double shoulderOutput, double elbowOutput) { } public void controlWithInput(double xInput, double yInput) { - m_position[0] += xInput * ClimberConstants.MOVE_SPEED; - m_position[1] += yInput * ClimberConstants.MOVE_SPEED; + m_position.x += xInput * ClimberConstants.MOVE_SPEED; + m_position.y += yInput * ClimberConstants.MOVE_SPEED; - System.out.println("x: " + m_position[0] + " y: " + m_position[1]); + System.out.println("x: " + m_position.x + " y: " + m_position.y); double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; @@ -259,7 +261,7 @@ public void setMotors(double shoulderOutput, double elbowOutput) { // double[] testAngles6 = getJointAngles(60, 25, 0); // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); - double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); + double[] jointAngles = getJointAngles(m_position, tiltAngle); setJointAngles(jointAngles); } @@ -274,5 +276,4 @@ public void setMotors(double shoulderOutput, double elbowOutput) { SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); } - } diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java new file mode 100644 index 0000000..be1e764 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -0,0 +1,96 @@ +// 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.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + +import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimberConstants; + +public class ClimberRewrite extends SubsystemBase { + private static HashMap shoulderFeedMap; + private static HashMap elbowFeedMap; + + public static void configureFeedMaps() { + + } + + private WPI_TalonFX m_shoulder; + private WPI_TalonFX m_elbow; + private WPI_Pigeon2 m_gyro; + + private double shoulderStartPosition; + private double elbowStartPosition; + + private boolean groundRelative; + + /** 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_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); + m_elbow.configForwardSoftLimitEnable(false); + m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); + m_elbow.configReverseSoftLimitEnable(false); + + m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); + m_shoulder.configForwardSoftLimitEnable(false); + m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); + m_shoulder.configReverseSoftLimitEnable(false); + + if(groundRelative) + m_gyro = gyro; + + groundRelative = groundRelative_; + } + + 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() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +}