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

Merge branch 'Climber' of https://github.com/Team4388/2022NoWayHome into Climber

Browse Source
This commit is contained in:
66945
2022-03-16 14:50:07 -06:00
parent a71fda4549 633a443194
commit d572aebc60
6 changed files with 292 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+3 -3
View File
@@ -197,10 +197,10 @@ public final class Constants {
public static final int LEFT_CLAW_ID = 44; public static final int LEFT_CLAW_ID = 44;
public static final int RIGHT_CLAW_ID = 45; public static final int RIGHT_CLAW_ID = 45;
public static final double OPEN_POSITION = 0; public static final double OPEN_POSITION = 0; // TODO: find actual position
public static final double CLOSE_POSITION = 0; public static final double CLOSE_POSITION = 0; // TODO: find actual position
public static final double THRESHOLD = 0; public static final double THRESHOLD = 0; // TODO: find actual threshold
public static final double CALIBRATION_SPEED = 0; public static final double CALIBRATION_SPEED = 0;
src/main/java/frc4388/robot/RobotContainer.java
+2 -2
View File
@@ -57,7 +57,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*; import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.Claws; import frc4388.robot.subsystems.Claws;
import frc4388.robot.commands.RunClaw; import frc4388.robot.commands.RunClaw;
import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.ClimberRewrite;
import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.subsystems.Claws.ClawType;
import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.AimToCenter;
import frc4388.robot.commands.Shoot; import frc4388.robot.commands.Shoot;
@@ -92,7 +92,7 @@ public class RobotContainer {
private final RobotMap m_robotMap = new RobotMap(); private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */ /* Subsystems */
private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); private final ClimberRewrite m_robotClimber= new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false);
private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
src/main/java/frc4388/robot/RobotMap.java
+6 -3
View File
@@ -8,6 +8,7 @@ package frc4388.robot;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.NeutralMode;
@@ -63,7 +64,7 @@ public class RobotMap {
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID);
public SwerveModule leftFront; public SwerveModule leftFront;
public SwerveModule leftBack; public SwerveModule leftBack;
@@ -171,8 +172,10 @@ public class RobotMap {
public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO
/* Hooks Subsystem */ /* Hooks Subsystem */
public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless); // public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless);
public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless); // public final CANSparkMax rightClaw = new CANSparkMax(ClawConstants.RIGHT_CLAW_ID, MotorType.kBrushless);
public final Servo leftClaw = new Servo(1); // TODO: find actual channel
public final Servo rightClaw = new Servo(1); // TODO: find actual channel
// Shooter Config // Shooter Config
/* Boom Boom Subsystem */ /* Boom Boom Subsystem */
src/main/java/frc4388/robot/commands/RunClaw.java
+2 -1
View File
@@ -47,6 +47,7 @@ public class RunClaw extends CommandBase {
// Returns true when the command should end. // Returns true when the command should end.
@Override @Override
public boolean isFinished() { public boolean isFinished() {
return m_claws.checkSwitchAndCurrent(clawType); // return m_claws.checkSwitchAndCurrent(clawType);
return false; // TODO: real return
} }
} }
src/main/java/frc4388/robot/subsystems/Claws.java
+58 -57
View File
@@ -7,18 +7,23 @@ import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxLimitSwitch;
import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.Constants.ClimberConstants;
public class Claws extends SubsystemBase { public class Claws extends SubsystemBase {
public CANSparkMax m_leftClaw;
public CANSparkMax m_rightClaw;
private SparkMaxLimitSwitch m_leftLimitSwitchF; public Servo m_leftClaw;
private SparkMaxLimitSwitch m_rightLimitSwitchF; public Servo m_rightClaw;
private SparkMaxLimitSwitch m_leftLimitSwitchR;
private SparkMaxLimitSwitch m_rightLimitSwitchR; // public CANSparkMax m_leftClaw;
// public CANSparkMax m_rightClaw;
// private SparkMaxLimitSwitch m_leftLimitSwitchF;
// private SparkMaxLimitSwitch m_rightLimitSwitchF;
// private SparkMaxLimitSwitch m_leftLimitSwitchR;
// private SparkMaxLimitSwitch m_rightLimitSwitchR;
private double m_leftOffset; private double m_leftOffset;
private double m_rightOffset; private double m_rightOffset;
@@ -26,21 +31,22 @@ public class Claws extends SubsystemBase {
private boolean m_open; private boolean m_open;
public static enum ClawType {LEFT, RIGHT} public static enum ClawType {LEFT, RIGHT}
public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) { public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) {
m_leftClaw = leftClaw; m_leftClaw = leftClaw;
m_rightClaw = rightClaw; m_rightClaw = rightClaw;
m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); // m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); // m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol // m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); // m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_leftLimitSwitchF.enableLimitSwitch(true); // m_leftLimitSwitchF.enableLimitSwitch(true);
m_rightLimitSwitchF.enableLimitSwitch(true); // m_rightLimitSwitchF.enableLimitSwitch(true);
m_leftLimitSwitchR.enableLimitSwitch(true); // m_leftLimitSwitchR.enableLimitSwitch(true);
m_rightLimitSwitchR.enableLimitSwitch(true); // m_rightLimitSwitchR.enableLimitSwitch(true);
leftClaw.setInverted(true);
rightClaw.setInverted(true); // m_leftClaw.setInverted(true);
// m_rightClaw.setInverted(true);
m_open = false; m_open = false;
@@ -48,11 +54,6 @@ public class Claws extends SubsystemBase {
// m_rightClaw.set(ClawConstants.CALIBRATION_SPEED); // m_rightClaw.set(ClawConstants.CALIBRATION_SPEED);
} }
public void setSpeed(double speed){
m_leftClaw.set(speed);
m_rightClaw.set(speed);
}
/** /**
* Run a specific claw to open or close. * Run a specific claw to open or close.
* @param which Which claw to run. * @param which Which claw to run.
@@ -66,38 +67,33 @@ public class Claws extends SubsystemBase {
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset;
// m_leftClaw.getEncoder().setPosition(setPos); // m_leftClaw.getEncoder().setPosition(setPos);
m_leftClaw.set(direction * 0.1); m_leftClaw.setSpeed(direction * 0.1);
} else if (which == Claws.ClawType.RIGHT) { } else if (which == Claws.ClawType.RIGHT) {
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset;
// m_rightClaw.getEncoder().setPosition(setPos); // m_rightClaw.getEncoder().setPosition(setPos);
m_rightClaw.set(direction * 0.1); m_rightClaw.setSpeed(direction * 0.1);
} }
m_open = open; m_open = open;
} }
public void runClaw(ClawType which, double input) { public void runClaw(ClawType which, double input) {
if (which == Claws.ClawType.LEFT) { if (which == Claws.ClawType.LEFT) {
m_leftClaw.set(input); m_leftClaw.setSpeed(input);
} else if (which == Claws.ClawType.RIGHT) { } else if (which == Claws.ClawType.RIGHT) {
m_rightClaw.set(input); m_rightClaw.setSpeed(input);
} }
} }
public void runClaws(double input) public void runClaws(double input)
{ {
m_leftClaw.set(input); m_leftClaw.setSpeed(input);
m_rightClaw.set(input); m_rightClaw.setSpeed(input);
} }
/** /**
* Sets the state of both hooks * Sets the state of both hooks
* @param open The state of the hooks * @param open The state of the hooks
@@ -114,8 +110,6 @@ public class Claws extends SubsystemBase {
m_leftClaw.set(-0.1); m_leftClaw.set(-0.1);
m_rightClaw.set(-0.1); m_rightClaw.set(-0.1);
} }
// m_open = open;
} }
public double[] getOffsets() { public double[] getOffsets() {
@@ -123,12 +117,12 @@ public class Claws extends SubsystemBase {
} }
public boolean fullyOpen() { public boolean fullyOpen() {
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
public boolean fullyClosed() { public boolean fullyClosed() {
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
} }
/** /**
@@ -137,26 +131,33 @@ public class Claws extends SubsystemBase {
* @param limit The current limit. * @param limit The current limit.
* @return Whether to interrupt the RunClaw command or not. * @return Whether to interrupt the RunClaw command or not.
*/ */
public boolean checkSwitchAndCurrent(ClawType which) { // public boolean checkSwitchAndCurrent(ClawType which) {
if (which == ClawType.LEFT) { // if (which == ClawType.LEFT) {
if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { // if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
return true; // return true;
} // }
} // }
else if (which == ClawType.RIGHT) { // else if (which == ClawType.RIGHT) {
if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { // if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
return true; // return true;
} // }
} // }
return false; // return false;
} // }
@Override @Override
public void periodic() { public void periodic() {
if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) if (fullyOpen() || fullyClosed()) {
m_leftOffset = m_leftClaw.getEncoder().getPosition(); m_leftClaw.setSpeed(0.0);
m_rightClaw.setSpeed(0.0);
}
// if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
// // m_leftOffset = m_leftClaw.getEncoder().getPosition();
// m_leftOffset = m_leftClaw.getPosition();
if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
m_rightOffset = m_rightClaw.getEncoder().getPosition(); // // m_rightOffset = m_rightClaw.getEncoder().getPosition();
// m_rightOffset = m_rightClaw.getPosition();
} }
} }
src/main/java/frc4388/robot/subsystems/Climber.java
+221 -221
View File
@@ -1,279 +1,279 @@
// Copyright (c) FIRST and other WPILib contributors. // // Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of // // 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. // // the WPILib BSD license file in the root directory of this project.
/* // /*
Safety // Safety
Hooks // Hooks
Add // Add
*/ // */
package frc4388.robot.subsystems; // package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.NeutralMode; // import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode; // import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; // import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.WPI_PigeonIMU; // import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import org.opencv.core.Point; // import org.opencv.core.Point;
import edu.wpi.first.math.Matrix; // import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Rotation2d; // import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase; // import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants; // import frc4388.robot.Constants;
import frc4388.robot.Constants.ClimberConstants; // import frc4388.robot.Constants.ClimberConstants;
public class Climber extends SubsystemBase { // public class Climber extends SubsystemBase {
public WPI_TalonFX m_shoulder; // public WPI_TalonFX m_shoulder;
public WPI_TalonFX m_elbow; // public WPI_TalonFX m_elbow;
private double m_shoulderOffset; // private double m_shoulderOffset;
private double m_elbowOffset; // private double m_elbowOffset;
private WPI_PigeonIMU m_gyro; // private WPI_PigeonIMU m_gyro;
private boolean m_groundRelative; // private boolean m_groundRelative;
private double m_robotAngle; // private double m_robotAngle;
private double m_robotPosition; // private double m_robotPosition;
private Point m_position = new Point(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) { // public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) {
m_shoulder = shoulder; // m_shoulder = shoulder;
m_shoulder.configFactoryDefault(); // m_shoulder.configFactoryDefault();
m_shoulder.setNeutralMode(NeutralMode.Brake); // m_shoulder.setNeutralMode(NeutralMode.Brake);
m_elbow = elbow; // m_elbow = elbow;
m_elbow.configFactoryDefault(); // m_elbow.configFactoryDefault();
m_elbow.setNeutralMode(NeutralMode.Brake); // m_elbow.setNeutralMode(NeutralMode.Brake);
setClimberGains(); // setClimberGains();
m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); // m_shoulderOffset = m_shoulder.getSelectedSensorPosition();
m_elbowOffset = m_elbow.getSelectedSensorPosition(); // m_elbowOffset = m_elbow.getSelectedSensorPosition();
m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); // m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD);
m_elbow.configForwardSoftLimitEnable(false); // m_elbow.configForwardSoftLimitEnable(false);
m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE);
m_elbow.configReverseSoftLimitEnable(false); // m_elbow.configReverseSoftLimitEnable(false);
m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); // m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD);
m_shoulder.configForwardSoftLimitEnable(false); // m_shoulder.configForwardSoftLimitEnable(false);
m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); // m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE);
m_shoulder.configReverseSoftLimitEnable(false); // m_shoulder.configReverseSoftLimitEnable(false);
if(groundRelative) // if(groundRelative)
m_gyro = gyro; // m_gyro = gyro;
m_groundRelative = groundRelative; // m_groundRelative = groundRelative;
} // }
/* getJointAngles // /* getJointAngles
* Gets joint angles for climber arm // * Gets joint angles for climber arm
* targetPoint.x and targetPoint.y 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. // * rotation of the robot. Both parameters should be in cm.
* returns [shoulderAngle, elbowAngle] in radians // * returns [shoulderAngle, elbowAngle] in radians
* Does not set the motors automatically // * Does not set the motors automatically
* // *
* IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */ // * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */
public static double[] getJointAngles(Point targetPoint, double tiltAngle) { // public static double[] getJointAngles(Point targetPoint, double tiltAngle) {
double [] angles = new double[2]; // double [] angles = new double[2];
double mag = Math.hypot(targetPoint.x, targetPoint.y); // double mag = Math.hypot(targetPoint.x, targetPoint.y);
if(mag >= ClimberConstants.MAX_ARM_LENGTH) { // if(mag >= ClimberConstants.MAX_ARM_LENGTH) {
targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH; // targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MAX_ARM_LENGTH;
targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH; // targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MAX_ARM_LENGTH;
mag = ClimberConstants.MAX_ARM_LENGTH; // mag = ClimberConstants.MAX_ARM_LENGTH;
} else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) { // } else if(mag < ClimberConstants.MIN_ARM_LENGTH && mag != 0) {
targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH; // targetPoint.x = (targetPoint.x / mag) * ClimberConstants.MIN_ARM_LENGTH;
targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH; // targetPoint.y = (targetPoint.y / mag) * ClimberConstants.MIN_ARM_LENGTH;
mag = ClimberConstants.MIN_ARM_LENGTH; // mag = ClimberConstants.MIN_ARM_LENGTH;
} else if(mag < ClimberConstants.MIN_ARM_LENGTH) { // } else if(mag < ClimberConstants.MIN_ARM_LENGTH) {
targetPoint.x = ClimberConstants.MIN_ARM_LENGTH; // targetPoint.x = ClimberConstants.MIN_ARM_LENGTH;
targetPoint.y = 0.d; // targetPoint.y = 0.d;
mag = ClimberConstants.MIN_ARM_LENGTH; // mag = ClimberConstants.MIN_ARM_LENGTH;
} // }
// The angle to the target point // // The angle to the target point
double theta; // double theta;
if(targetPoint.x == 0.d) { // if(targetPoint.x == 0.d) {
theta = Math.PI/2.d; // TODO rename variable // theta = Math.PI/2.d; // TODO rename variable
} else { // } else {
theta = Math.atan(targetPoint.y / targetPoint.x); // theta = Math.atan(targetPoint.y / targetPoint.x);
} // }
theta += tiltAngle; // theta += tiltAngle;
if(targetPoint.x < 0.d) // if(targetPoint.x < 0.d)
theta += Math.PI; // theta += Math.PI;
// Correct target position for tilt // // Correct target position for tilt
targetPoint.x = Math.cos(theta) * mag; // targetPoint.x = Math.cos(theta) * mag;
targetPoint.y = Math.sin(theta) * mag; // targetPoint.y = Math.sin(theta) * mag;
// Law and Order: Cosines edition // // Law and Order: Cosines edition
double shoulderAngle; // double shoulderAngle;
if(mag == 0) { // if(mag == 0) {
shoulderAngle = 0; // shoulderAngle = 0;
} else { // } else {
shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / // 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)); // (2.d * ClimberConstants.LOWER_ARM_LENGTH * mag));
} // }
shoulderAngle = theta - shoulderAngle; // 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)) / // 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)); // (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH));
//elbowAngle = Math.PI - elbowAngle; // //elbowAngle = Math.PI - elbowAngle;
// System.out.println("sa1: " + shoulderAngle); // // System.out.println("sa1: " + shoulderAngle);
// System.out.println("ea1: " + elbowAngle); // // 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 the climber is resting on the robot base, rotate the upper arm in the direction of the target
if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { // if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) {
shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; // shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE;
double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH; // double xDiff = targetPoint.x - ClimberConstants.LOWER_ARM_LENGTH;
// System.out.println("xDiff: " + xDiff); // // System.out.println("xDiff: " + xDiff);
elbowAngle = Math.atan(-targetPoint.y / xDiff); // elbowAngle = Math.atan(-targetPoint.y / xDiff);
// System.out.println("ea2: " + elbowAngle); // // System.out.println("ea2: " + elbowAngle);
if(elbowAngle < 0) { // if(elbowAngle < 0) {
elbowAngle = Math.PI - Math.abs(elbowAngle); // elbowAngle = Math.PI - Math.abs(elbowAngle);
} // }
if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) // if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE)
elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; // elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE;
// System.out.println("ea3: " + elbowAngle); // // System.out.println("ea3: " + elbowAngle);
// Deal with negative wraparound // // Deal with negative wraparound
// if(xDiff >= 0.d) // // if(xDiff >= 0.d)
// elbowAngle += Math.PI; // // elbowAngle += Math.PI;
// System.out.println("ea4: " + elbowAngle); // // System.out.println("ea4: " + elbowAngle);
} // }
angles[0] = shoulderAngle; // angles[0] = shoulderAngle;
angles[1] = elbowAngle; // angles[1] = elbowAngle;
return angles; // return angles;
} // }
public void setMotors(double shoulderOutput, double elbowOutput) { // public void setMotors(double shoulderOutput, double elbowOutput) {
m_shoulder.set(shoulderOutput); // m_shoulder.set(shoulderOutput);
m_elbow.set(elbowOutput); // m_elbow.set(elbowOutput);
} // }
/* Rotation Matrix // /* Rotation Matrix
R = [cos(0) -sin(0) \n sin(0) cos(0)] // R = [cos(0) -sin(0) \n sin(0) cos(0)]
Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)] // Rv = [cos(0) -sin(0) \n sin(0) cos(0)] = [x \n y] = [xcos(0) - ysin(0), xsin(0) + ycos(0)]
Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix // Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix
Rotation Matrix video: https://youtu.be/Ta8cKqltPfU // Rotation Matrix video: https://youtu.be/Ta8cKqltPfU
*/ // */
public double getRobotTilt() { // public double getRobotTilt() {
double[] ypr = new double[3]; // double[] ypr = new double[3];
m_gyro.getYawPitchRoll(ypr); // m_gyro.getYawPitchRoll(ypr);
double theta = 0; // double theta = 0;
// double sin; // // double sin;
// double cos; // // double cos;
// xsin = 0; //placeholder for sin, cos // // xsin = 0; //placeholder for sin, cos
// ysin = 0; // // ysin = 0;
// xcos = 0; // // xcos = 0;
// ycos = 0; // // ycos = 0;
double[][] rotMax = { // double[][] rotMax = {
{Math.cos(theta) - Math.sin(theta), 0 }, // {Math.cos(theta) - Math.sin(theta), 0 },
{Math.sin(theta) + Math.cos(theta), 0}, // {Math.sin(theta) + Math.cos(theta), 0},
{0, 0, 1} // {0, 0, 1}
}; // };
if (m_robotPosition != m_robotAngle){ // if (m_robotPosition != m_robotAngle){
setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); // setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition);
} // }
return Math.toRadians(ypr[1]); // Pitch // return Math.toRadians(ypr[1]); // Pitch
// multiply by pie and then divide by 180 // // multiply by pie and then divide by 180
} // }
public void setClimberGains() { // public void setClimberGains() {
// shoulder PIDs // // shoulder PIDs
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); // 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_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_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_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_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
// elbow PIDs // // elbow PIDs
m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); // 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_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_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_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); // m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
} // }
public void setJointAngles(double[] angles) { // public void setJointAngles(double[] angles) {
System.out.println(angles); // System.out.println(angles);
setJointAngles(angles[0], angles[1]); // setJointAngles(angles[0], angles[1]);
} // }
public void setJointAngles(double shoulderAngle, double elbowAngle) { // public void setJointAngles(double shoulderAngle, double elbowAngle) {
shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle; // shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle; // elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle; // shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; // elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
// Convert radians to ticks // // Convert radians to ticks
System.out.println("angles: " + shoulderAngle + ", " + elbowAngle); // System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; // double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI; // double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO; // shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO;
elbowAngle *= ClimberConstants.GEAR_BOX_RATIO; // elbowAngle *= ClimberConstants.GEAR_BOX_RATIO;
shoulderPosition += m_shoulderOffset; // shoulderPosition += m_shoulderOffset;
elbowPosition += m_elbowOffset; // elbowPosition += m_elbowOffset;
m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); // m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
m_elbow.set(TalonFXControlMode.Position, elbowPosition); // m_elbow.set(TalonFXControlMode.Position, elbowPosition);
} // }
public void controlWithInput(double xInput, double yInput) { // public void controlWithInput(double xInput, double yInput) {
m_position.x += xInput * ClimberConstants.MOVE_SPEED; // m_position.x += xInput * ClimberConstants.MOVE_SPEED;
m_position.y += yInput * ClimberConstants.MOVE_SPEED; // m_position.y += yInput * ClimberConstants.MOVE_SPEED;
System.out.println("x: " + m_position.x + " y: " + m_position.y); // System.out.println("x: " + m_position.x + " y: " + m_position.y);
double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; // double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d;
// double[] testAngles = getJointAngles(0, 0, 0); // // double[] testAngles = getJointAngles(0, 0, 0);
// System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); // // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]);
// double[] testAngles2 = getJointAngles(5000, 5000, 0); // // double[] testAngles2 = getJointAngles(5000, 5000, 0);
// System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); // // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]);
// double[] testAngles3 = getJointAngles(0, 75, 0); // // double[] testAngles3 = getJointAngles(0, 75, 0);
// System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); // // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]);
// double[] testAngles4 = getJointAngles(75, 0, 0); // // double[] testAngles4 = getJointAngles(75, 0, 0);
// System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); // // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]);
// double[] testAngles5 = getJointAngles(-75, 0, 0); // // double[] testAngles5 = getJointAngles(-75, 0, 0);
// System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); // // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]);
// double[] testAngles6 = getJointAngles(60, 25, 0); // // double[] testAngles6 = getJointAngles(60, 25, 0);
// System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); // // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]);
double[] jointAngles = getJointAngles(m_position, tiltAngle); // double[] jointAngles = getJointAngles(m_position, tiltAngle);
setJointAngles(jointAngles); // setJointAngles(jointAngles);
} // }
public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { // public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) {
m_robotPosition = robotPosition; // m_robotPosition = robotPosition;
m_robotAngle = robotAngle; // m_robotAngle = robotAngle;
m_robotAngle = 45; //45 is placeholder // m_robotAngle = 45; //45 is placeholder
} // }
@Override // @Override
public void periodic(){ // public void periodic(){
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); // SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
} // }
} // }