From 1e27851fe5b34b0199b301c7212cf0d4d3543405 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=AEZach=20Wilke?= <90875734+76842@users.noreply.github.com> Date: Sat, 15 Jan 2022 13:24:09 -0700 Subject: [PATCH 01/55] Make blank Climber Subsystem --- .../java/frc4388/robot/RobotContainer.java | 2 ++ .../frc4388/robot/subsystems/Climber.java | 19 +++++++++++++++++++ 2 files changed, 21 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 38c3459..1680f31 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -41,6 +42,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); + private final Climber m_climber = new Climber(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..d49184b --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,19 @@ +// 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 edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + /** Creates a new Climber. */ + + public Climber() { + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 738d9271d1f457e7949a2f9b6fe6c5482211e538 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=AEZach=20Wilke?= <90875734+76842@users.noreply.github.com> Date: Sat, 15 Jan 2022 13:36:01 -0700 Subject: [PATCH 02/55] Update Constants.java --- src/main/java/frc4388/robot/Constants.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5581adf..7db6ce1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -73,6 +73,11 @@ public final class Constants { public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; } + public static final class ClimberConstants { + /* TODO: Update motor IDS */ + public static final int MOTOR_1_ID = -1; + public static final int MOTOR_2_ID = -1; + } /** * The OIConstants class contains the ID for the XBox controllers */ From c344394938909fe0be750569a4f87c3053b30a0f Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Sat, 15 Jan 2022 15:56:17 -0700 Subject: [PATCH 03/55] Started IK calc --- src/main/java/frc4388/robot/Constants.java | 8 +++-- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 4 +++ .../frc4388/robot/subsystems/Climber.java | 30 +++++++++++++++---- 4 files changed, 36 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7db6ce1..950a574 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -75,8 +75,12 @@ public final class Constants { public static final class ClimberConstants { /* TODO: Update motor IDS */ - public static final int MOTOR_1_ID = -1; - public static final int MOTOR_2_ID = -1; + public static final int SHOULDER_ID = -1; + public static final int ELBOW_ID = -1; + + // TODO Update this stuff too + public static final double UPPER_ARM_LENGTH = 50; // Units should be in cm + public static final double LOWER_ARM_LENGTH = 50; } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1680f31..691d448 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,7 +42,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final Climber m_climber = new Climber(); + private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 843323c..90c5dc1 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.wpilibj.motorcontrol.Spark; +import frc4388.robot.Constants.ClimberConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -92,4 +93,7 @@ public class RobotMap { //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); } + /* Climb Subsystem */ + public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOLDER_ID); // TODO + public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index d49184b..0deb63f 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,16 +4,36 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { - /** Creates a new Climber. */ + WPI_TalonFX m_shoulder; + WPI_TalonFX m_elbow; - public Climber() { + public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow) { + m_shoulder = shoulder; + m_elbow = elbow; } - @Override - public void periodic() { - // This method will be called once per scheduler run + /* getJointAngles + * Gets joint angles for climber arm + * xTarget and yTarget are set in the xy plane of the climber, not accounting for the + * rotation of the robot. Both parameters should be in cm. + * returns [shoulderAngle, elbowAngle] + * Does not set the motors automatically + * + * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */ + public double[] getJointAngles(double xTarget, double yTarget) { + double mag = Math.hypot(xTarget, yTarget); + double upperArm_2 = ClimberConstants.UPPER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH; + double lowerArm_2 = ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.LOWER_ARM_LENGTH; + + double shoulderAngle = Math.acos((-lowerArm_2 + upperArm_2 - mag) / (2.d * ClimberConstants.UPPER_ARM_LENGTH * mag)); + double elbowAngle = Math.acos((lowerArm_2 + upperArm_2 - mag) / (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); + + return null; } } From 3567cfbeb097a19507e6d6146605abeb6943814f Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Tue, 18 Jan 2022 20:00:03 -0700 Subject: [PATCH 04/55] Finish IK, accounting for robot tilt --- src/main/java/frc4388/robot/Constants.java | 4 ++ src/main/java/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/subsystems/Climber.java | 66 +++++++++++++++---- 3 files changed, 62 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 950a574..f514404 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -77,10 +77,14 @@ public final class Constants { /* TODO: Update motor IDS */ public static final int SHOULDER_ID = -1; public static final int ELBOW_ID = -1; + public static final int GYRO_ID = -1; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 50; // Units should be in cm public static final double LOWER_ARM_LENGTH = 50; + + public static final double MAX_ARM_LENGTH = LOWER_ARM_LENGTH + UPPER_ARM_LENGTH; + public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH); } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 90c5dc1..3aa9b0a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -6,6 +6,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; @@ -94,6 +95,7 @@ public class RobotMap { } /* Climb Subsystem */ - public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOLDER_ID); // TODO + public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO + public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 0deb63f..8a170b3 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -5,35 +5,79 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { - WPI_TalonFX m_shoulder; - WPI_TalonFX m_elbow; + private WPI_TalonFX m_shoulder; + private WPI_TalonFX m_elbow; + + private WPI_PigeonIMU m_gyro; - public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow) { + public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { m_shoulder = shoulder; m_elbow = elbow; + m_gyro = gyro; } /* getJointAngles * Gets joint angles for climber arm - * xTarget and yTarget are set in the xy plane of the climber, not accounting for the + * xTarget and yTarget 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] + * 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 double[] getJointAngles(double xTarget, double yTarget) { + public static double[] getJointAngles(double xTarget, double yTarget, double tiltAngle) { + double [] angles = new double[2]; + double mag = Math.hypot(xTarget, yTarget); - double upperArm_2 = ClimberConstants.UPPER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH; - double lowerArm_2 = ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.LOWER_ARM_LENGTH; + if(mag > ClimberConstants.MAX_ARM_LENGTH) { + xTarget = (xTarget / mag) * ClimberConstants.MAX_ARM_LENGTH; + yTarget = (yTarget / mag) * ClimberConstants.MAX_ARM_LENGTH; + } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { + xTarget = (xTarget / mag) * ClimberConstants.MIN_ARM_LENGTH; + yTarget = (yTarget / mag) * ClimberConstants.MIN_ARM_LENGTH; + } - double shoulderAngle = Math.acos((-lowerArm_2 + upperArm_2 - mag) / (2.d * ClimberConstants.UPPER_ARM_LENGTH * mag)); - double elbowAngle = Math.acos((lowerArm_2 + upperArm_2 - mag) / (2.d * ClimberConstants.LOWER_ARM_LENGTH * ClimberConstants.UPPER_ARM_LENGTH)); + // The angle to the target point + double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable + // Correct target position for tilt + xTarget = Math.cos(theta) * mag; + yTarget = Math.sin(theta) * mag; + + // Law and Order: Cosines edition + double 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; - return null; + // If the climber is resting on the robot base, rotate the upper arm in the direction of the target + if(shoulderAngle <= 0.d) { + shoulderAngle = 0.d; + double xDiff = xTarget - ClimberConstants.LOWER_ARM_LENGTH; + + elbowAngle = Math.atan(-yTarget / xDiff); + elbowAngle = Math.PI - elbowAngle; + + if(xDiff >= 0.d) { + elbowAngle += Math.PI; + } + } + + return angles; + } + + public void setJointAngles(double[] angles) { + setJointAngles(angles[0], angles[1]); + } + + public void setJointAngles(double shoulderAngle, double elbowAngle) { + // Set PIDs } } From 50c83e98f234c1093df988563edf64e3d1554d2b Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Thu, 20 Jan 2022 16:37:57 -0700 Subject: [PATCH 05/55] Add getRobotTilt --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Climber.java | 9 ++++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 691d448..bc62e9f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,7 +42,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow); + private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 8a170b3..2cd8cdd 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -44,12 +44,13 @@ public class Climber extends SubsystemBase { // The angle to the target point double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable + // Correct target position for tilt xTarget = Math.cos(theta) * mag; yTarget = Math.sin(theta) * mag; // Law and Order: Cosines edition - double shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) -Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / + double 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; @@ -73,6 +74,12 @@ public class Climber extends SubsystemBase { return angles; } + public double getRobotTilt() { + double[] ypr = new double[3]; + m_gyro.getYawPitchRoll(ypr); + return ypr[1]; // Pitch + } + public void setJointAngles(double[] angles) { setJointAngles(angles[0], angles[1]); } From 94a0af64d014bb34e03a94d2673c6111ba3c5786 Mon Sep 17 00:00:00 2001 From: dj12ha <90010681+dj12ha@users.noreply.github.com> Date: Thu, 20 Jan 2022 17:10:07 -0700 Subject: [PATCH 06/55] Added input controls --- src/main/java/frc4388/robot/Constants.java | 2 ++ src/main/java/frc4388/robot/RobotContainer.java | 7 ++++++- .../java/frc4388/robot/subsystems/Climber.java | 14 +++++++++++++- 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f514404..227779f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -85,6 +85,8 @@ public final class Constants { public static final double MAX_ARM_LENGTH = LOWER_ARM_LENGTH + UPPER_ARM_LENGTH; public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH); + + public static final double MOVE_SPEED = 50; // cm per second } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bc62e9f..ef171b2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,7 +42,7 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final Climber m_climber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro); + private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); @@ -59,6 +59,11 @@ public class RobotContainer { new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); + // moves climber in xy space with two-axis input from the operator controller + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(), + getOperatorController().getLeftYAxis()), m_robotClimber)); + // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 2cd8cdd..74f08c3 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -15,6 +15,8 @@ public class Climber extends SubsystemBase { private WPI_TalonFX m_elbow; private WPI_PigeonIMU m_gyro; + + private double[] position = {0.d, 0.d}; public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { m_shoulder = shoulder; @@ -77,7 +79,7 @@ public class Climber extends SubsystemBase { public double getRobotTilt() { double[] ypr = new double[3]; m_gyro.getYawPitchRoll(ypr); - return ypr[1]; // Pitch + return Math.toRadians(ypr[1]); // Pitch } public void setJointAngles(double[] angles) { @@ -87,4 +89,14 @@ public class Climber extends SubsystemBase { public void setJointAngles(double shoulderAngle, double elbowAngle) { // Set PIDs } + + public void controlWithInput(double xInput, double yInput) { + position[0] += xInput * ClimberConstants.MOVE_SPEED; + position[1] += yInput * ClimberConstants.MOVE_SPEED; + + double tiltAngle = getRobotTilt(); + + double[] jointAngles = getJointAngles(position[0], position[1], tiltAngle); + setJointAngles(jointAngles); + } } From 7949205cc4ef629f86582eb491675fa063b17f9d Mon Sep 17 00:00:00 2001 From: dj12ha <90010681+dj12ha@users.noreply.github.com> Date: Thu, 20 Jan 2022 18:13:11 -0700 Subject: [PATCH 07/55] Added Input control and PIDs Still need to configure PIDs PIDs are currently set to max values. Please do not run. --- src/main/java/frc4388/robot/Constants.java | 12 +++++++ .../frc4388/robot/subsystems/Climber.java | 33 +++++++++++++++++-- 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 227779f..4fc0de6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -87,6 +87,18 @@ public final class Constants { public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH); public static final double MOVE_SPEED = 50; // cm per second + + // PID Constants + public static final int SHOULDER_SLOT_IDX = 0; + public static final int SHOULDER_PID_LOOP_IDX = 1; + + public static final int ELBOW_SLOT_IDX = 0; + public static final int ELBOW_PID_LOOP_IDX = 1; + + public static final Gains SHOULDER_GAINS = new Gains(1.0, 1.0, 1.0, 1.0, 0, 1.0); + public static final Gains ELBOW_GAINS = new Gains(1.0, 1.0, 1.0, 1.0, 0, 1.0); + + public static final int CLIMBER_TIMEOUT_MS = 100; } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 74f08c3..f8b5e90 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.WPI_PigeonIMU; @@ -16,11 +18,17 @@ public class Climber extends SubsystemBase { private WPI_PigeonIMU m_gyro; - private double[] position = {0.d, 0.d}; + private double[] position = {0.d, 0.d}; // TODO change position public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { m_shoulder = shoulder; + m_shoulder.configFactoryDefault(); + m_shoulder.setNeutralMode(NeutralMode.Brake); + m_elbow = elbow; + m_elbow.configFactoryDefault(); + m_elbow.setNeutralMode(NeutralMode.Brake); + m_gyro = gyro; } @@ -82,12 +90,33 @@ public class Climber extends SubsystemBase { return Math.toRadians(ypr[1]); // Pitch } + public void setClimberGains() { + // shoulder PIDs + m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kD, ClimberConstants.CLIMBER_TIMEOUT_MS); + + // elbow PIDs + m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kD, ClimberConstants.CLIMBER_TIMEOUT_MS); + } + public void setJointAngles(double[] angles) { setJointAngles(angles[0], angles[1]); } public void setJointAngles(double shoulderAngle, double elbowAngle) { - // Set PIDs + // Convert radians to ticks + double shoulderPosition = (shoulderAngle * 1024.d) / Math.PI; + double elbowPosition = (elbowAngle * 1024.d) / Math.PI; + + m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); + m_elbow.set(TalonFXControlMode.Position, elbowPosition); } public void controlWithInput(double xInput, double yInput) { From e01ffd33299f34a14bc84822581f1a2d15b6e6fc Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Thu, 20 Jan 2022 19:58:05 -0700 Subject: [PATCH 08/55] Code review stuff --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Climber.java | 10 +++++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4fc0de6..d24efca 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -95,8 +95,8 @@ public final class Constants { public static final int ELBOW_SLOT_IDX = 0; public static final int ELBOW_PID_LOOP_IDX = 1; - public static final Gains SHOULDER_GAINS = new Gains(1.0, 1.0, 1.0, 1.0, 0, 1.0); - public static final Gains ELBOW_GAINS = new Gains(1.0, 1.0, 1.0, 1.0, 0, 1.0); + public static final Gains SHOULDER_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains ELBOW_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); public static final int CLIMBER_TIMEOUT_MS = 100; } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index f8b5e90..a55c329 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -2,6 +2,13 @@ // 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. +/* +Safety +Hooks +Add +Overextension when lower arm is resting check in processing +*/ + package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -18,7 +25,7 @@ public class Climber extends SubsystemBase { private WPI_PigeonIMU m_gyro; - private double[] position = {0.d, 0.d}; // TODO change position + private double[] position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { m_shoulder = shoulder; @@ -62,6 +69,7 @@ public class Climber extends SubsystemBase { // Law and Order: Cosines edition double 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 = acos(LowerArmLength^2 + mag^2) 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)) / From ab2a3f6866f306d275c9c848aa0d73d6af166e59 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Fri, 21 Jan 2022 17:20:31 -0700 Subject: [PATCH 09/55] Added hooks and did code review stuff Arm will no longer teleport --- src/main/java/frc4388/robot/Constants.java | 17 ++++++++-- .../java/frc4388/robot/RobotContainer.java | 11 +++++- src/main/java/frc4388/robot/RobotMap.java | 6 ++++ .../frc4388/robot/subsystems/Climber.java | 33 +++++++++++------- .../java/frc4388/robot/subsystems/Hooks.java | 34 +++++++++++++++++++ 5 files changed, 85 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Hooks.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d24efca..4d42016 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -21,6 +21,8 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { + public static final double TICKS_PER_ROTATION_FX = 2048; + public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 0.1; public static final double WHEEL_SPEED = 0.1; @@ -83,11 +85,14 @@ public final class Constants { public static final double UPPER_ARM_LENGTH = 50; // Units should be in cm public static final double LOWER_ARM_LENGTH = 50; - public static final double MAX_ARM_LENGTH = LOWER_ARM_LENGTH + UPPER_ARM_LENGTH; - public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH); + public static final double MAX_ARM_LENGTH = 100; + public static final double MIN_ARM_LENGTH = 5; public static final double MOVE_SPEED = 50; // cm per second + public static final double SHOULDER_RESTING_ANGLE = 0; + public static final double ELBOW_RESTING_ANGLE = 0; + // PID Constants public static final int SHOULDER_SLOT_IDX = 0; public static final int SHOULDER_PID_LOOP_IDX = 1; @@ -100,6 +105,14 @@ public final class Constants { public static final int CLIMBER_TIMEOUT_MS = 100; } + + public static final class HooksConstants { + public static final int LEFT_HOOK_ID = -1; + public static final int RIGHT_HOOK_ID = -1; + + public static final double OPEN_POSITION = 0; + public static final double CLOSE_POSITION = 0; + } /** * The OIConstants class contains the ID for the XBox controllers */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ef171b2..5df7097 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Hooks; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -43,6 +44,9 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro); + + private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); + /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); @@ -62,7 +66,8 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(), - getOperatorController().getLeftYAxis()), m_robotClimber)); + getOperatorController().getLeftYAxis(), + getDriverJoystick().getRawButtonPressed(XboxController.A_BUTTON)), m_robotClimber)); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); @@ -82,6 +87,10 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(() -> m_hooks.setOpen(true)) + .whenReleased(() -> m_hooks.setOpen(false)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3aa9b0a..f3fbc4a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,8 +8,10 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; +import frc4388.robot.Constants.HooksConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -98,4 +100,8 @@ public class RobotMap { public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO + + /* Hooks Subsystem */ + public final Servo leftHook = new Servo(HooksConstants.LEFT_HOOK_ID); + public final Servo rightHook = new Servo(HooksConstants.RIGHT_HOOK_ID); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index a55c329..c234b99 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -5,8 +5,7 @@ /* Safety Hooks -Add -Overextension when lower arm is resting check in processing +Add */ package frc4388.robot.subsystems; @@ -17,6 +16,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { @@ -25,7 +25,7 @@ public class Climber extends SubsystemBase { private WPI_PigeonIMU m_gyro; - private double[] position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; + private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { m_shoulder = shoulder; @@ -77,16 +77,20 @@ public class Climber extends SubsystemBase { elbowAngle = Math.PI - elbowAngle; // If the climber is resting on the robot base, rotate the upper arm in the direction of the target - if(shoulderAngle <= 0.d) { - shoulderAngle = 0.d; + if(shoulderAngle <= ClimberConstants.SHOULDER_RESTING_ANGLE) { + shoulderAngle = ClimberConstants.SHOULDER_RESTING_ANGLE; double xDiff = xTarget - ClimberConstants.LOWER_ARM_LENGTH; elbowAngle = Math.atan(-yTarget / xDiff); + if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) + elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; + + elbowAngle = Math.PI - elbowAngle; - if(xDiff >= 0.d) { + // Deal with negative wraparound + if(xDiff >= 0.d) elbowAngle += Math.PI; - } } return angles; @@ -120,20 +124,23 @@ public class Climber extends SubsystemBase { public void setJointAngles(double shoulderAngle, double elbowAngle) { // Convert radians to ticks - double shoulderPosition = (shoulderAngle * 1024.d) / Math.PI; - double elbowPosition = (elbowAngle * 1024.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; m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); m_elbow.set(TalonFXControlMode.Position, elbowPosition); } - public void controlWithInput(double xInput, double yInput) { - position[0] += xInput * ClimberConstants.MOVE_SPEED; - position[1] += yInput * ClimberConstants.MOVE_SPEED; + public void controlWithInput(double xInput, double yInput, boolean safety) { + if(!safety) + return; + + m_position[0] += xInput * ClimberConstants.MOVE_SPEED; + m_position[1] += yInput * ClimberConstants.MOVE_SPEED; double tiltAngle = getRobotTilt(); - double[] jointAngles = getJointAngles(position[0], position[1], tiltAngle); + double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles); } } diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java new file mode 100644 index 0000000..555bec2 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Hooks.java @@ -0,0 +1,34 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.HooksConstants; + +public class Hooks extends SubsystemBase { + private Servo m_leftHook; + private Servo m_rightHook; + + private boolean m_open; + + public Hooks(Servo leftHook, Servo rightHook) { + m_leftHook = leftHook; + m_rightHook = rightHook; + + m_open = false; + setOpen(m_open); + } + + public void setOpen(boolean open) { + if(open) { + m_leftHook.setPosition(HooksConstants.OPEN_POSITION); + m_rightHook.setPosition(HooksConstants.OPEN_POSITION); + } else { + m_leftHook.setPosition(HooksConstants.CLOSE_POSITION); + m_rightHook.setPosition(HooksConstants.CLOSE_POSITION); + } + } + + public boolean getOpen() { + return m_open; + } +} From 050d60ef36534ae1738fd0e8e2cf92280646f986 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 22 Jan 2022 11:25:39 -0700 Subject: [PATCH 10/55] fixed motor controls --- build.gradle | 2 +- src/main/java/frc4388/robot/Constants.java | 8 +++---- .../java/frc4388/robot/RobotContainer.java | 19 +++++++-------- src/main/java/frc4388/robot/RobotMap.java | 6 ++--- .../frc4388/robot/subsystems/Climber.java | 24 +++++++++++++------ 5 files changed, 34 insertions(+), 25 deletions(-) diff --git a/build.gradle b/build.gradle index c1f07ee..7e60634 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2022.1.1" + id "edu.wpi.first.GradleRIO" version "2022.2.1" } sourceCompatibility = JavaVersion.VERSION_11 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4d42016..757383c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -77,8 +77,8 @@ public final class Constants { public static final class ClimberConstants { /* TODO: Update motor IDS */ - public static final int SHOULDER_ID = -1; - public static final int ELBOW_ID = -1; + public static final int SHOULDER_ID = 1; + public static final int ELBOW_ID = 3; public static final int GYRO_ID = -1; // TODO Update this stuff too @@ -88,7 +88,7 @@ public final class Constants { public static final double MAX_ARM_LENGTH = 100; public static final double MIN_ARM_LENGTH = 5; - public static final double MOVE_SPEED = 50; // cm per second + public static final double MOVE_SPEED = .2; // cm per second public static final double SHOULDER_RESTING_ANGLE = 0; public static final double ELBOW_RESTING_ANGLE = 0; @@ -103,7 +103,7 @@ public final class Constants { public static final Gains SHOULDER_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains ELBOW_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final int CLIMBER_TIMEOUT_MS = 100; + public static final int CLIMBER_TIMEOUT_MS = 50; } public static final class HooksConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5df7097..9b8923b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -43,9 +43,9 @@ public class RobotContainer { private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro); + private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); + //private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -59,15 +59,14 @@ public class RobotContainer { /* Default Commands */ // drives the swerve drive with a two-axis input from the driver controller - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), - getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), + // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(), - getOperatorController().getLeftYAxis(), - getDriverJoystick().getRawButtonPressed(XboxController.A_BUTTON)), m_robotClimber)); + getOperatorController().getLeftYAxis()), m_robotClimber)); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); @@ -88,9 +87,9 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(() -> m_hooks.setOpen(true)) - .whenReleased(() -> m_hooks.setOpen(false)); + // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + // .whenPressed(() -> m_hooks.setOpen(true)) + // .whenReleased(() -> m_hooks.setOpen(false)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index f3fbc4a..6de2f65 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -99,9 +99,9 @@ public class RobotMap { /* Climb Subsystem */ public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO - public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO + public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO /* Hooks Subsystem */ - public final Servo leftHook = new Servo(HooksConstants.LEFT_HOOK_ID); - public final Servo rightHook = new Servo(HooksConstants.RIGHT_HOOK_ID); + //public final Servo leftHook = new Servo(HooksConstants.LEFT_HOOK_ID); + //public final Servo rightHook = new Servo(HooksConstants.RIGHT_HOOK_ID); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index c234b99..032a85f 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -24,10 +24,11 @@ public class Climber extends SubsystemBase { private WPI_TalonFX m_elbow; private WPI_PigeonIMU m_gyro; + private boolean m_groundRelative; private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; - public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) { + public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { m_shoulder = shoulder; m_shoulder.configFactoryDefault(); m_shoulder.setNeutralMode(NeutralMode.Brake); @@ -36,7 +37,12 @@ public class Climber extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - m_gyro = gyro; + setClimberGains(); + + if(groundRelative) + m_gyro = gyro; + + m_groundRelative = groundRelative; } /* getJointAngles @@ -93,6 +99,8 @@ public class Climber extends SubsystemBase { elbowAngle += Math.PI; } + angles[0] = shoulderAngle; + angles[1] = elbowAngle; return angles; } @@ -119,11 +127,14 @@ public class Climber extends SubsystemBase { } public void setJointAngles(double[] angles) { + System.out.println(angles); setJointAngles(angles[0], angles[1]); } public void setJointAngles(double shoulderAngle, double 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; @@ -131,14 +142,13 @@ public class Climber extends SubsystemBase { m_elbow.set(TalonFXControlMode.Position, elbowPosition); } - public void controlWithInput(double xInput, double yInput, boolean safety) { - if(!safety) - return; - + public void controlWithInput(double xInput, double yInput) { m_position[0] += xInput * ClimberConstants.MOVE_SPEED; m_position[1] += yInput * ClimberConstants.MOVE_SPEED; - double tiltAngle = getRobotTilt(); + System.out.println("x: " + m_position[0] + " y: " + m_position[1]); + + double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles); From 7386912b1398d9033b2287e6b72e6aa5e7603e58 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Sat, 22 Jan 2022 11:44:19 -0700 Subject: [PATCH 11/55] Corrected IK mistakes --- src/main/java/frc4388/robot/subsystems/Climber.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 032a85f..74b8880 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -60,13 +60,17 @@ public class Climber extends SubsystemBase { if(mag > ClimberConstants.MAX_ARM_LENGTH) { xTarget = (xTarget / mag) * ClimberConstants.MAX_ARM_LENGTH; yTarget = (yTarget / mag) * ClimberConstants.MAX_ARM_LENGTH; + mag = ClimberConstants.MAX_ARM_LENGTH; } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { xTarget = (xTarget / mag) * ClimberConstants.MIN_ARM_LENGTH; yTarget = (yTarget / mag) * ClimberConstants.MIN_ARM_LENGTH; + mag = ClimberConstants.MIN_ARM_LENGTH; } // The angle to the target point double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable + if(xTarget < 0.d) + theta += Math.PI; // Correct target position for tilt xTarget = Math.cos(theta) * mag; From e0c4bb479be123451ff6a2bc601103ea708a7d1b Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Sat, 22 Jan 2022 13:21:31 -0700 Subject: [PATCH 12/55] Test print lines --- src/main/java/frc4388/robot/subsystems/Climber.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 74b8880..d9d006e 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -154,6 +154,8 @@ public class Climber extends SubsystemBase { double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; + System.out.println(getJointAngles(0, 0, 0)); + double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles); } From 20f4bdf5badc59184b7a1d5658b6ec9e50e88d5b Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Sat, 22 Jan 2022 14:14:35 -0700 Subject: [PATCH 13/55] Fixed IK math origin: 0.0, 0.0 extended: 0.7853981633974483, 3.141592653589793 just y: 0.1367898269242458, 0.27357965384849187 just x: 0.0, 0.0 just x: 1.7075861537191424, 0.27357965384849187 --- .../frc4388/robot/subsystems/Climber.java | 45 ++++++++++++------- 1 file changed, 29 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index d9d006e..419e8d2 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -55,54 +55,67 @@ public class Climber extends SubsystemBase { * IK resource: https://devforum.roblox.com/t/2-joint-2-limb-inverse-kinematics/252399 */ public static double[] getJointAngles(double xTarget, double yTarget, double tiltAngle) { double [] angles = new double[2]; - + double mag = Math.hypot(xTarget, yTarget); - if(mag > ClimberConstants.MAX_ARM_LENGTH) { + if(mag >= ClimberConstants.MAX_ARM_LENGTH) { xTarget = (xTarget / mag) * ClimberConstants.MAX_ARM_LENGTH; yTarget = (yTarget / mag) * ClimberConstants.MAX_ARM_LENGTH; mag = ClimberConstants.MAX_ARM_LENGTH; - } else if(mag < ClimberConstants.MIN_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; mag = ClimberConstants.MIN_ARM_LENGTH; + } else if(mag < ClimberConstants.MIN_ARM_LENGTH) { + xTarget = ClimberConstants.MIN_ARM_LENGTH; + yTarget = 0.d; + mag = ClimberConstants.MIN_ARM_LENGTH; } - + // The angle to the target point - double theta = Math.atan(yTarget / xTarget) + tiltAngle; // TODO rename variable + double theta; + if(xTarget == 0.d) { + theta = Math.PI/2.d; // TODO rename variable + } else { + theta = Math.atan(yTarget / xTarget); + } + theta += tiltAngle; + if(xTarget < 0.d) - theta += Math.PI; + theta += Math.PI; + // Correct target position for tilt xTarget = Math.cos(theta) * mag; yTarget = Math.sin(theta) * mag; // Law and Order: Cosines edition - double shoulderAngle = Math.acos((Math.pow(ClimberConstants.LOWER_ARM_LENGTH, 2) + Math.pow(mag, 2) - Math.pow(ClimberConstants.UPPER_ARM_LENGTH, 2)) / + 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 = acos(LowerArmLength^2 + mag^2) + } 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; - + //elbowAngle = Math.PI - 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 = xTarget - ClimberConstants.LOWER_ARM_LENGTH; - + elbowAngle = Math.atan(-yTarget / xDiff); if(elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE) elbowAngle = ClimberConstants.ELBOW_RESTING_ANGLE; - - - elbowAngle = Math.PI - elbowAngle; - + // Deal with negative wraparound if(xDiff >= 0.d) elbowAngle += Math.PI; } - + angles[0] = shoulderAngle; angles[1] = elbowAngle; return angles; From 6a44632d2a7743d6081fd4a86b121a23f4634db3 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Sat, 22 Jan 2022 14:17:15 -0700 Subject: [PATCH 14/55] Added printlines --- .../java/frc4388/robot/subsystems/Climber.java | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 419e8d2..3e9eaba 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -167,9 +167,22 @@ public class Climber extends SubsystemBase { double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; - System.out.println(getJointAngles(0, 0, 0)); + double[] testAngles = getJointAngles(0, 0, 0); + System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); + + double[] testAngles2 = getJointAngles(5000, 5000, 0); + System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); + + double[] testAngles3 = getJointAngles(0, 75, 0); + System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); + + double[] testAngles4 = getJointAngles(75, 0, 0); + System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); + + double[] testAngles5 = getJointAngles(-75, 0, 0); + System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); - setJointAngles(jointAngles); + //setJointAngles(jointAngles); } } From 0ecae16cfe8e689752544ff7abe8a942756be7af Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 22 Jan 2022 15:58:34 -0700 Subject: [PATCH 15/55] Fixed IK really --- src/main/java/frc4388/robot/Constants.java | 2 +- .../frc4388/robot/subsystems/Climber.java | 19 ++++++++++++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 757383c..e694ac0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -86,7 +86,7 @@ public final class Constants { public static final double LOWER_ARM_LENGTH = 50; public static final double MAX_ARM_LENGTH = 100; - public static final double MIN_ARM_LENGTH = 5; + public static final double MIN_ARM_LENGTH = 0; public static final double MOVE_SPEED = .2; // cm per second diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 3e9eaba..a3de194 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -101,19 +101,29 @@ public class Climber extends SubsystemBase { 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 = xTarget - ClimberConstants.LOWER_ARM_LENGTH; + System.out.println("xDiff: " + xDiff); elbowAngle = Math.atan(-yTarget / 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; + // if(xDiff >= 0.d) + // elbowAngle += Math.PI; + // System.out.println("ea4: " + elbowAngle); } angles[0] = shoulderAngle; @@ -182,7 +192,10 @@ public class Climber extends SubsystemBase { double[] testAngles5 = getJointAngles(-75, 0, 0); System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); + 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); - //setJointAngles(jointAngles); + setJointAngles(jointAngles); } } From fe07715d95d1b640c167c21ad7df53494560f2be Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 24 Jan 2022 17:55:23 -0700 Subject: [PATCH 16/55] minor angle adjustments --- src/main/java/frc4388/robot/Constants.java | 14 +++--- .../frc4388/robot/subsystems/Climber.java | 43 +++++++++++-------- 2 files changed, 33 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e694ac0..4076a30 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -82,13 +82,13 @@ public final class Constants { public static final int GYRO_ID = -1; // TODO Update this stuff too - public static final double UPPER_ARM_LENGTH = 50; // Units should be in cm - public static final double LOWER_ARM_LENGTH = 50; + public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm + public static final double LOWER_ARM_LENGTH = 27; - public static final double MAX_ARM_LENGTH = 100; - public static final double MIN_ARM_LENGTH = 0; + public static final double MAX_ARM_LENGTH = 53; + public static final double MIN_ARM_LENGTH = 1; - public static final double MOVE_SPEED = .2; // cm per second + public static final double MOVE_SPEED = .1; // cm per second public static final double SHOULDER_RESTING_ANGLE = 0; public static final double ELBOW_RESTING_ANGLE = 0; @@ -100,8 +100,8 @@ public final class Constants { public static final int ELBOW_SLOT_IDX = 0; public static final int ELBOW_PID_LOOP_IDX = 1; - public static final Gains SHOULDER_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains ELBOW_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOULDER_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains ELBOW_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0); public static final int CLIMBER_TIMEOUT_MS = 50; } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index a3de194..7f9b883 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -23,6 +23,9 @@ public class Climber extends SubsystemBase { private WPI_TalonFX m_shoulder; private WPI_TalonFX m_elbow; + private double m_shoulderOffset; + private double m_elbowOffset; + private WPI_PigeonIMU m_gyro; private boolean m_groundRelative; @@ -39,6 +42,9 @@ public class Climber extends SubsystemBase { setClimberGains(); + m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); + m_elbowOffset = m_elbow.getSelectedSensorPosition(); + if(groundRelative) m_gyro = gyro; @@ -101,24 +107,24 @@ public class Climber extends SubsystemBase { 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); + // 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 = xTarget - ClimberConstants.LOWER_ARM_LENGTH; - System.out.println("xDiff: " + xDiff); + // System.out.println("xDiff: " + xDiff); elbowAngle = Math.atan(-yTarget / xDiff); - System.out.println("ea2: " + elbowAngle); + // 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); + // System.out.println("ea3: " + elbowAngle); // Deal with negative wraparound // if(xDiff >= 0.d) @@ -165,6 +171,9 @@ public class Climber extends SubsystemBase { 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 += m_shoulderOffset; + elbowPosition += m_elbowOffset; + m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); m_elbow.set(TalonFXControlMode.Position, elbowPosition); } @@ -177,23 +186,23 @@ public class Climber extends SubsystemBase { double tiltAngle = m_groundRelative ? getRobotTilt() : 0.d; - double[] testAngles = getJointAngles(0, 0, 0); - System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); + // double[] testAngles = getJointAngles(0, 0, 0); + // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); - double[] testAngles2 = getJointAngles(5000, 5000, 0); - System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); + // double[] testAngles2 = getJointAngles(5000, 5000, 0); + // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); - double[] testAngles3 = getJointAngles(0, 75, 0); - System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); + // double[] testAngles3 = getJointAngles(0, 75, 0); + // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); - double[] testAngles4 = getJointAngles(75, 0, 0); - System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); + // double[] testAngles4 = getJointAngles(75, 0, 0); + // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); - double[] testAngles5 = getJointAngles(-75, 0, 0); - System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); + // double[] testAngles5 = getJointAngles(-75, 0, 0); + // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); - double[] testAngles6 = getJointAngles(60, 25, 0); - System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); + // 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); setJointAngles(jointAngles); From 38387e317346d4892cab22ca7460bcc08053b0e4 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Mon, 24 Jan 2022 19:09:20 -0700 Subject: [PATCH 17/55] Added limits --- src/main/java/frc4388/robot/Constants.java | 5 +++++ src/main/java/frc4388/robot/subsystems/Climber.java | 6 ++++++ 2 files changed, 11 insertions(+) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4076a30..c6b23bf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -93,6 +93,11 @@ public final class Constants { public static final double SHOULDER_RESTING_ANGLE = 0; public static final double ELBOW_RESTING_ANGLE = 0; + 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; + // PID Constants public static final int SHOULDER_SLOT_IDX = 0; public static final int SHOULDER_PID_LOOP_IDX = 1; diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 7f9b883..374f44f 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -165,12 +165,18 @@ public class Climber extends SubsystemBase { } public void setJointAngles(double shoulderAngle, double 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; + shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO; + elbowAngle *= ClimberConstants.GEAR_BOX_RATIO; + shoulderPosition += m_shoulderOffset; elbowPosition += m_elbowOffset; From 2502f71ecdca5ac207d4708bcdd4bf753199a8f8 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Mon, 24 Jan 2022 19:20:07 -0700 Subject: [PATCH 18/55] Added more limits --- src/main/java/frc4388/robot/subsystems/Climber.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 374f44f..0e55952 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -165,6 +165,9 @@ public class Climber extends SubsystemBase { } 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; From e9fc3354d8cbfea29a8ade3a5c2de8af2633b8e5 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Wed, 26 Jan 2022 17:41:52 -0700 Subject: [PATCH 19/55] Switched hook motors --- .../java/frc4388/robot/subsystems/Hooks.java | 25 ++++--- vendordeps/REVLib.json | 73 +++++++++++++++++++ 2 files changed, 88 insertions(+), 10 deletions(-) create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java index 555bec2..8eb2dfd 100644 --- a/src/main/java/frc4388/robot/subsystems/Hooks.java +++ b/src/main/java/frc4388/robot/subsystems/Hooks.java @@ -1,31 +1,36 @@ package frc4388.robot.subsystems; -import edu.wpi.first.wpilibj.Servo; +import com.revrobotics.CANSparkMax; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.HooksConstants; public class Hooks extends SubsystemBase { - private Servo m_leftHook; - private Servo m_rightHook; + private CANSparkMax m_leftHook; + private CANSparkMax m_rightHook; + + double m_leftOffset; + double m_rightOffset; private boolean m_open; - public Hooks(Servo leftHook, Servo rightHook) { + public Hooks(CANSparkMax leftHook, CANSparkMax rightHook) { m_leftHook = leftHook; m_rightHook = rightHook; - m_open = false; - setOpen(m_open); + setOpen(false); } public void setOpen(boolean open) { if(open) { - m_leftHook.setPosition(HooksConstants.OPEN_POSITION); - m_rightHook.setPosition(HooksConstants.OPEN_POSITION); + m_leftHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_leftOffset); + m_rightHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_rightOffset); } else { - m_leftHook.setPosition(HooksConstants.CLOSE_POSITION); - m_rightHook.setPosition(HooksConstants.CLOSE_POSITION); + m_leftHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_leftOffset); + m_rightHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_rightOffset); } + + m_open = open; } public boolean getOpen() { diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..997e2a4 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +} \ No newline at end of file From f9a12a3b7af661323704a061c7a63985530569a4 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Fri, 28 Jan 2022 16:27:11 -0700 Subject: [PATCH 20/55] Added limit switches and auto open --- .../java/frc4388/robot/subsystems/Hooks.java | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java index 8eb2dfd..de07e93 100644 --- a/src/main/java/frc4388/robot/subsystems/Hooks.java +++ b/src/main/java/frc4388/robot/subsystems/Hooks.java @@ -1,5 +1,6 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -9,16 +10,23 @@ public class Hooks extends SubsystemBase { private CANSparkMax m_leftHook; private CANSparkMax m_rightHook; - double m_leftOffset; - double m_rightOffset; + private LimitSwitchNormal m_limitSwitch; + + private double m_leftOffset; + private double m_rightOffset; private boolean m_open; - public Hooks(CANSparkMax leftHook, CANSparkMax rightHook) { + public Hooks(CANSparkMax leftHook, CANSparkMax rightHook, LimitSwitchNormal limitSwitch) { m_leftHook = leftHook; m_rightHook = rightHook; - setOpen(false); + m_limitSwitch = limitSwitch; + + m_open = false; + + m_leftHook.set(.1); + m_rightHook.set(.1); } public void setOpen(boolean open) { @@ -36,4 +44,12 @@ public class Hooks extends SubsystemBase { public boolean getOpen() { return m_open; } + + @Override + public void periodic() { + if(m_limitSwitch.compareTo(LimitSwitchNormal.NormallyClosed) == 1) { + m_leftOffset = m_leftHook.getEncoder().getPosition(); + m_leftOffset = m_leftHook.getEncoder().getPosition(); + } + } } From a4393d9f0210f0d52178f10a3c5cce9dcadda4ea Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Fri, 28 Jan 2022 16:40:49 -0700 Subject: [PATCH 21/55] configured input --- src/main/java/frc4388/robot/Constants.java | 4 ++++ .../java/frc4388/robot/RobotContainer.java | 8 ++++---- src/main/java/frc4388/robot/RobotMap.java | 12 +++++++++-- .../java/frc4388/robot/subsystems/Hooks.java | 20 +++++++++++-------- 4 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c6b23bf..a0dcc1e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -114,9 +114,13 @@ public final class Constants { public static final class HooksConstants { public static final int LEFT_HOOK_ID = -1; public static final int RIGHT_HOOK_ID = -1; + public static final int LEFT_LIMIT_ID = -1; + public static final int RIGHT_LIMIT_ID = -1; public static final double OPEN_POSITION = 0; public static final double CLOSE_POSITION = 0; + + public static final double CALIBRATION_SPEED = 0; } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b8923b..4d2bad0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -45,7 +45,7 @@ public class RobotContainer { private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - //private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); + private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook, m_robotMap.leftHookLimit, m_robotMap.rightHookLimit); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -87,9 +87,9 @@ public class RobotContainer { .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - // .whenPressed(() -> m_hooks.setOpen(true)) - // .whenReleased(() -> m_hooks.setOpen(false)); + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(() -> m_hooks.setOpen(true)) + .whenReleased(() -> m_hooks.setOpen(false)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 6de2f65..6483ebc 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,10 +4,15 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.CAN; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; @@ -102,6 +107,9 @@ public class RobotMap { public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO /* Hooks Subsystem */ - //public final Servo leftHook = new Servo(HooksConstants.LEFT_HOOK_ID); - //public final Servo rightHook = new Servo(HooksConstants.RIGHT_HOOK_ID); + public final CANSparkMax leftHook = new CANSparkMax(HooksConstants.LEFT_HOOK_ID, MotorType.kBrushless); + public final CANSparkMax rightHook = new CANSparkMax(HooksConstants.RIGHT_HOOK_ID, MotorType.kBrushless); + + public final DigitalInput leftHookLimit = new DigitalInput(HooksConstants.LEFT_LIMIT_ID); + public final DigitalInput rightHookLimit = new DigitalInput(HooksConstants.RIGHT_LIMIT_ID); } diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java index de07e93..0348ccb 100644 --- a/src/main/java/frc4388/robot/subsystems/Hooks.java +++ b/src/main/java/frc4388/robot/subsystems/Hooks.java @@ -3,6 +3,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.HooksConstants; @@ -10,23 +11,25 @@ public class Hooks extends SubsystemBase { private CANSparkMax m_leftHook; private CANSparkMax m_rightHook; - private LimitSwitchNormal m_limitSwitch; + private DigitalInput m_leftLimitSwitch; + private DigitalInput m_rightLimitSwitch; private double m_leftOffset; private double m_rightOffset; private boolean m_open; - public Hooks(CANSparkMax leftHook, CANSparkMax rightHook, LimitSwitchNormal limitSwitch) { + public Hooks(CANSparkMax leftHook, CANSparkMax rightHook, DigitalInput leftLimitSwitch, DigitalInput rightLimitSwitch) { m_leftHook = leftHook; m_rightHook = rightHook; - m_limitSwitch = limitSwitch; + m_leftLimitSwitch = leftLimitSwitch; + m_rightLimitSwitch = rightLimitSwitch; m_open = false; - m_leftHook.set(.1); - m_rightHook.set(.1); + m_leftHook.set(HooksConstants.CALIBRATION_SPEED); + m_rightHook.set(HooksConstants.CALIBRATION_SPEED); } public void setOpen(boolean open) { @@ -47,9 +50,10 @@ public class Hooks extends SubsystemBase { @Override public void periodic() { - if(m_limitSwitch.compareTo(LimitSwitchNormal.NormallyClosed) == 1) { + if(m_leftLimitSwitch.get()) m_leftOffset = m_leftHook.getEncoder().getPosition(); - m_leftOffset = m_leftHook.getEncoder().getPosition(); - } + + if(m_rightLimitSwitch.get()) + m_rightOffset = m_rightHook.getEncoder().getPosition(); } } From fcf33e7dbe18ead776e83bb8a5aa522f0014b140 Mon Sep 17 00:00:00 2001 From: 66945 <66945@psdschools.org> Date: Fri, 28 Jan 2022 17:14:05 -0700 Subject: [PATCH 22/55] updated deprecated class --- src/main/java/frc4388/robot/Constants.java | 2 -- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 3 --- .../java/frc4388/robot/subsystems/Hooks.java | 17 ++++++++++------- 4 files changed, 11 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a0dcc1e..df8d86b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -114,8 +114,6 @@ public final class Constants { public static final class HooksConstants { public static final int LEFT_HOOK_ID = -1; public static final int RIGHT_HOOK_ID = -1; - public static final int LEFT_LIMIT_ID = -1; - public static final int RIGHT_LIMIT_ID = -1; public static final double OPEN_POSITION = 0; public static final double CLOSE_POSITION = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4d2bad0..8f7cd6a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -45,7 +45,7 @@ public class RobotContainer { private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook, m_robotMap.leftHookLimit, m_robotMap.rightHookLimit); + private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 6483ebc..a1ddadb 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -109,7 +109,4 @@ public class RobotMap { /* Hooks Subsystem */ public final CANSparkMax leftHook = new CANSparkMax(HooksConstants.LEFT_HOOK_ID, MotorType.kBrushless); public final CANSparkMax rightHook = new CANSparkMax(HooksConstants.RIGHT_HOOK_ID, MotorType.kBrushless); - - public final DigitalInput leftHookLimit = new DigitalInput(HooksConstants.LEFT_LIMIT_ID); - public final DigitalInput rightHookLimit = new DigitalInput(HooksConstants.RIGHT_LIMIT_ID); } diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java index 0348ccb..6c6aebf 100644 --- a/src/main/java/frc4388/robot/subsystems/Hooks.java +++ b/src/main/java/frc4388/robot/subsystems/Hooks.java @@ -2,6 +2,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -11,20 +12,22 @@ public class Hooks extends SubsystemBase { private CANSparkMax m_leftHook; private CANSparkMax m_rightHook; - private DigitalInput m_leftLimitSwitch; - private DigitalInput m_rightLimitSwitch; + private SparkMaxLimitSwitch m_leftLimitSwitch; + private SparkMaxLimitSwitch m_rightLimitSwitch; private double m_leftOffset; private double m_rightOffset; private boolean m_open; - public Hooks(CANSparkMax leftHook, CANSparkMax rightHook, DigitalInput leftLimitSwitch, DigitalInput rightLimitSwitch) { + public Hooks(CANSparkMax leftHook, CANSparkMax rightHook) { m_leftHook = leftHook; m_rightHook = rightHook; - m_leftLimitSwitch = leftLimitSwitch; - m_rightLimitSwitch = rightLimitSwitch; + m_leftLimitSwitch = m_leftHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_rightLimitSwitch = m_rightHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_leftLimitSwitch.enableLimitSwitch(true); + m_rightLimitSwitch.enableLimitSwitch(true); m_open = false; @@ -50,10 +53,10 @@ public class Hooks extends SubsystemBase { @Override public void periodic() { - if(m_leftLimitSwitch.get()) + if(m_leftLimitSwitch.isPressed()) m_leftOffset = m_leftHook.getEncoder().getPosition(); - if(m_rightLimitSwitch.get()) + if(m_rightLimitSwitch.isPressed()) m_rightOffset = m_rightHook.getEncoder().getPosition(); } } From 6b131e23421d5be7d3f2d97fbcbca21a02918311 Mon Sep 17 00:00:00 2001 From: Raghav66296 <90011037+Raghav66296@users.noreply.github.com> Date: Fri, 18 Feb 2022 20:53:34 -0700 Subject: [PATCH 23/55] Rotation Matrix + Review robotAngle Code --- .../java/frc4388/robot/subsystems/Climber.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 0e55952..aa5da1e 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -15,6 +15,7 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; @@ -28,6 +29,7 @@ public class Climber extends SubsystemBase { private WPI_PigeonIMU m_gyro; private boolean m_groundRelative; + private double m_robotAngle; private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; @@ -136,11 +138,20 @@ public class Climber extends SubsystemBase { angles[1] = elbowAngle; return angles; } - +/* Rotation Matrix + 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)] + Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix + Rotation Matrix video: https://youtu.be/Ta8cKqltPfU + */ public double getRobotTilt() { double[] ypr = new double[3]; m_gyro.getYawPitchRoll(ypr); + + return Math.toRadians(ypr[1]); // Pitch + // multiply by pie and then divide by 180 + } public void setClimberGains() { @@ -216,4 +227,9 @@ public class Climber extends SubsystemBase { double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles); } + public void setRobotAngle(double robotAngle) { + m_robotAngle = robotAngle; + m_robotAngle = 45; + } + } From e7358d7eb55b19c8bf73a8c8f539808289e75cd7 Mon Sep 17 00:00:00 2001 From: Raghav66296 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 22 Feb 2022 19:47:31 -0700 Subject: [PATCH 24/55] rotation matrix started + unsure code --- src/main/java/frc4388/robot/Constants.java | 4 +++ .../frc4388/robot/subsystems/Climber.java | 31 ++++++++++++++++++- 2 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index df8d86b..b79a13d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -109,6 +109,10 @@ public final class Constants { public static final Gains ELBOW_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0); public static final int CLIMBER_TIMEOUT_MS = 50; + + /* TODO: Update Constants */ + public static final double ROBOT_ANGLE_ID = 0; + } public static final class HooksConstants { diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index aa5da1e..2105e92 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -15,6 +15,7 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; @@ -30,6 +31,8 @@ public class Climber extends SubsystemBase { private WPI_PigeonIMU m_gyro; private boolean m_groundRelative; private double m_robotAngle; + private double m_robotPosition; + private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; @@ -147,6 +150,29 @@ public class Climber extends SubsystemBase { public double getRobotTilt() { double[] ypr = new double[3]; m_gyro.getYawPitchRoll(ypr); + double theta = 0; + + // double sin; + // double cos; + // xsin = 0; //placeholder for sin, cos + // ysin = 0; + // xcos = 0; + // ycos = 0; + double[][] rotMax = { + {Math.cos(theta) - Math.sin(theta), 0 }, + {Math.sin(theta) + Math.cos(theta), 0}, + {0, 0, 1} + + }; + + if (m_robotPosition < 45){ + + setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle); + } + if( m_robotPosition > 45){ + setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle); + } + return Math.toRadians(ypr[1]); // Pitch @@ -227,9 +253,12 @@ public class Climber extends SubsystemBase { double[] jointAngles = getJointAngles(m_position[0], m_position[1], tiltAngle); setJointAngles(jointAngles); } - public void setRobotAngle(double robotAngle) { + + public void setRobotAngle(double robotAngle, double robotPosition) { + m_robotPosition = robotPosition; m_robotAngle = robotAngle; m_robotAngle = 45; + } } From 6a639544ae572dfb06af16862cdd76f7d904a71d Mon Sep 17 00:00:00 2001 From: Ryan Manley Date: Sat, 5 Mar 2022 14:21:48 -0700 Subject: [PATCH 25/55] hook fixes --- src/main/java/frc4388/robot/Constants.java | 11 +-- .../java/frc4388/robot/RobotContainer.java | 27 ++++--- src/main/java/frc4388/robot/RobotMap.java | 24 +++--- .../java/frc4388/robot/subsystems/Claws.java | 76 +++++++++++++++++++ .../frc4388/robot/subsystems/Climber.java | 17 ++--- .../java/frc4388/robot/subsystems/Hooks.java | 62 --------------- 6 files changed, 114 insertions(+), 103 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Claws.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Hooks.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b79a13d..cfc8496 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -80,7 +80,7 @@ public final class Constants { public static final int SHOULDER_ID = 1; public static final int ELBOW_ID = 3; public static final int GYRO_ID = -1; - + // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm public static final double LOWER_ARM_LENGTH = 27; @@ -110,14 +110,15 @@ public final class Constants { public static final int CLIMBER_TIMEOUT_MS = 50; - /* TODO: Update Constants */ + // TODO: Update Constants + // Robot Angle public static final double ROBOT_ANGLE_ID = 0; } - public static final class HooksConstants { - public static final int LEFT_HOOK_ID = -1; - public static final int RIGHT_HOOK_ID = -1; + public static final class ClawConstants { + public static final int LEFT_CLAW_ID = 44; + public static final int RIGHT_CLAW_ID = 45; public static final double OPEN_POSITION = 0; public static final double CLOSE_POSITION = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8f7cd6a..1b15a07 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,8 +10,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.subsystems.Claws; import frc4388.robot.subsystems.Climber; -import frc4388.robot.subsystems.Hooks; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -29,7 +29,7 @@ public class RobotContainer { /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); - /* Subsystems */ + /* Subsystems *//* private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, @@ -39,13 +39,13 @@ public class RobotContainer { m_robotMap.rightFrontEncoder, m_robotMap.leftBackEncoder, m_robotMap.rightBackEncoder - ); + );*/ - private final LED m_robotLED = new LED(m_robotMap.LEDController); + /*private final LED m_robotLED = new LED(m_robotMap.LEDController); private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - - private final Hooks m_hooks = new Hooks(m_robotMap.leftHook, m_robotMap.rightHook); + */ + private final Claws m_claws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -64,12 +64,12 @@ public class RobotContainer { // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); // moves climber in xy space with two-axis input from the operator controller - m_robotClimber.setDefaultCommand( + /*m_robotClimber.setDefaultCommand( new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(), getOperatorController().getLeftYAxis()), m_robotClimber)); // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));*/ } /** @@ -83,13 +83,16 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + /*new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));*/ new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(() -> m_hooks.setOpen(true)) - .whenReleased(() -> m_hooks.setOpen(false)); + .whenPressed(() -> m_claws.setSpeed(0.5)) + .whenReleased(() -> m_claws.setSpeed(0.0)); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .whenPressed(() -> m_claws.setSpeed(-0.5)) + .whenReleased(() -> m_claws.setSpeed(0.0)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a1ddadb..bfa738e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.ClimberConstants; -import frc4388.robot.Constants.HooksConstants; +import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -27,18 +27,18 @@ import frc4388.robot.Constants.SwerveDriveConstants; public class RobotMap { public RobotMap() { - configureLEDMotorControllers(); - configureSwerveMotorControllers(); + //configureLEDMotorControllers(); + //configureSwerveMotorControllers(); } /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDMotorControllers() { + /*void configureLEDMotorControllers() { - } + }*/ - /* Swerve Subsystem */ + /* Swerve Subsystem *//* public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); @@ -99,14 +99,14 @@ public class RobotMap { //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } + }/* /* Climb Subsystem */ - public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO + /*public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO - public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO + public final WPI_PigeonIMU gyro = null;//new WPI_PigeonIMU(ClimberConstants.GYRO_ID); // TODO*/ /* Hooks Subsystem */ - public final CANSparkMax leftHook = new CANSparkMax(HooksConstants.LEFT_HOOK_ID, MotorType.kBrushless); - public final CANSparkMax rightHook = new CANSparkMax(HooksConstants.RIGHT_HOOK_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); } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java new file mode 100644 index 0000000..d76c7c1 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -0,0 +1,76 @@ +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ClawConstants; + +public class Claws extends SubsystemBase { + private CANSparkMax m_leftClaw; + private 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_rightOffset; + + private boolean m_open; + + public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) { + m_leftClaw = leftClaw; + m_rightClaw = rightClaw; + + m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol + m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + + m_leftLimitSwitchF.enableLimitSwitch(true); + m_rightLimitSwitchF.enableLimitSwitch(true); + m_leftLimitSwitchR.enableLimitSwitch(true); + m_rightLimitSwitchR.enableLimitSwitch(true); + leftClaw.setInverted(true); + rightClaw.setInverted(true); + + m_open = false; + + // m_leftClaw.set(ClawConstants.CALIBRATION_SPEED); + // m_rightClaw.set(ClawConstants.CALIBRATION_SPEED); + } + + public void setSpeed(double speed){ + m_leftClaw.set(speed); + m_rightClaw.set(speed); + } + + public void setOpen(boolean open) { + if(open) { + m_leftClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_leftOffset); + m_rightClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_rightOffset); + } else { + m_leftClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_leftOffset); + m_rightClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_rightOffset); + } + + m_open = open; + } + + public boolean getOpen() { + return m_open; + } + + @Override + public void periodic() { + if(m_leftLimitSwitchR.isPressed()) + m_leftOffset = m_leftClaw.getEncoder().getPosition(); + + if(m_rightLimitSwitchR.isPressed()) + m_rightOffset = m_rightClaw.getEncoder().getPosition(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 2105e92..0956280 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -32,6 +32,7 @@ public class Climber extends SubsystemBase { private boolean m_groundRelative; private double m_robotAngle; private double m_robotPosition; + private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; @@ -162,18 +163,11 @@ public class Climber extends SubsystemBase { {Math.cos(theta) - Math.sin(theta), 0 }, {Math.sin(theta) + Math.cos(theta), 0}, {0, 0, 1} - }; - if (m_robotPosition < 45){ - - setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle); + if (m_robotPosition < m_robotAngle || m_robotPosition > m_robotAngle){ + setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); } - if( m_robotPosition > 45){ - setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, m_robotAngle); - } - - return Math.toRadians(ypr[1]); // Pitch // multiply by pie and then divide by 180 @@ -254,11 +248,10 @@ public class Climber extends SubsystemBase { setJointAngles(jointAngles); } - public void setRobotAngle(double robotAngle, double robotPosition) { + public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { m_robotPosition = robotPosition; m_robotAngle = robotAngle; - m_robotAngle = 45; - + m_robotAngle = 45; //45 is placeholder } } diff --git a/src/main/java/frc4388/robot/subsystems/Hooks.java b/src/main/java/frc4388/robot/subsystems/Hooks.java deleted file mode 100644 index 6c6aebf..0000000 --- a/src/main/java/frc4388/robot/subsystems/Hooks.java +++ /dev/null @@ -1,62 +0,0 @@ -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkMaxLimitSwitch; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.HooksConstants; - -public class Hooks extends SubsystemBase { - private CANSparkMax m_leftHook; - private CANSparkMax m_rightHook; - - private SparkMaxLimitSwitch m_leftLimitSwitch; - private SparkMaxLimitSwitch m_rightLimitSwitch; - - private double m_leftOffset; - private double m_rightOffset; - - private boolean m_open; - - public Hooks(CANSparkMax leftHook, CANSparkMax rightHook) { - m_leftHook = leftHook; - m_rightHook = rightHook; - - m_leftLimitSwitch = m_leftHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_rightLimitSwitch = m_rightHook.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_leftLimitSwitch.enableLimitSwitch(true); - m_rightLimitSwitch.enableLimitSwitch(true); - - m_open = false; - - m_leftHook.set(HooksConstants.CALIBRATION_SPEED); - m_rightHook.set(HooksConstants.CALIBRATION_SPEED); - } - - public void setOpen(boolean open) { - if(open) { - m_leftHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_leftOffset); - m_rightHook.getEncoder().setPosition(HooksConstants.OPEN_POSITION + m_rightOffset); - } else { - m_leftHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_leftOffset); - m_rightHook.getEncoder().setPosition(HooksConstants.CLOSE_POSITION + m_rightOffset); - } - - m_open = open; - } - - public boolean getOpen() { - return m_open; - } - - @Override - public void periodic() { - if(m_leftLimitSwitch.isPressed()) - m_leftOffset = m_leftHook.getEncoder().getPosition(); - - if(m_rightLimitSwitch.isPressed()) - m_rightOffset = m_rightHook.getEncoder().getPosition(); - } -} From 11359e4c02268893fab3d072ebe98c960faa77e1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 14:38:54 -0700 Subject: [PATCH 26/55] run claw and claw stuff (vcool) --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 21 ++++-- .../java/frc4388/robot/commands/RunClaw.java | 57 +++++++++++++++ .../java/frc4388/robot/subsystems/Claws.java | 73 ++++++++++++++++--- 4 files changed, 139 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunClaw.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cfc8496..e3508db 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -124,6 +124,8 @@ public final class Constants { public static final double CLOSE_POSITION = 0; public static final double CALIBRATION_SPEED = 0; + + public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit; } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b15a07..b74b0b4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; +import frc4388.robot.commands.RunClaw; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Claws.ClawType; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -86,13 +88,22 @@ public class RobotContainer { /*new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));*/ - + + // run claws new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(() -> m_claws.setSpeed(0.5)) - .whenReleased(() -> m_claws.setSpeed(0.0)); + .whenPressed(new RunClaw(m_claws, ClawType.LEFT, true)) + .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, true)); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whenPressed(() -> m_claws.setSpeed(-0.5)) - .whenReleased(() -> m_claws.setSpeed(0.0)); + .whenPressed(new RunClaw(m_claws, ClawType.LEFT, false)) + .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, false)); + + // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + // .whenPressed(() -> m_claws.setSpeed(0.5)) + // .whenReleased(() -> m_claws.setSpeed(0.0)); + // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + // .whenPressed(() -> m_claws.setSpeed(-0.5)) + // .whenReleased(() -> m_claws.setSpeed(0.0)); } /** diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java new file mode 100644 index 0000000..8f2cfa0 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -0,0 +1,57 @@ +// 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.Constants.ClawConstants; +import frc4388.robot.subsystems.Claws; + +public class RunClaw extends CommandBase { + + // parameters + public Claws m_claws; + public Claws.ClawType clawType; + public boolean open; + + // current limit + public double currentLimit; + + /** + * Creates a new RunClaw, which runs a claw. + * @param sClaws Claws subsystem. + * @param which Which claw to run. + * @param open Whether to open or close the claw. + */ + public RunClaw(Claws sClaws, Claws.ClawType which, boolean open) { + // Use addRequirements() here to declare subsystem dependencies. + m_claws = sClaws; + clawType = which; + this.open = open; + + currentLimit = ClawConstants.CURRENT_LIMIT; + + addRequirements(m_claws); + } + + // 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() { + m_claws.runClaw(clawType, open); + } + + // 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 m_claws.checkSwitchAndCurrent(clawType, currentLimit); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index d76c7c1..cbebc47 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -9,8 +9,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClawConstants; public class Claws extends SubsystemBase { - private CANSparkMax m_leftClaw; - private CANSparkMax m_rightClaw; + public CANSparkMax m_leftClaw; + public CANSparkMax m_rightClaw; private SparkMaxLimitSwitch m_leftLimitSwitchF; private SparkMaxLimitSwitch m_rightLimitSwitchF; @@ -21,6 +21,7 @@ public class Claws extends SubsystemBase { private double m_rightOffset; private boolean m_open; + public static enum ClawType {LEFT, RIGHT} public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) { m_leftClaw = leftClaw; @@ -49,28 +50,82 @@ public class Claws extends SubsystemBase { m_rightClaw.set(speed); } + /** + * Run a specific claw to open or close. + * @param which Which claw to run. + * @param open Whether to open or close the claw. + */ + public void runClaw(ClawType which, boolean open) { + if (which == Claws.ClawType.LEFT) { + + // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; + // m_leftClaw.getEncoder().setPosition(setPos); + m_leftClaw.set(0.1); + + } else if (which == Claws.ClawType.RIGHT) { + + // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; + // m_rightClaw.getEncoder().setPosition(setPos); + m_rightClaw.set(0.1); + } + } + public void setOpen(boolean open) { if(open) { - m_leftClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_leftOffset); - m_rightClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_rightOffset); + // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); + // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); + m_leftClaw.set(0.1); + m_rightClaw.set(0.1); } else { - m_leftClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_leftOffset); - m_rightClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_rightOffset); + // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); + // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); + m_leftClaw.set(-0.1); + m_rightClaw.set(-0.1); } m_open = open; } + public double[] getOffsets() { + return new double[] {m_leftOffset, m_rightOffset}; + } + public boolean getOpen() { return m_open; } + /** + * Check if a limit switch is pressed or current limit exceeded for a claw. + * @param which Which claw to check. + * @param limit The current limit. + * @return Whether to interrupt the RunClaw command or not. + */ + public boolean checkSwitchAndCurrent(ClawType which, double limit) { + + // if still calibrating, stop RunClaw + /*if (((Double) m_leftOffset == null) || ((Double) m_rightOffset == null)) { + return true; + }*/ + + if (which == ClawType.LEFT) { + if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= limit) { + return true; + } + } else if (which == ClawType.RIGHT) { + if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= limit) { + return true; + } + } + + return false; + } + @Override public void periodic() { - if(m_leftLimitSwitchR.isPressed()) + if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) m_leftOffset = m_leftClaw.getEncoder().getPosition(); - if(m_rightLimitSwitchR.isPressed()) + if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) m_rightOffset = m_rightClaw.getEncoder().getPosition(); } -} +} \ No newline at end of file From 5f88e6862234d778fb8eb9e822aa6e3218cacba7 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 17:56:04 -0700 Subject: [PATCH 27/55] some changes to make it build --- .../java/frc4388/robot/subsystems/Climber.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 0956280..2dcc7fe 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -177,17 +177,17 @@ public class Climber extends SubsystemBase { public void setClimberGains() { // shoulder PIDs m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kD, 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_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); // elbow PIDs m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kD, 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_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 setJointAngles(double[] angles) { From 0a0082e8231997626f336c224e70411dccb07685 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 18:01:09 -0700 Subject: [PATCH 28/55] code builds now --- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1c8f7dc..5e4881a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -96,7 +96,7 @@ public class RobotContainer { public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - private final LED m_robotLED = new LED(m_robotMap.LEDController); + // private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); @@ -130,8 +130,8 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftXAxis(), - getOperatorController().getLeftYAxis()), m_robotClimber)); + new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), + getOperatorController().getLeftY()), m_robotClimber)); // Turret default command // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, From 20353cdbe049c609a499bc51bba45e35c06d8d85 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 23:07:12 -0700 Subject: [PATCH 29/55] runClaw fixes --- src/main/java/frc4388/robot/subsystems/Claws.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index cbebc47..0f79620 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -56,18 +56,23 @@ public class Claws extends SubsystemBase { * @param open Whether to open or close the claw. */ public void runClaw(ClawType which, boolean open) { + + int direction = open ? 1 : -1; + if (which == Claws.ClawType.LEFT) { // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; // m_leftClaw.getEncoder().setPosition(setPos); - m_leftClaw.set(0.1); + m_leftClaw.set(direction * 0.1); } else if (which == Claws.ClawType.RIGHT) { // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; // m_rightClaw.getEncoder().setPosition(setPos); - m_rightClaw.set(0.1); + m_rightClaw.set(direction * 0.1); } + + m_open = open; } public void setOpen(boolean open) { From 772bd4d8dd405676e619b5669c713133c661fd8d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 23:16:29 -0700 Subject: [PATCH 30/55] current limit is constant --- src/main/java/frc4388/robot/commands/RunClaw.java | 7 +------ src/main/java/frc4388/robot/subsystems/Claws.java | 6 +++--- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java index 8f2cfa0..2cf76af 100644 --- a/src/main/java/frc4388/robot/commands/RunClaw.java +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -15,9 +15,6 @@ public class RunClaw extends CommandBase { public Claws.ClawType clawType; public boolean open; - // current limit - public double currentLimit; - /** * Creates a new RunClaw, which runs a claw. * @param sClaws Claws subsystem. @@ -29,8 +26,6 @@ public class RunClaw extends CommandBase { m_claws = sClaws; clawType = which; this.open = open; - - currentLimit = ClawConstants.CURRENT_LIMIT; addRequirements(m_claws); } @@ -52,6 +47,6 @@ public class RunClaw extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return m_claws.checkSwitchAndCurrent(clawType, currentLimit); + return m_claws.checkSwitchAndCurrent(clawType); } } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 0f79620..7476238 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -105,7 +105,7 @@ public class Claws extends SubsystemBase { * @param limit The current limit. * @return Whether to interrupt the RunClaw command or not. */ - public boolean checkSwitchAndCurrent(ClawType which, double limit) { + public boolean checkSwitchAndCurrent(ClawType which) { // if still calibrating, stop RunClaw /*if (((Double) m_leftOffset == null) || ((Double) m_rightOffset == null)) { @@ -113,11 +113,11 @@ public class Claws extends SubsystemBase { }*/ if (which == ClawType.LEFT) { - if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= limit) { + if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { return true; } } else if (which == ClawType.RIGHT) { - if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= limit) { + if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { return true; } } From 33f59d4931b29bb3646462f79c203d427c2a5df8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 23:43:15 -0700 Subject: [PATCH 31/55] some changes --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/subsystems/Climber.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5e4881a..b3f3f3d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -131,7 +131,7 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), - getOperatorController().getLeftY()), m_robotClimber)); + getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Turret default command // m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 2dcc7fe..d952561 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -165,7 +165,7 @@ public class Climber extends SubsystemBase { {0, 0, 1} }; - if (m_robotPosition < m_robotAngle || m_robotPosition > m_robotAngle){ + if (m_robotPosition != m_robotAngle){ setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); } From 9fe5dfd08610b9867480682a3252c0fdd939e14e Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 5 Mar 2022 23:43:29 -0700 Subject: [PATCH 32/55] stuff --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/subsystems/Claws.java | 12 +++--------- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1504977..554f1cf 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -131,7 +131,7 @@ public final class Constants { /* TODO: Update motor IDS */ public static final int SHOULDER_ID = 1; public static final int ELBOW_ID = 3; - public static final int GYRO_ID = -1; + public static final int GYRO_ID = 14; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 7476238..66d84ec 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -105,23 +105,17 @@ public class Claws extends SubsystemBase { * @param limit The current limit. * @return Whether to interrupt the RunClaw command or not. */ - public boolean checkSwitchAndCurrent(ClawType which) { - - // if still calibrating, stop RunClaw - /*if (((Double) m_leftOffset == null) || ((Double) m_rightOffset == null)) { - return true; - }*/ - + public boolean checkSwitchAndCurrent(ClawType which) { if (which == ClawType.LEFT) { if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { 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) { return true; } } - return false; } From 92a453517e0eee48a8b08d4862b2319a0c5fdf78 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 5 Mar 2022 23:47:24 -0700 Subject: [PATCH 33/55] sadd --- src/main/java/frc4388/robot/RobotContainer.java | 8 +------- src/main/java/frc4388/robot/subsystems/Claws.java | 5 ++++- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b3f3f3d..d7754ae 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -146,6 +146,7 @@ public class RobotContainer { true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED // .setDefaultCommand(new RunCommand(m_robotLED::updateLED, @@ -194,13 +195,6 @@ public class RobotContainer { .whenPressed(new RunClaw(m_claws, ClawType.LEFT, false)) .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, false)); - // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - // .whenPressed(() -> m_claws.setSpeed(0.5)) - // .whenReleased(() -> m_claws.setSpeed(0.0)); - // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - // .whenPressed(() -> m_claws.setSpeed(-0.5)) - // .whenReleased(() -> m_claws.setSpeed(0.0)); - /* * new JoystickButton(getOperatorController(), XboxController.Button.kB.value) * .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 66d84ec..cb1bb93 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -74,7 +74,10 @@ public class Claws extends SubsystemBase { m_open = open; } - + /** + * Sets the state of both hooks + * @param open The state of the hooks + */ public void setOpen(boolean open) { if(open) { // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); From 42bfaa7027dad2f0de6a85f4ab7c2fffc00597d9 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 5 Mar 2022 23:51:38 -0700 Subject: [PATCH 34/55] asd --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aefc3b4..59daacc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -100,7 +100,7 @@ public class RobotContainer { private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); - private final LED m_robotLED = new LED(m_robotMap.LEDController); + //private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); From b5345a53cbc55dd44370a9b6f6613b5c720c828f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 6 Mar 2022 12:57:41 -0700 Subject: [PATCH 35/55] climber default command not IK --- src/main/java/frc4388/robot/RobotContainer.java | 9 +++++++-- src/main/java/frc4388/robot/subsystems/Climber.java | 12 ++++++++---- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c4ae0a2..865208a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,8 +137,13 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), - getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), getOperatorController().getLeftY()), + m_robotClimber)); + + // IK command + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), + // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Turret default command //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index d952561..23478db 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -22,8 +22,8 @@ import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { - private WPI_TalonFX m_shoulder; - private WPI_TalonFX m_elbow; + public WPI_TalonFX m_shoulder; + public WPI_TalonFX m_elbow; private double m_shoulderOffset; private double m_elbowOffset; @@ -33,8 +33,6 @@ public class Climber extends SubsystemBase { private double m_robotAngle; private double m_robotPosition; - - private double[] m_position = {ClimberConstants.MIN_ARM_LENGTH, 0.d}; public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro, boolean groundRelative) { @@ -142,6 +140,12 @@ public class Climber extends SubsystemBase { angles[1] = elbowAngle; return angles; } + +public void setMotors(double shoulderOutput, double elbowOutput) { + m_shoulder.set(shoulderOutput); + m_elbow.set(elbowOutput); +} + /* Rotation Matrix 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)] From 2b0dd5336036769afaaefbd8884d4a7192a4749d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 10 Mar 2022 17:06:17 -0700 Subject: [PATCH 36/55] TestButtons + soft limits --- src/main/java/frc4388/robot/Constants.java | 5 +++++ .../java/frc4388/robot/RobotContainer.java | 21 +++++++++++++------ .../java/frc4388/robot/subsystems/Claws.java | 21 +++++++++++++++++++ .../frc4388/robot/subsystems/Climber.java | 17 +++++++++++++++ 4 files changed, 58 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c2a3108..64f285c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -169,6 +169,11 @@ public final class Constants { public static final double GEAR_BOX_RATIO = 144.d * 60.d / 24.d; + public static final double SHOULDER_SOFT_LIMIT_FORWARD = 0; + public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0; + public static final double ELBOW_SOFT_LIMIT_FORWARD = 0; + public static final double ELBOW_SOFT_LIMIT_REVERSE = 0; + // PID Constants public static final int SHOULDER_SLOT_IDX = 0; public static final int SHOULDER_PID_LOOP_IDX = 1; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 865208a..a48646f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -94,7 +94,7 @@ public class RobotContainer { /* Subsystems */ private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - private final Claws m_claws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); + private 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); @@ -137,9 +137,10 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), getOperatorController().getLeftY()), + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), m_robotClimber)); + // IK command // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), @@ -217,12 +218,20 @@ public class RobotContainer { /* Operator Buttons */ // run claws new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new RunClaw(m_claws, ClawType.LEFT, true)) - .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, true)); + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new RunClaw(m_claws, ClawType.LEFT, false)) - .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, false)); + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); /* * // activates "BoomBoom" diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index cb1bb93..2382dbc 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -74,6 +74,27 @@ public class Claws extends SubsystemBase { m_open = open; } + + + + public void runClaw(ClawType which, double input) { + + + if (which == Claws.ClawType.LEFT) { + m_leftClaw.set(input); + + } else if (which == Claws.ClawType.RIGHT) { + m_rightClaw.set(input); + } + } + + + + public void runClaws(double input) + { + m_leftClaw.set(input); + m_rightClaw.set(input); + } /** * Sets the state of both hooks * @param open The state of the hooks diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 23478db..9ad1f97 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -17,6 +17,7 @@ import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; @@ -49,6 +50,16 @@ public class Climber extends SubsystemBase { m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); m_elbowOffset = 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; @@ -257,5 +268,11 @@ public void setMotors(double shoulderOutput, double elbowOutput) { m_robotAngle = robotAngle; m_robotAngle = 45; //45 is placeholder } + + @Override + public void periodic(){ + SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); + SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); + } } From 9a0e3f66d2b5be48624a0472f3973c943264c5a8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 10 Mar 2022 17:44:27 -0700 Subject: [PATCH 37/55] changed/fixed some things up --- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/Robot.java | 10 +- .../java/frc4388/robot/RobotContainer.java | 502 +++++++++--------- 3 files changed, 259 insertions(+), 257 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 64f285c..bef637c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -149,8 +149,8 @@ public final class Constants { public static final class ClimberConstants { /* TODO: Update motor IDS */ public static final int SHOULDER_ID = 1; - public static final int ELBOW_ID = 3; - public static final int GYRO_ID = 14; + public static final int ELBOW_ID = 30; + public static final int GYRO_ID = 31; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1e1ae82..b83b7c8 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -131,9 +131,9 @@ public class Robot extends TimedRobot { // print odometry data to smart dashboard for debugging (if causing timeout // errors, you can comment it) - SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); - SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); - SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); + // SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); + // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); + // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); } /** @@ -150,7 +150,7 @@ public class Robot extends TimedRobot { File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner") .resolve("recording." + System.currentTimeMillis() + ".path").toFile(); if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { - m_robotContainer.createPath(null, null, false).write(outputFile); + // m_robotContainer.createPath(null, null, false).write(outputFile); LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath()); } else LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); @@ -187,7 +187,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { LOGGER.fine("teleopInit()"); - m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); + // m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a48646f..6290768 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -96,17 +96,17 @@ public class RobotContainer { private 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 SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, + // m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); - private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); - private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); - //private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); - private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); + // private final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); + // private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); + // private final Storage m_robotStorage = new Storage(m_robotMap.storageMotor, m_robotMap.beamIntake, m_robotMap.beamShooter); + // //private final LED m_robotLED = new LED(m_robotMap.LEDController); + // private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + // private final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + // private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); + // private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); /* Controllers */ @@ -115,7 +115,7 @@ public class RobotContainer { /* Autonomous */ private PathPlannerTrajectory loadedPathTrajectory = null; - private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); + // private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); private final List pathPoints = new ArrayList<>(); private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording"); @@ -148,39 +148,39 @@ public class RobotContainer { // Turret default command //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); - m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)); + // m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)); - //Swerve Drive - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - getDriverController().getLeftX(), - getDriverController().getLeftY(), - getDriverController().getRightX(), - getDriverController().getRightY(), - true), - m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - //Intake with Triggers - m_robotIntake.setDefaultCommand( - new RunCommand(() -> m_robotIntake.runWithTriggers( - getOperatorController().getLeftTriggerAxis(), - getOperatorController().getRightTriggerAxis()), - m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - //Storage Management - m_robotStorage.setDefaultCommand( - new RunCommand(() -> m_robotStorage.manageStorage(), - m_robotStorage).withName("Storage manageStorage defaultCommand")); - //Serializer Management - m_robotSerializer.setDefaultCommand( - new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), - m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); + // //Swerve Drive + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + // getDriverController().getLeftX(), + // getDriverController().getLeftY(), + // getDriverController().getRightX(), + // getDriverController().getRightY(), + // true), + // m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + // //Intake with Triggers + // m_robotIntake.setDefaultCommand( + // new RunCommand(() -> m_robotIntake.runWithTriggers( + // getOperatorController().getLeftTriggerAxis(), + // getOperatorController().getRightTriggerAxis()), + // m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + // //Storage Management + // m_robotStorage.setDefaultCommand( + // new RunCommand(() -> m_robotStorage.manageStorage(), + // m_robotStorage).withName("Storage manageStorage defaultCommand")); + // //Serializer Management + // m_robotSerializer.setDefaultCommand( + // new RunCommand(() -> m_robotSerializer.setSerializerStateWithBeam(), + // m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED // .setDefaultCommand(new RunCommand(m_robotLED::updateLED, // m_robotLED).withName("LED update defaultCommand")); - autoInit(); - recordInit(); + // autoInit(); + // recordInit(); } /** @@ -193,27 +193,27 @@ public class RobotContainer { /* Driver Buttons */ // "XboxController.Button.kBack" was undefined yet, 7 works just fine - new JoystickButton(getDriverController(), XboxController.Button.kBack.value) - .whenPressed(m_robotSwerveDrive::resetGyro); + // new JoystickButton(getDriverController(), XboxController.Button.kBack.value) + // .whenPressed(m_robotSwerveDrive::resetGyro); - new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) - // new XboxControllerRawButton(m_driverXbox, - // XboxControllerRaw.LEFT_BUMPER_BUTTON) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) + // // new XboxControllerRawButton(m_driverXbox, + // // XboxControllerRaw.LEFT_BUMPER_BUTTON) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) - // new XboxControllerRawButton(m_driverXbox, - // XboxControllerRaw.RIGHT_BUMPER_BUTTON) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + // new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + // // new XboxControllerRawButton(m_driverXbox, + // // XboxControllerRaw.RIGHT_BUMPER_BUTTON) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); + // new JoystickButton(getDriverController(), XboxController.Button.kA.value) + // .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp - .whenPressed(() -> m_robotMap.leftFront.reset()) - .whenPressed(() -> m_robotMap.rightFront.reset()) - .whenPressed(() -> m_robotMap.leftBack.reset()) - .whenPressed(() -> m_robotMap.rightBack.reset()); + // new JoystickButton(getDriverController(), XboxController.Button.kX.value) //Temp + // .whenPressed(() -> m_robotMap.leftFront.reset()) + // .whenPressed(() -> m_robotMap.rightFront.reset()) + // .whenPressed(() -> m_robotMap.leftBack.reset()) + // .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ // run claws @@ -232,6 +232,7 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2))) .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); + } /* * // activates "BoomBoom" @@ -241,31 +242,31 @@ public class RobotContainer { */ //Extender - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(() -> m_robotIntake.runExtender(true)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(() -> m_robotIntake.runExtender(true)); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(() -> m_robotIntake.runExtender(false)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(() -> m_robotIntake.runExtender(false)); - //Storage - new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)) - .whenReleased(() -> m_robotStorage.runStorage(0.0)); + // //Storage + // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + // .whenPressed(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)) + // .whenReleased(() -> m_robotStorage.runStorage(0.0)); - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) - .whenReleased(() -> m_robotStorage.runStorage(0.0)); + // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + // .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)) + // .whenReleased(() -> m_robotStorage.runStorage(0.0)); - //Shooter - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); + // //Shooter + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); - } + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); + // } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -273,26 +274,27 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - if (loadedPathTrajectory != null) { - PIDController xController = SwerveDriveConstants.X_CONTROLLER; - PIDController yController = SwerveDriveConstants.Y_CONTROLLER; - ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; - thetaController.enableContinuousInput(-Math.PI, Math.PI); + // if (loadedPathTrajectory != null) { + // PIDController xController = SwerveDriveConstants.X_CONTROLLER; + // PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + // ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + // thetaController.enableContinuousInput(-Math.PI, Math.PI); - PathPlannerState initialState = loadedPathTrajectory.getInitialState(); - Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); - return new SequentialCommandGroup( - new InstantCommand(m_robotSwerveDrive.m_gyro::reset), - new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), - new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, - m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, - m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), - new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); - } else { - LOGGER.severe("No auto selected."); - return new RunCommand(() -> { - }).withName("No Autonomous Path"); - } + // PathPlannerState initialState = loadedPathTrajectory.getInitialState(); + // Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); + // return new SequentialCommandGroup( + // new InstantCommand(m_robotSwerveDrive.m_gyro::reset), + // new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)), + // new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, + // m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive), + // new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path"); + // } else { + // LOGGER.severe("No auto selected."); + // return new RunCommand(() -> { + // }).withName("No Autonomous Path"); + // } + return null; } public XboxController getDriverController() { @@ -304,18 +306,18 @@ public class RobotContainer { * * @return Odometry */ - public Pose2d getOdometry() { - return m_robotSwerveDrive.getOdometry(); - } + // public Pose2d getOdometry() { + // return m_robotSwerveDrive.getOdometry(); + // } /** * Set odometry to given pose. * * @param pose Pose to set odometry to. */ - public void resetOdometry(Pose2d pose) { - m_robotSwerveDrive.resetOdometry(pose); - } + // public void resetOdometry(Pose2d pose) { + // m_robotSwerveDrive.resetOdometry(pose); + // } public XboxController getOperatorController() { return m_operatorXbox; @@ -328,169 +330,169 @@ public class RobotContainer { * latest path files. * Finally, adds the existing path files to the auto chooser */ - private void autoInit() { - try { - WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), - StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, - StandardWatchEventKinds.ENTRY_DELETE); - // TODO: Store this and other commands as fields so they can be rescheduled. - new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }.withName("Path Watcher").schedule(); - } catch (IOException exception) { - LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); - } - Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) - .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) - .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); - SmartDashboard.putData("Auto Chooser", autoChooser); - } + // private void autoInit() { + // try { + // WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), + // StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, + // StandardWatchEventKinds.ENTRY_DELETE); + // // TODO: Store this and other commands as fields so they can be rescheduled. + // new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { + // @Override + // public boolean runsWhenDisabled() { + // return true; + // } + // }.withName("Path Watcher").schedule(); + // } catch (IOException exception) { + // LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); + // } + // Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) + // .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) + // .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); + // SmartDashboard.putData("Auto Chooser", autoChooser); + // } /** * Creates a button on the SmartDashboard that will record the path of the * robot. */ - public void recordInit() { - SmartDashboard.putData("Recording", - new RunCommand(this::recordPeriodic) { - @Override - public void end(boolean interupted) { - new InstantCommand(RobotContainer.this::saveRecording) { - @Override - public boolean runsWhenDisabled() { - return true; - } - }.withName("Save Recording").schedule(); - } - }.withName("Record Path (Cancel to Save)")); - } + // public void recordInit() { + // SmartDashboard.putData("Recording", + // new RunCommand(this::recordPeriodic) { + // @Override + // public void end(boolean interupted) { + // new InstantCommand(RobotContainer.this::saveRecording) { + // @Override + // public boolean runsWhenDisabled() { + // return true; + // } + // }.withName("Save Recording").schedule(); + // } + // }.withName("Record Path (Cancel to Save)")); + // } - /** - * Called when a file is created, modified, or deleted. - * Adds newly created .path files to the SendableChooser. - * Reloads the path if the currently selected file is modified. - * - * @param watchKey The WatchKey that is being observed. - */ - private void updateAutoChooser(WatchKey watchKey) { - List> watchEvents = watchKey.pollEvents(); - if (!watchEvents.isEmpty()) { - List> pathWatchEvents = watchEvents.stream() - .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList()); - for (WatchEvent pathWatchEvent : pathWatchEvents) { - Path watchEventPath = (Path) pathWatchEvent.context(); - File watchEventFile = watchEventPath.toFile(); - String watchEventFileName = watchEventFile.getName(); - if (watchEventFileName.endsWith(".path")) { - if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) { - LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", - watchEventFileName); - autoChooser.addOption(watchEventFile.getName(), watchEventFile); - } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) { - LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName); - if (watchEventFileName.equals(autoChooser.getSelected().getName())) { - LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName); - loadPath(watchEventFileName); - } - } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) { - LOGGER.log(Level.SEVERE, - "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", - watchEventFileName); - } - } - } - } - if (!watchKey.reset()) - LOGGER.severe("File watch key invalid."); - } + // /** + // * Called when a file is created, modified, or deleted. + // * Adds newly created .path files to the SendableChooser. + // * Reloads the path if the currently selected file is modified. + // * + // * @param watchKey The WatchKey that is being observed. + // */ + // private void updateAutoChooser(WatchKey watchKey) { + // List> watchEvents = watchKey.pollEvents(); + // if (!watchEvents.isEmpty()) { + // List> pathWatchEvents = watchEvents.stream() + // .filter(e -> e.kind().type().isAssignableFrom(Path.class)).collect(Collectors.toList()); + // for (WatchEvent pathWatchEvent : pathWatchEvents) { + // Path watchEventPath = (Path) pathWatchEvent.context(); + // File watchEventFile = watchEventPath.toFile(); + // String watchEventFileName = watchEventFile.getName(); + // if (watchEventFileName.endsWith(".path")) { + // if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) { + // LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", + // watchEventFileName); + // autoChooser.addOption(watchEventFile.getName(), watchEventFile); + // } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) { + // LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName); + // if (watchEventFileName.equals(autoChooser.getSelected().getName())) { + // LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName); + // loadPath(watchEventFileName); + // } + // } else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) { + // LOGGER.log(Level.SEVERE, + // "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", + // watchEventFileName); + // } + // } + // } + // } + // if (!watchKey.reset()) + // LOGGER.severe("File watch key invalid."); + // } - private void loadPath(String pathName) { - LOGGER.warning("Loading path " + pathName); - loadedPathTrajectory = null; - loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), - SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION); - LOGGER.info("Done loading"); - } + // private void loadPath(String pathName) { + // LOGGER.warning("Loading path " + pathName); + // loadedPathTrajectory = null; + // loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(Objects.requireNonNullElse(pathName, "")), + // SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION); + // LOGGER.info("Done loading"); + // } - private void saveRecording() { - // IMPORTANT: Had to chown the pathplanner folder in order to save autos. - File outputFile = PATHPLANNER_DIRECTORY - .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); - LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); - if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { - // TODO: Change to use measured maximum velocity and acceleration. - var path = createPath(null, null, false); - if (RobotBase.isReal()) - path.write(outputFile); - StringWriter writer = new StringWriter(); - path.write(writer); - recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString()); - autoChooser.setDefaultOption(outputFile.getName(), outputFile); - LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath()); - } else - LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); - } + // private void saveRecording() { + // // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + // File outputFile = PATHPLANNER_DIRECTORY + // .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); + // LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); + // if (!pathPoints.isEmpty() && Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) { + // // TODO: Change to use measured maximum velocity and acceleration. + // var path = createPath(null, null, false); + // if (RobotBase.isReal()) + // path.write(outputFile); + // StringWriter writer = new StringWriter(); + // path.write(writer); + // recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString()); + // autoChooser.setDefaultOption(outputFile.getName(), outputFile); + // LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath()); + // } else + // LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); + // } - public void recordPeriodic() { - Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); - Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); - // FIXME: Chassis speeds are created from joystick inputs and do not reflect - // actual robot velocity. - Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, - m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond); - Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, - SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false); - pathPoints.add(waypoint); - } + // // public void recordPeriodic() { + // // Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); + // // Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); + // // // FIXME: Chassis speeds are created from joystick inputs and do not reflect + // // // actual robot velocity. + // // Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond, + // // m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond); + // // Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, + // // SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false); + // // pathPoints.add(waypoint); + // // } - public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { - // Remove points whose angles to neighboring points are less than 10 degrees - // apart. - int j = 0; - for (int i = 1; i < pathPoints.size() - 1; i++) { - var prev = pathPoints.get(j).anchorPoint.orElseThrow(); - var current = pathPoints.get(i).anchorPoint.orElseThrow(); - var next = pathPoints.get(i + 1).anchorPoint.orElseThrow(); - var fromPrevious = current.minus(prev); - var toNext = next.minus(current); - var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY()); - var angleToNext = new Rotation2d(toNext.getX(), toNext.getY()); - if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE - || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE - && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false))) - pathPoints.set(i, null); - else - j = i; - } - pathPoints.removeIf(Objects::isNull); - // Make control points - pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), - pathPoints.get(1).anchorPoint.orElseThrow()).getSecond()); - for (int i = 1; i < pathPoints.size() - 1; i++) { - var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), - pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow()); - pathPoints.get(i).prevControl = Optional.of(controls.getFirst()); - pathPoints.get(i).nextControl = Optional.of(controls.getSecond()); - } - pathPoints.get(pathPoints.size() - 1).prevControl = Optional - .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), - pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst()); - // Create the path - PathPlannerUtil.Path path = new PathPlannerUtil.Path(); - path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new)); - path.maxVelocity = Optional.ofNullable(maxVelocity); - path.maxAcceleration = Optional.ofNullable(maxAcceleration); - path.isReversed = Optional.ofNullable(isReversed); - pathPoints.clear(); - return path; - } + // public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) { + // // Remove points whose angles to neighboring points are less than 10 degrees + // // apart. + // int j = 0; + // for (int i = 1; i < pathPoints.size() - 1; i++) { + // var prev = pathPoints.get(j).anchorPoint.orElseThrow(); + // var current = pathPoints.get(i).anchorPoint.orElseThrow(); + // var next = pathPoints.get(i + 1).anchorPoint.orElseThrow(); + // var fromPrevious = current.minus(prev); + // var toNext = next.minus(current); + // var angleFromPrevious = new Rotation2d(fromPrevious.getX(), fromPrevious.getY()); + // var angleToNext = new Rotation2d(toNext.getX(), toNext.getY()); + // if (Math.abs(angleFromPrevious.minus(angleToNext).getDegrees()) < SwerveDriveConstants.MIN_WAYPOINT_ANGLE + // || (next.getDistance(prev) < SwerveDriveConstants.MIN_WAYPOINT_DISTANCE + // && pathPoints.get(i).velOverride.map(v -> v < SwerveDriveConstants.MIN_WAYPOINT_VELOCITY).orElse(false))) + // pathPoints.set(i, null); + // else + // j = i; + // } + // pathPoints.removeIf(Objects::isNull); + // // Make control points + // pathPoints.get(0).nextControl = Optional.of(makeControlPoints(null, pathPoints.get(0).anchorPoint.orElseThrow(), + // pathPoints.get(1).anchorPoint.orElseThrow()).getSecond()); + // for (int i = 1; i < pathPoints.size() - 1; i++) { + // var controls = makeControlPoints(pathPoints.get(i - 1).anchorPoint.orElseThrow(), + // pathPoints.get(i).anchorPoint.orElseThrow(), pathPoints.get(i + 1).anchorPoint.orElseThrow()); + // pathPoints.get(i).prevControl = Optional.of(controls.getFirst()); + // pathPoints.get(i).nextControl = Optional.of(controls.getSecond()); + // } + // pathPoints.get(pathPoints.size() - 1).prevControl = Optional + // .of(makeControlPoints(pathPoints.get(pathPoints.size() - 2).anchorPoint.orElseThrow(), + // pathPoints.get(pathPoints.size() - 1).anchorPoint.orElseThrow(), null).getFirst()); + // // Create the path + // PathPlannerUtil.Path path = new PathPlannerUtil.Path(); + // path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new)); + // path.maxVelocity = Optional.ofNullable(maxVelocity); + // path.maxAcceleration = Optional.ofNullable(maxAcceleration); + // path.isReversed = Optional.ofNullable(isReversed); + // pathPoints.clear(); + // return path; + // } - private static Pair makeControlPoints(Translation2d prev, Translation2d current, - Translation2d next) { - var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4); - return Pair.of(current.minus(line), current.plus(line)); - } + // private static Pair makeControlPoints(Translation2d prev, Translation2d current, + // Translation2d next) { + // var line = Objects.requireNonNullElse(next, current).minus(Objects.requireNonNullElse(prev, current)).div(4); + // return Pair.of(current.minus(line), current.plus(line)); + // } } 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 38/55] 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 + } +} From 8aedbd33dcbf26fbc6b0f7f51db3a5a928bb5d17 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 14:33:40 -0600 Subject: [PATCH 39/55] Create Vector2D.java --- src/main/java/frc4388/utility/Vector2D.java | 101 ++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 src/main/java/frc4388/utility/Vector2D.java diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java new file mode 100644 index 0000000..1b76bb2 --- /dev/null +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -0,0 +1,101 @@ +// 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.utility; + +import edu.wpi.first.wpilibj.drive.Vector2d; + +/** Aarav's good vector class (better than WPILib) */ +public class Vector2D extends Vector2d { + + public double x; + public double y; + public double angle; + + public Vector2D() { + super(); + this.angle = Math.atan2(this.y, this.x); + } + + public Vector2D(double x, double y) { + super(x, y); + + this.x = x; + this.y = y; + this.angle = Math.atan2(this.y, this.x); + } + + /** + * Add two vectors, component-wise. + * @param v1 First vector in the addition. + * @param v2 Second vector in the addition. + * @return New vector which is the sum. + */ + public static Vector2D add(Vector2D v1, Vector2D v2) { + return new Vector2D(v1.x + v2.x, v1.y + v2.y); + } + + /** + * Subtract two vectors, component-wise. + * @param v1 First vector in the subtraction. + * @param v2 Second vector in the subtraction. + * @return New vector which is the difference. + */ + public static Vector2D subtract(Vector2D v1, Vector2D v2) { + return new Vector2D(v1.x - v2.x, v1.y - v2.y); + } + + /** + * Multiply a vector with a scalar, component-wise. + * @param v1 Vector to multiply. + * @param v2 Scalar to multiply. + * @return New vector which is the product. + */ + public static Vector2D multiply(Vector2D v1, double scalar) { + return new Vector2D(scalar * v1.x, scalar * v1.y); + } + + /** + * Divide a vector with a scalar, component-wise. + * @param v1 Vector to divide. + * @param v2 Scalar to divide. + * @return New vector which is the division. + */ + public static Vector2D divide(Vector2D v1, double scalar) { + return new Vector2D(v1.x / scalar, v1.y / scalar); + } + + /** + * Find unit vector. + * @return The unit vector. + */ + public Vector2D unit() { + return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude()); + } + + /** + * Round a vector to a certain number of places, component-wise. + * @param v Vector to round. + * @param places Number of places to round to. + * @return New rounded vector. + */ + public static Vector2D round(Vector2D v, int places) { + int scale = (int) Math.pow(10, places); + + v = Vector2D.multiply(v, scale); + + v.x = Math.round(v.x); + v.y = Math.round(v.y); + v.x = v.x / scale; + v.y = v.y / scale; + + return v; + } + + @Override + public String toString() { + return ("(" + this.x + ", " + this.y + ")"); + } + +} From b2857841268a8c6e9a25fe4e12fd30ca7738274d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 14:35:55 -0600 Subject: [PATCH 40/55] shtuff --- src/main/java/frc4388/utility/Vector2D.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 1b76bb2..824b1e3 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -6,7 +6,11 @@ package frc4388.utility; import edu.wpi.first.wpilibj.drive.Vector2d; -/** Aarav's good vector class (better than WPILib) */ +/** + * + * Aarav's good vector class (better than WPILib) + * @author Aarav Shah */ + public class Vector2D extends Vector2d { public double x; From c28a8d0599e294dcad413b2e7d18969bec528dd1 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Wed, 16 Mar 2022 14:41:27 -0600 Subject: [PATCH 41/55] Path stuffs --- src/main/java/frc4388/robot/Constants.java | 5 +- .../commands/climber/RunClimberPath.java | 60 ++++++ .../java/frc4388/robot/subsystems/Claws.java | 14 +- .../robot/subsystems/ClimberRewrite.java | 175 +++++++++++++++++- 4 files changed, 241 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/climber/RunClimberPath.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bef637c..36fafd3 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -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; diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java new file mode 100644 index 0000000..eeb3f46 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java @@ -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; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 2382dbc..f4ef26f 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -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; } /** diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index be1e764..b97a1b0 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -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 shoulderFeedMap; - private static HashMap 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 + *

+ * 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; } } From 633a443194c4ef59b2232e03cd53bc288e1ef3e6 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 14:49:41 -0600 Subject: [PATCH 42/55] sum changes --- src/main/java/frc4388/robot/Constants.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 4 +- src/main/java/frc4388/robot/RobotMap.java | 9 +- .../java/frc4388/robot/commands/RunClaw.java | 3 +- .../java/frc4388/robot/subsystems/Claws.java | 115 ++--- .../frc4388/robot/subsystems/Climber.java | 442 +++++++++--------- 6 files changed, 292 insertions(+), 287 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 36fafd3..bc8776b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -197,10 +197,10 @@ public final class Constants { public static final int LEFT_CLAW_ID = 44; public static final int RIGHT_CLAW_ID = 45; - public static final double OPEN_POSITION = 0; - public static final double CLOSE_POSITION = 0; + public static final double OPEN_POSITION = 0; // TODO: find actual position + 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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6290768..2d5a2d4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -57,7 +57,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; import frc4388.robot.commands.RunClaw; -import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.ClimberRewrite; import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; @@ -92,7 +92,7 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* 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); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4044cc9..63f0ac1 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,6 +8,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix.sensors.WPI_PigeonIMU; 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 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 leftBack; @@ -171,8 +172,10 @@ public class RobotMap { public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO /* Hooks Subsystem */ - 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 leftClaw = new CANSparkMax(ClawConstants.LEFT_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 /* Boom Boom Subsystem */ diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java index 2cf76af..f3c70ec 100644 --- a/src/main/java/frc4388/robot/commands/RunClaw.java +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -47,6 +47,7 @@ public class RunClaw extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return m_claws.checkSwitchAndCurrent(clawType); + // return m_claws.checkSwitchAndCurrent(clawType); + return false; // TODO: real return } } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index f4ef26f..c4f3f3f 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -7,18 +7,23 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxLimitSwitch; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Servo; 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; - public CANSparkMax m_rightClaw; - private SparkMaxLimitSwitch m_leftLimitSwitchF; - private SparkMaxLimitSwitch m_rightLimitSwitchF; - private SparkMaxLimitSwitch m_leftLimitSwitchR; - private SparkMaxLimitSwitch m_rightLimitSwitchR; + public Servo m_leftClaw; + public Servo m_rightClaw; + + // 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_rightOffset; @@ -26,21 +31,22 @@ public class Claws extends SubsystemBase { private boolean m_open; public static enum ClawType {LEFT, RIGHT} - public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) { + public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) { m_leftClaw = leftClaw; m_rightClaw = rightClaw; - m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol - m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol + // m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_leftLimitSwitchF.enableLimitSwitch(true); - m_rightLimitSwitchF.enableLimitSwitch(true); - m_leftLimitSwitchR.enableLimitSwitch(true); - m_rightLimitSwitchR.enableLimitSwitch(true); - leftClaw.setInverted(true); - rightClaw.setInverted(true); + // m_leftLimitSwitchF.enableLimitSwitch(true); + // m_rightLimitSwitchF.enableLimitSwitch(true); + // m_leftLimitSwitchR.enableLimitSwitch(true); + // m_rightLimitSwitchR.enableLimitSwitch(true); + + // m_leftClaw.setInverted(true); + // m_rightClaw.setInverted(true); m_open = false; @@ -48,11 +54,6 @@ public class Claws extends SubsystemBase { // 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. * @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; // m_leftClaw.getEncoder().setPosition(setPos); - m_leftClaw.set(direction * 0.1); + m_leftClaw.setSpeed(direction * 0.1); } else if (which == Claws.ClawType.RIGHT) { // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; // m_rightClaw.getEncoder().setPosition(setPos); - m_rightClaw.set(direction * 0.1); + m_rightClaw.setSpeed(direction * 0.1); } m_open = open; } - - public void runClaw(ClawType which, double input) { - - if (which == Claws.ClawType.LEFT) { - m_leftClaw.set(input); + m_leftClaw.setSpeed(input); } else if (which == Claws.ClawType.RIGHT) { - m_rightClaw.set(input); + m_rightClaw.setSpeed(input); } } - - public void runClaws(double input) { - m_leftClaw.set(input); - m_rightClaw.set(input); + m_leftClaw.setSpeed(input); + m_rightClaw.setSpeed(input); } + /** * Sets the state of both hooks * @param open The state of the hooks @@ -114,8 +110,6 @@ public class Claws extends SubsystemBase { m_leftClaw.set(-0.1); m_rightClaw.set(-0.1); } - - // m_open = open; } public double[] getOffsets() { @@ -123,12 +117,12 @@ public class Claws extends SubsystemBase { } 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; } + return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && + Math.abs(m_rightClaw.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; + return Math.abs(m_leftClaw.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. * @return Whether to interrupt the RunClaw command or not. */ - public boolean checkSwitchAndCurrent(ClawType which) { - if (which == ClawType.LEFT) { - if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { - return true; - } - } - else if (which == ClawType.RIGHT) { - if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { - return true; - } - } - return false; - } + // public boolean checkSwitchAndCurrent(ClawType which) { + // if (which == ClawType.LEFT) { + // if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { + // return true; + // } + // } + // else if (which == ClawType.RIGHT) { + // if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { + // return true; + // } + // } + // return false; + // } @Override public void periodic() { - if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) - m_leftOffset = m_leftClaw.getEncoder().getPosition(); + if (fullyOpen() || fullyClosed()) { + 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()) - m_rightOffset = m_rightClaw.getEncoder().getPosition(); + // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) + // // m_rightOffset = m_rightClaw.getEncoder().getPosition(); + // m_rightOffset = m_rightClaw.getPosition(); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 42b5886..cb8a9f1 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -1,279 +1,279 @@ -// 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. +// // 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. -/* -Safety -Hooks -Add -*/ +// /* +// Safety +// Hooks +// Add +// */ -package frc4388.robot.subsystems; +// package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; +// import com.ctre.phoenix.motorcontrol.NeutralMode; +// 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 org.opencv.core.Point; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; -import frc4388.robot.Constants.ClimberConstants; +// import edu.wpi.first.math.Matrix; +// import edu.wpi.first.math.geometry.Rotation2d; +// 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 { - public WPI_TalonFX m_shoulder; - public WPI_TalonFX m_elbow; +// public class Climber extends SubsystemBase { +// public WPI_TalonFX m_shoulder; +// public WPI_TalonFX m_elbow; - private double m_shoulderOffset; - private double m_elbowOffset; +// private double m_shoulderOffset; +// private double m_elbowOffset; - private WPI_PigeonIMU m_gyro; - private boolean m_groundRelative; - private double m_robotAngle; - private double m_robotPosition; +// private WPI_PigeonIMU m_gyro; +// private boolean m_groundRelative; +// private double m_robotAngle; +// 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) { - m_shoulder = shoulder; - m_shoulder.configFactoryDefault(); - m_shoulder.setNeutralMode(NeutralMode.Brake); +// public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU 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); +// m_elbow = elbow; +// m_elbow.configFactoryDefault(); +// m_elbow.setNeutralMode(NeutralMode.Brake); - setClimberGains(); +// setClimberGains(); - m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); - m_elbowOffset = m_elbow.getSelectedSensorPosition(); +// m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); +// m_elbowOffset = 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_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); +// 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; +// if(groundRelative) +// m_gyro = gyro; - m_groundRelative = groundRelative; - } +// m_groundRelative = groundRelative; +// } - /* getJointAngles - * Gets joint angles for climber arm - * 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(Point targetPoint, double tiltAngle) { - double [] angles = new double[2]; +// /* getJointAngles +// * Gets joint angles for climber arm +// * 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(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; - } +// 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; +// // 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; +// 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; +// // 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; +// // 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); +// 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); +// // 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); - } +// 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); +// 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); - } +// // Deal with negative wraparound +// // if(xDiff >= 0.d) +// // elbowAngle += Math.PI; +// // System.out.println("ea4: " + elbowAngle); +// } - angles[0] = shoulderAngle; - angles[1] = elbowAngle; - return angles; - } +// angles[0] = shoulderAngle; +// angles[1] = elbowAngle; +// return angles; +// } -public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput); - m_elbow.set(elbowOutput); -} +// public void setMotors(double shoulderOutput, double elbowOutput) { +// m_shoulder.set(shoulderOutput); +// m_elbow.set(elbowOutput); +// } -/* Rotation Matrix - 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)] - Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix - Rotation Matrix video: https://youtu.be/Ta8cKqltPfU - */ - public double getRobotTilt() { - double[] ypr = new double[3]; - m_gyro.getYawPitchRoll(ypr); - double theta = 0; +// /* Rotation Matrix +// 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)] +// Rotation Matrix resource: https://en.wikipedia.org/wiki/Rotation_matrix +// Rotation Matrix video: https://youtu.be/Ta8cKqltPfU +// */ +// public double getRobotTilt() { +// double[] ypr = new double[3]; +// m_gyro.getYawPitchRoll(ypr); +// double theta = 0; - // double sin; - // double cos; - // xsin = 0; //placeholder for sin, cos - // ysin = 0; - // xcos = 0; - // ycos = 0; - double[][] rotMax = { - {Math.cos(theta) - Math.sin(theta), 0 }, - {Math.sin(theta) + Math.cos(theta), 0}, - {0, 0, 1} - }; +// // double sin; +// // double cos; +// // xsin = 0; //placeholder for sin, cos +// // ysin = 0; +// // xcos = 0; +// // ycos = 0; +// double[][] rotMax = { +// {Math.cos(theta) - Math.sin(theta), 0 }, +// {Math.sin(theta) + Math.cos(theta), 0}, +// {0, 0, 1} +// }; - if (m_robotPosition != m_robotAngle){ - setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); - } +// if (m_robotPosition != m_robotAngle){ +// setRobotAngle(ClimberConstants.ROBOT_ANGLE_ID, rotMax, m_robotPosition); +// } - return Math.toRadians(ypr[1]); // Pitch - // multiply by pie and then divide by 180 +// return Math.toRadians(ypr[1]); // Pitch +// // multiply by pie and then divide by 180 - } +// } - public void setClimberGains() { - // shoulder PIDs - 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); +// public void setClimberGains() { +// // shoulder PIDs +// 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); - // elbow PIDs - 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); - } +// // elbow PIDs +// 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 setJointAngles(double[] angles) { - System.out.println(angles); - setJointAngles(angles[0], angles[1]); - } +// 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; +// 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; +// 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); +// // 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; +// double shoulderPosition = (shoulderAngle * (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; - elbowAngle *= ClimberConstants.GEAR_BOX_RATIO; +// shoulderAngle *= ClimberConstants.GEAR_BOX_RATIO; +// elbowAngle *= ClimberConstants.GEAR_BOX_RATIO; - shoulderPosition += m_shoulderOffset; - elbowPosition += m_elbowOffset; +// shoulderPosition += m_shoulderOffset; +// elbowPosition += m_elbowOffset; - m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); - m_elbow.set(TalonFXControlMode.Position, elbowPosition); - } +// m_shoulder.set(TalonFXControlMode.Position, shoulderPosition); +// m_elbow.set(TalonFXControlMode.Position, elbowPosition); +// } - public void controlWithInput(double xInput, double yInput) { - m_position.x += xInput * ClimberConstants.MOVE_SPEED; - m_position.y += yInput * ClimberConstants.MOVE_SPEED; +// public void controlWithInput(double xInput, double yInput) { +// m_position.x += xInput * 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); - // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); +// // double[] testAngles = getJointAngles(0, 0, 0); +// // System.out.println("origin: " + testAngles[0] + ", " + testAngles[1]); - // double[] testAngles2 = getJointAngles(5000, 5000, 0); - // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); +// // double[] testAngles2 = getJointAngles(5000, 5000, 0); +// // System.out.println("extended: " + testAngles2[0] + ", " + testAngles2[1]); - // double[] testAngles3 = getJointAngles(0, 75, 0); - // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); +// // double[] testAngles3 = getJointAngles(0, 75, 0); +// // System.out.println("just y: " + testAngles3[0] + ", " + testAngles3[1]); - // double[] testAngles4 = getJointAngles(75, 0, 0); - // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); +// // double[] testAngles4 = getJointAngles(75, 0, 0); +// // System.out.println("just x: " + testAngles4[0] + ", " + testAngles4[1]); - // double[] testAngles5 = getJointAngles(-75, 0, 0); - // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); +// // double[] testAngles5 = getJointAngles(-75, 0, 0); +// // System.out.println("just x: " + testAngles5[0] + ", " + testAngles5[1]); - // double[] testAngles6 = getJointAngles(60, 25, 0); - // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); +// // double[] testAngles6 = getJointAngles(60, 25, 0); +// // System.out.println("just xy: " + testAngles6[0] + ", " + testAngles6[1]); - double[] jointAngles = getJointAngles(m_position, tiltAngle); - setJointAngles(jointAngles); - } +// double[] jointAngles = getJointAngles(m_position, tiltAngle); +// setJointAngles(jointAngles); +// } - public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { - m_robotPosition = robotPosition; - m_robotAngle = robotAngle; - m_robotAngle = 45; //45 is placeholder - } +// public void setRobotAngle(double robotAngle, double[][] rotMax, double robotPosition) { +// m_robotPosition = robotPosition; +// m_robotAngle = robotAngle; +// m_robotAngle = 45; //45 is placeholder +// } - @Override - public void periodic(){ - SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); - SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); - } -} +// @Override +// public void periodic(){ +// SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); +// SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); +// } +// } From a71fda454924f6967892a27e618009c696bb7c58 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Wed, 16 Mar 2022 14:49:53 -0600 Subject: [PATCH 43/55] Added ClimberRewrite --- .../robot/commands/climber/RunClimberPath.java | 14 +++++++------- .../frc4388/robot/subsystems/ClimberRewrite.java | 6 ++++++ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java index eeb3f46..3fc832a 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java @@ -10,16 +10,18 @@ import edu.wpi.first.wpilibj.drive.Vector2d; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Claws; import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.ClimberRewrite; +import frc4388.utility.Vector2D; public class RunClimberPath extends CommandBase { - Climber climber; + ClimberRewrite climber; Claws claws; Point[] path; int nextIndex; /** Creates a new RunClimberPath. */ - public RunClimberPath(Point[] _path, Climber _climber, Claws _claws) { + public RunClimberPath(Point[] _path, ClimberRewrite _climber, Claws _claws) { path = _path; climber = _climber; @@ -40,12 +42,10 @@ public class RunClimberPath extends CommandBase { if(!claws.fullyOpen()) return; - Vector2d vec = new Vector2d(1,1); - double mag = vec.magnitude(); - vec.x /= mag; - vec.y /= mag; + Point climberPos = climber.getClimberPosition(); + Vector2D dir = new Vector2D(); - climber.controlWithInput(vec.x * .02, vec.y * .02); + climber.controlWithInput(dir.x * .02, dir.y * .02); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index b97a1b0..c1886c6 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -250,4 +250,10 @@ public class ClimberRewrite extends SubsystemBase { angles[1] = elbowAngle; return angles; } + + public static Point getClimberPoint(double shoulderAngle, double elbowAngle) { + return null; + } + + public static Point getClimberPoint } From 1c9d6e0befbab08b4ba4be308f7562d185515e2b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 15:05:33 -0600 Subject: [PATCH 44/55] sum vector2D shtuff --- src/main/java/frc4388/utility/Vector2D.java | 24 +++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 824b1e3..db6b361 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -4,6 +4,10 @@ package frc4388.utility; +import java.security.InvalidParameterException; + +import org.opencv.core.Point; + import edu.wpi.first.wpilibj.drive.Vector2d; /** @@ -30,6 +34,26 @@ public class Vector2D extends Vector2d { this.angle = Math.atan2(this.y, this.x); } + public Vector2D(double[] vec) { + super(vec[0], vec[1]); + + if (vec.length != 2) { + throw new InvalidParameterException(); + } + + this.x = vec[0]; + this.y = vec[1]; + this.angle = Math.atan2(this.y, this.x); + } + + public Vector2D(Point p) { + super(p.x, p.y); + + this.x = p.x; + this.y = p.y; + this.angle = Math.atan2(this.y, this.x); + } + /** * Add two vectors, component-wise. * @param v1 First vector in the addition. From dbe87cd7c1d9b073a120ba39f59b5a401adc29e0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 15:13:34 -0600 Subject: [PATCH 45/55] sum change --- .../java/frc4388/robot/RobotContainer.java | 1 + src/main/java/frc4388/utility/Vector2D.java | 18 +++++++----------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2d5a2d4..13fa067 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -76,6 +76,7 @@ import frc4388.robot.subsystems.VisionOdometry; import frc4388.utility.LEDPatterns; import frc4388.utility.ListeningSendableChooser; import frc4388.utility.PathPlannerUtil; +import frc4388.utility.Vector2D; import frc4388.utility.PathPlannerUtil.Path.Waypoint; import frc4388.utility.controller.DeadbandedXboxController; diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index db6b361..3d658fc 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -8,6 +8,7 @@ import java.security.InvalidParameterException; import org.opencv.core.Point; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.drive.Vector2d; /** @@ -22,8 +23,7 @@ public class Vector2D extends Vector2d { public double angle; public Vector2D() { - super(); - this.angle = Math.atan2(this.y, this.x); + this(0, 0); } public Vector2D(double x, double y) { @@ -35,23 +35,19 @@ public class Vector2D extends Vector2d { } public Vector2D(double[] vec) { - super(vec[0], vec[1]); + this(vec[0], vec[1]); if (vec.length != 2) { throw new InvalidParameterException(); } - - this.x = vec[0]; - this.y = vec[1]; - this.angle = Math.atan2(this.y, this.x); } public Vector2D(Point p) { - super(p.x, p.y); + this(p.x, p.y); + } - this.x = p.x; - this.y = p.y; - this.angle = Math.atan2(this.y, this.x); + public Vector2D(Translation2d t) { + this(t.getX(), t.getY()); } /** From 6c741d6cca9ccc7a02aef2e0d87057eb60954ce9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 15:19:29 -0600 Subject: [PATCH 46/55] fix --- src/main/java/frc4388/utility/Vector2D.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 3d658fc..266d49e 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -11,9 +11,7 @@ import org.opencv.core.Point; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.drive.Vector2d; -/** - * - * Aarav's good vector class (better than WPILib) +/** Aarav's good vector class (better than WPILib) * @author Aarav Shah */ public class Vector2D extends Vector2d { From 970fd819b5af104d67c8d7761d3ac88ace10c8d5 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Wed, 16 Mar 2022 15:46:39 -0600 Subject: [PATCH 47/55] Climber auto --- src/main/java/frc4388/robot/Constants.java | 2 ++ .../commands/climber/RunClimberPath.java | 23 +++++++++--- .../robot/subsystems/ClimberRewrite.java | 33 ++++++++++++----- src/main/java/frc4388/utility/Vector2D.java | 36 +++++++++++++++++++ 4 files changed, 81 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bc8776b..52c0845 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -187,6 +187,8 @@ public final class Constants { public static final int CLIMBER_TIMEOUT_MS = 50; + public static final double THRESHOLD = 0; + // TODO: Update Constants // Robot Angle public static final double ROBOT_ANGLE_ID = 0; diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java index 3fc832a..1866be3 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java @@ -8,8 +8,8 @@ import org.opencv.core.Point; 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.Climber; import frc4388.robot.subsystems.ClimberRewrite; import frc4388.utility.Vector2D; @@ -20,9 +20,12 @@ public class RunClimberPath extends CommandBase { Point[] path; int nextIndex; + boolean endPath; + /** Creates a new RunClimberPath. */ public RunClimberPath(Point[] _path, ClimberRewrite _climber, Claws _claws) { path = _path; + endPath = false; climber = _climber; claws = _claws; @@ -42,10 +45,20 @@ public class RunClimberPath extends CommandBase { if(!claws.fullyOpen()) return; - Point climberPos = climber.getClimberPosition(); - Vector2D dir = new Vector2D(); + Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); - climber.controlWithInput(dir.x * .02, dir.y * .02); + Vector2D dir = new Vector2D(path[nextIndex]); + dir.subtract(new Vector2D(climberPos)); + + if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) + nextIndex++; + else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { + endPath = true; + claws.setOpen(false); + } else if(!endPath) { + dir = dir.unit(); + climber.controlWithInput(dir.x, dir.y); + } } // Called once the command ends or is interrupted. @@ -55,6 +68,6 @@ public class RunClimberPath extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return endPath && claws.fullyClosed(); } } diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index c1886c6..dd75f58 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -123,6 +123,13 @@ public class ClimberRewrite extends SubsystemBase { m_elbow.set(elbowOutput); } + 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]); @@ -152,13 +159,13 @@ public class ClimberRewrite extends SubsystemBase { } public void controlWithInput(double xInput, double yInput) { - tPoint.x += xInput * ClimberConstants.MOVE_SPEED; - tPoint.y += yInput * ClimberConstants.MOVE_SPEED; + tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02; + tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; } @Override public void periodic() { - double[] jointAngles = getJointAngles(tPoint, 0.d); + double[] jointAngles = getTargetJointAngles(tPoint, 0.d); setJointAngles(jointAngles); } @@ -172,8 +179,8 @@ public class ClimberRewrite extends SubsystemBase { * * @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) { + * @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); @@ -251,9 +258,19 @@ public class ClimberRewrite extends SubsystemBase { return angles; } - public static Point getClimberPoint(double shoulderAngle, double elbowAngle) { - return null; + 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 getClimberPoint + public static Point getClimberPosition(double[] jointAngles) { + return getClimberPosition(jointAngles[0], jointAngles[1]); + } } diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 266d49e..3a3619c 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -58,6 +58,15 @@ public class Vector2D extends Vector2d { return new Vector2D(v1.x + v2.x, v1.y + v2.y); } + /** + * Adds vector to current object + * @param v Vector to add + */ + public void add(Vector2D v) { + x += v.x; + y += v.x; + } + /** * Subtract two vectors, component-wise. * @param v1 First vector in the subtraction. @@ -68,6 +77,15 @@ public class Vector2D extends Vector2d { return new Vector2D(v1.x - v2.x, v1.y - v2.y); } + /** + * Subtracts vector from current object + * @param v Vector to subtract + */ + public void subtract(Vector2D v) { + x -= v.x; + y -= v.x; + } + /** * Multiply a vector with a scalar, component-wise. * @param v1 Vector to multiply. @@ -78,6 +96,15 @@ public class Vector2D extends Vector2d { return new Vector2D(scalar * v1.x, scalar * v1.y); } + /** + * Multiply a vector with a scalar, component-wise. + * @param scalar Scalar to multiply + */ + public void multiply(double scalar) { + x *= scalar; + y *= scalar; + } + /** * Divide a vector with a scalar, component-wise. * @param v1 Vector to divide. @@ -88,6 +115,15 @@ public class Vector2D extends Vector2d { return new Vector2D(v1.x / scalar, v1.y / scalar); } + /** + * Divide a vector with a scalar, component-wise. + * @param scalar Scalar to divide + */ + public void divide(double scalar) { + x /= scalar; + y /= scalar; + } + /** * Find unit vector. * @return The unit vector. From a7378ef38a40b14870f0006dd2b0d2145bce8f6c Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Wed, 16 Mar 2022 20:16:32 -0600 Subject: [PATCH 48/55] updated claws --- src/main/java/frc4388/robot/Constants.java | 4 +-- .../java/frc4388/robot/RobotContainer.java | 30 +++++++++++-------- src/main/java/frc4388/robot/RobotMap.java | 2 +- .../java/frc4388/robot/subsystems/Claws.java | 8 ++--- 4 files changed, 25 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 52c0845..aea43a0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -200,9 +200,9 @@ public final class Constants { public static final int RIGHT_CLAW_ID = 45; public static final double OPEN_POSITION = 0; // TODO: find actual position - public static final double CLOSE_POSITION = 0; // TODO: find actual position + public static final double CLOSE_POSITION = 1; // TODO: find actual position - public static final double THRESHOLD = 0; // TODO: find actual threshold + public static final double THRESHOLD = .1; // TODO: find actual threshold public static final double CALIBRATION_SPEED = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 13fa067..b94ae50 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -218,21 +218,27 @@ public class RobotContainer { /* Operator Buttons */ // run claws - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2))) - .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2))) + // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2))) - .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2))) + // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2))) + // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); + + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2))) + // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(true))); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2))) - .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); - - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2))) - .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); + .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(false))); } /* diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 63f0ac1..31f5296 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -175,7 +175,7 @@ public class RobotMap { // 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 Servo leftClaw = new Servo(1); // TODO: find actual channel - public final Servo rightClaw = new Servo(1); // TODO: find actual channel + public final Servo rightClaw = new Servo(2); // TODO: find actual channel // Shooter Config /* Boom Boom Subsystem */ diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index c4f3f3f..3ea40a8 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -102,13 +102,13 @@ public class Claws extends SubsystemBase { if(open) { // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); - m_leftClaw.set(0.1); - m_rightClaw.set(0.1); + m_leftClaw.set(-ClawConstants.OPEN_POSITION); + m_rightClaw.set(ClawConstants.OPEN_POSITION); } else { // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); - m_leftClaw.set(-0.1); - m_rightClaw.set(-0.1); + m_leftClaw.set(-ClawConstants.CLOSE_POSITION); + m_rightClaw.set(ClawConstants.CLOSE_POSITION); } } From d5ccd5e46ebc61be25a07c1d0826c601daba61e8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 22:32:57 -0600 Subject: [PATCH 49/55] test code --- src/main/java/frc4388/robot/Constants.java | 4 + .../java/frc4388/robot/RobotContainer.java | 10 +- .../java/frc4388/robot/commands/RunClaw.java | 2 +- .../commands/climber/RunClimberPath.java | 29 +++--- .../java/frc4388/robot/subsystems/Claws.java | 98 ++++++++++--------- 5 files changed, 77 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index aea43a0..b9fbf40 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -207,6 +207,10 @@ public final class Constants { public static final double CALIBRATION_SPEED = 0; public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit; + + public static final int TOP_LIMIT = 1800; + public static final int BOTTOM_LIMIT = 1200; + } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b94ae50..35ae23d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,9 +137,9 @@ public class RobotContainer { /* Default Commands */ // moves climber in xy space with two-axis input from the operator controller - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), - m_robotClimber)); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), + // m_robotClimber)); // IK command @@ -235,10 +235,10 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(true))); + .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(false))); + .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); } /* diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java index f3c70ec..0c4e8b7 100644 --- a/src/main/java/frc4388/robot/commands/RunClaw.java +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -37,7 +37,7 @@ public class RunClaw extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_claws.runClaw(clawType, open); + // m_claws.runClaw(clawType, open); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java index 1866be3..38ad651 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java @@ -42,23 +42,23 @@ public class RunClimberPath extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(!claws.fullyOpen()) + // if(!claws.fullyOpen()) return; - Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); + // Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); - Vector2D dir = new Vector2D(path[nextIndex]); - dir.subtract(new Vector2D(climberPos)); + // Vector2D dir = new Vector2D(path[nextIndex]); + // dir.subtract(new Vector2D(climberPos)); - if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) - nextIndex++; - else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { - endPath = true; - claws.setOpen(false); - } else if(!endPath) { - dir = dir.unit(); - climber.controlWithInput(dir.x, dir.y); - } + // if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) + // nextIndex++; + // else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { + // endPath = true; + // claws.setOpen(false); + // } else if(!endPath) { + // dir = dir.unit(); + // climber.controlWithInput(dir.x, dir.y); + // } } // Called once the command ends or is interrupted. @@ -68,6 +68,7 @@ public class RunClimberPath extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return endPath && claws.fullyClosed(); + // return endPath && claws.fullyClosed(); + return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 3ea40a8..300fb34 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -8,6 +8,7 @@ import com.revrobotics.SparkMaxLimitSwitch; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.ClimberConstants; @@ -59,71 +60,76 @@ public class Claws extends SubsystemBase { * @param which Which claw to run. * @param open Whether to open or close the claw. */ - public void runClaw(ClawType which, boolean open) { + // public void runClaw(ClawType which, boolean open) { - int direction = open ? 1 : -1; + // int direction = open ? 1 : -1; - if (which == Claws.ClawType.LEFT) { + // if (which == Claws.ClawType.LEFT) { - // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; - // m_leftClaw.getEncoder().setPosition(setPos); - m_leftClaw.setSpeed(direction * 0.1); + // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; + // // m_leftClaw.getEncoder().setPosition(setPos); + // 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; - // m_rightClaw.getEncoder().setPosition(setPos); - m_rightClaw.setSpeed(direction * 0.1); - } + // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; + // // m_rightClaw.getEncoder().setPosition(setPos); + // m_rightClaw.setSpeed(direction * 0.1); + // } - m_open = open; - } + // m_open = open; + // } - public void runClaw(ClawType which, double input) { - if (which == Claws.ClawType.LEFT) { - m_leftClaw.setSpeed(input); + // public void runClaw(ClawType which, double input) { + // if (which == Claws.ClawType.LEFT) { + // m_leftClaw.setSpeed(input); - } else if (which == Claws.ClawType.RIGHT) { - m_rightClaw.setSpeed(input); - } - } + // } else if (which == Claws.ClawType.RIGHT) { + // m_rightClaw.setSpeed(input); + // } + // } - public void runClaws(double input) - { - m_leftClaw.setSpeed(input); - m_rightClaw.setSpeed(input); - } + // public void runClaws(double input) + // { + // m_leftClaw.setSpeed(input); + // m_rightClaw.setSpeed(input); + // } /** * Sets the state of both hooks * @param open The state of the hooks */ public void setOpen(boolean open) { + SmartDashboard.putBoolean("open", open); if(open) { // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); - m_leftClaw.set(-ClawConstants.OPEN_POSITION); - m_rightClaw.set(ClawConstants.OPEN_POSITION); + // m_leftClaw.setPosition(.7); + // m_rightClaw.setPosition(.7); + m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT);//ClawConstants.OPEN_POSITION); + m_rightClaw.setRaw(ClawConstants.TOP_LIMIT);//ClawConstants.OPEN_POSITION); } else { // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); - m_leftClaw.set(-ClawConstants.CLOSE_POSITION); - m_rightClaw.set(ClawConstants.CLOSE_POSITION); + // m_leftClaw.setPosition(.3); + // m_rightClaw.setPosition(.3); + m_leftClaw.setRaw(ClawConstants.TOP_LIMIT); + m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT); } } - public double[] getOffsets() { - return new double[] {m_leftOffset, m_rightOffset}; - } + // public double[] getOffsets() { + // return new double[] {m_leftOffset, m_rightOffset}; + // } - public boolean fullyOpen() { - return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && - Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } + // public boolean fullyOpen() { + // return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && + // Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } - public boolean fullyClosed() { - return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && - Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; - } + // public boolean fullyClosed() { + // return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && + // Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; + // } /** * Check if a limit switch is pressed or current limit exceeded for a claw. @@ -145,12 +151,12 @@ public class Claws extends SubsystemBase { // return false; // } - @Override - public void periodic() { - if (fullyOpen() || fullyClosed()) { - m_leftClaw.setSpeed(0.0); - m_rightClaw.setSpeed(0.0); - } + // @Override + // public void periodic() { + // if (fullyOpen() || fullyClosed()) { + // m_leftClaw.setSpeed(0.0); + // m_rightClaw.setSpeed(0.0); + // } // if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) // // m_leftOffset = m_leftClaw.getEncoder().getPosition(); @@ -159,5 +165,5 @@ public class Claws extends SubsystemBase { // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) // // m_rightOffset = m_rightClaw.getEncoder().getPosition(); // m_rightOffset = m_rightClaw.getPosition(); - } + // } } \ No newline at end of file From f1ea7cc84c1b9892ff412d24824c343593d7dc1a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 17:03:39 -0600 Subject: [PATCH 50/55] updated climber motor ids --- src/main/java/frc4388/robot/Constants.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b9fbf40..2999836 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -147,10 +147,9 @@ public final class Constants { } public static final class ClimberConstants { - /* TODO: Update motor IDS */ - public static final int SHOULDER_ID = 1; - public static final int ELBOW_ID = 30; - public static final int GYRO_ID = 31; + public static final int SHOULDER_ID = 30; + public static final int ELBOW_ID = 31; + public static final int GYRO_ID = 14; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm From 71c9cd2e40d523699c397ab3c66f1318f8dea605 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 10:45:08 -0600 Subject: [PATCH 51/55] claws working --- src/main/java/frc4388/robot/subsystems/Claws.java | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 300fb34..2d8f726 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -106,15 +106,15 @@ public class Claws extends SubsystemBase { // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); // m_leftClaw.setPosition(.7); // m_rightClaw.setPosition(.7); - m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT);//ClawConstants.OPEN_POSITION); - m_rightClaw.setRaw(ClawConstants.TOP_LIMIT);//ClawConstants.OPEN_POSITION); + m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);//ClawConstants.OPEN_POSITION); + m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);//ClawConstants.OPEN_POSITION); } else { // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); // m_leftClaw.setPosition(.3); // m_rightClaw.setPosition(.3); - m_leftClaw.setRaw(ClawConstants.TOP_LIMIT); - m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT); + m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 100); + m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 700); } } @@ -152,7 +152,9 @@ public class Claws extends SubsystemBase { // } // @Override - // public void periodic() { + public void periodic() { + SmartDashboard.putNumber("Servo Left Pos", m_leftClaw.getRaw()); + SmartDashboard.putNumber("Servo Right Pos", m_rightClaw.getRaw()); // if (fullyOpen() || fullyClosed()) { // m_leftClaw.setSpeed(0.0); // m_rightClaw.setSpeed(0.0); @@ -165,5 +167,5 @@ public class Claws extends SubsystemBase { // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) // // m_rightOffset = m_rightClaw.getEncoder().getPosition(); // m_rightOffset = m_rightClaw.getPosition(); - // } + } } \ No newline at end of file From f250c8dfceda6d60e7b43f962decb2e6db7b37fc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 12:05:29 -0600 Subject: [PATCH 52/55] clikber --- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- .../java/frc4388/robot/subsystems/ClimberRewrite.java | 11 ++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 35ae23d..7da5107 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,9 +137,9 @@ public class RobotContainer { /* Default Commands */ // moves climber in xy space with two-axis input from the operator controller - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), - // m_robotClimber)); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.2, getOperatorController().getLeftY() * 0.2), + m_robotClimber)); // IK command diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index dd75f58..cac715a 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -6,6 +6,8 @@ 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; @@ -45,7 +47,7 @@ public class ClimberRewrite extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - setClimberGains(); + // setClimberGains(); // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); // elbowStartPosition = m_elbow.getSelectedSensorPosition(); @@ -62,6 +64,13 @@ public class ClimberRewrite extends SubsystemBase { m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); 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) From d66f426dad403eb8a3515929f4986aee5bcbe8cc Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Fri, 18 Mar 2022 12:10:12 -0600 Subject: [PATCH 53/55] Hello darkness my old friend --- src/main/java/frc4388/robot/subsystems/ClimberRewrite.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index cac715a..6b4b8ea 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -175,7 +175,7 @@ public class ClimberRewrite extends SubsystemBase { @Override public void periodic() { double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - setJointAngles(jointAngles); + // setJointAngles(jointAngles); } /** From 1106ecd760f619ab537a9d1321837587179ff8e8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 12:21:12 -0600 Subject: [PATCH 54/55] IMPORTANT CHANG --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7da5107..1888284 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -93,7 +93,7 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final ClimberRewrite m_robotClimber= new ClimberRewrite(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); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 31f5296..761839b 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -239,7 +239,7 @@ public class RobotMap { // public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); - /* Intake Subsytem */ + /* Intake Subsystem */ public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); From 559f84b3b726a36b8bf0fd843c84c98d359272e8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 12:21:47 -0600 Subject: [PATCH 55/55] climber man --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- src/main/java/frc4388/robot/subsystems/ClimberRewrite.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7da5107..47399e4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -138,7 +138,7 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.2, getOperatorController().getLeftY() * 0.2), + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.7, getOperatorController().getRightY() * 0.7), m_robotClimber)); @@ -147,7 +147,7 @@ public class RobotContainer { // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); - // Turret default command + // Turret default command% //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); // m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)); diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index cac715a..ef6e973 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -174,8 +174,8 @@ public class ClimberRewrite extends SubsystemBase { @Override public void periodic() { - double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - setJointAngles(jointAngles); + // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); + // setJointAngles(jointAngles); } /**