Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Started rewrite

Browse Source
This commit is contained in:
66945
2022-03-14 20:11:07 -06:00
parent 9a0e3f66d2
commit f2b8b915dc
2 changed files with 119 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Climber.java
+23 -22
View File
@@ -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());
}
}
src/main/java/frc4388/robot/subsystems/ClimberRewrite.java
+96
View File
@@ -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<Double, Double> shoulderFeedMap;
private static HashMap<Double, Double> 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
}
}