mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Path stuffs
This commit is contained in:
66945
@@ -167,7 +167,8 @@ public final class Constants {
|
||||
public static final double SHOULDER_MAX_ANGLE = 135;
|
||||
public static final double ELBOW_MAX_ANGLE = 180;
|
||||
|
||||
public static final double GEAR_BOX_RATIO = 144.d * 60.d / 24.d;
|
||||
public static final double ELBOW_GB_RATIO = 1.d;
|
||||
public static final double SHOULDER_GB_RATIO = 1.d;
|
||||
|
||||
public static final double SHOULDER_SOFT_LIMIT_FORWARD = 0;
|
||||
public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0;
|
||||
@@ -199,6 +200,8 @@ public final class Constants {
|
||||
public static final double OPEN_POSITION = 0;
|
||||
public static final double CLOSE_POSITION = 0;
|
||||
|
||||
public static final double THRESHOLD = 0;
|
||||
|
||||
public static final double CALIBRATION_SPEED = 0;
|
||||
|
||||
public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit;
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
// 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.climber;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.Vector2d;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import frc4388.robot.subsystems.Claws;
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
|
||||
public class RunClimberPath extends CommandBase {
|
||||
Climber climber;
|
||||
Claws claws;
|
||||
|
||||
Point[] path;
|
||||
int nextIndex;
|
||||
|
||||
/** Creates a new RunClimberPath. */
|
||||
public RunClimberPath(Point[] _path, Climber _climber, Claws _claws) {
|
||||
path = _path;
|
||||
|
||||
climber = _climber;
|
||||
claws = _claws;
|
||||
addRequirements(climber, claws);
|
||||
}
|
||||
|
||||
// Called when the command is initially scheduled.
|
||||
@Override
|
||||
public void initialize() {
|
||||
claws.setOpen(true);
|
||||
nextIndex = 0;
|
||||
}
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
if(!claws.fullyOpen())
|
||||
return;
|
||||
|
||||
Vector2d vec = new Vector2d(1,1);
|
||||
double mag = vec.magnitude();
|
||||
vec.x /= mag;
|
||||
vec.y /= mag;
|
||||
|
||||
climber.controlWithInput(vec.x * .02, vec.y * .02);
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
@@ -1,5 +1,7 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.nio.file.ClosedWatchServiceException;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.SparkMaxLimitSwitch;
|
||||
@@ -7,6 +9,7 @@ import com.revrobotics.SparkMaxLimitSwitch;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.ClawConstants;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
|
||||
public class Claws extends SubsystemBase {
|
||||
public CANSparkMax m_leftClaw;
|
||||
@@ -112,15 +115,20 @@ public class Claws extends SubsystemBase {
|
||||
m_rightClaw.set(-0.1);
|
||||
}
|
||||
|
||||
m_open = open;
|
||||
// m_open = open;
|
||||
}
|
||||
|
||||
public double[] getOffsets() {
|
||||
return new double[] {m_leftOffset, m_rightOffset};
|
||||
}
|
||||
|
||||
public boolean getOpen() {
|
||||
return m_open;
|
||||
public boolean fullyOpen() {
|
||||
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
|
||||
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
|
||||
|
||||
public boolean fullyClosed() {
|
||||
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
|
||||
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -7,17 +7,21 @@ package frc4388.robot.subsystems;
|
||||
import java.util.HashMap;
|
||||
|
||||
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.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants;
|
||||
import frc4388.robot.Constants.ClimberConstants;
|
||||
|
||||
public class ClimberRewrite extends SubsystemBase {
|
||||
private static HashMap<Double, Double> shoulderFeedMap;
|
||||
private static HashMap<Double, Double> elbowFeedMap;
|
||||
private static double[][] shoulderFeedMap;
|
||||
private static double[][] elbowFeedMap;
|
||||
|
||||
public static void configureFeedMaps() {
|
||||
|
||||
@@ -27,13 +31,12 @@ public class ClimberRewrite extends SubsystemBase {
|
||||
private WPI_TalonFX m_elbow;
|
||||
private WPI_Pigeon2 m_gyro;
|
||||
|
||||
private double shoulderStartPosition;
|
||||
private double elbowStartPosition;
|
||||
private Point tPoint;
|
||||
|
||||
private boolean groundRelative;
|
||||
|
||||
/** Creates a new ClimberRewrite. */
|
||||
public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean groundRelative_) {
|
||||
public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) {
|
||||
m_shoulder = shoulder;
|
||||
m_shoulder.configFactoryDefault();
|
||||
m_shoulder.setNeutralMode(NeutralMode.Brake);
|
||||
@@ -44,8 +47,10 @@ public class ClimberRewrite extends SubsystemBase {
|
||||
|
||||
setClimberGains();
|
||||
|
||||
shoulderStartPosition = m_shoulder.getSelectedSensorPosition();
|
||||
elbowStartPosition = m_elbow.getSelectedSensorPosition();
|
||||
// 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_SOFT_LIMIT_FORWARD);
|
||||
m_elbow.configForwardSoftLimitEnable(false);
|
||||
@@ -57,10 +62,12 @@ public class ClimberRewrite extends SubsystemBase {
|
||||
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
|
||||
m_shoulder.configReverseSoftLimitEnable(false);
|
||||
|
||||
tPoint = new Point();
|
||||
|
||||
if(groundRelative)
|
||||
m_gyro = gyro;
|
||||
|
||||
groundRelative = groundRelative_;
|
||||
groundRelative = _groundRelative;
|
||||
}
|
||||
|
||||
public void setClimberGains() {
|
||||
@@ -86,11 +93,161 @@ public class ClimberRewrite extends SubsystemBase {
|
||||
}
|
||||
|
||||
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);
|
||||
m_elbow.set(elbowOutput);
|
||||
}
|
||||
|
||||
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;
|
||||
tPoint.y += yInput * ClimberConstants.MOVE_SPEED;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
double[] jointAngles = getJointAngles(tPoint, 0.d);
|
||||
setJointAngles(jointAngles);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
* @returns [shoulderAngle, elbowAngle] in radians */
|
||||
public static double[] getJointAngles(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;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user