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 001/180] 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 002/180] 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 003/180] 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 004/180] 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 005/180] 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 006/180] 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 007/180] 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 008/180] 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 009/180] 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 010/180] 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 011/180] 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 012/180] 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 013/180] 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 014/180] 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 015/180] 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 016/180] 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 017/180] 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 018/180] 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 019/180] 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 020/180] 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 021/180] 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 022/180] 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 023/180] 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 024/180] 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 025/180] 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 026/180] 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 027/180] 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 028/180] 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 029/180] 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 030/180] 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 031/180] 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 032/180] 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 033/180] 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 034/180] 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 035/180] 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 dce8ab320c8352009d8600989cbfdd412f5cf6b5 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 8 Mar 2022 16:14:34 -0700 Subject: [PATCH 036/180] Monday Fixes --- src/main/java/frc4388/robot/Constants.java | 44 +++++++------- .../java/frc4388/robot/RobotContainer.java | 57 ++++++++++++------- .../frc4388/robot/subsystems/BoomBoom.java | 12 +++- .../java/frc4388/robot/subsystems/Hood.java | 12 ++-- .../frc4388/robot/subsystems/SwerveDrive.java | 25 ++++---- .../robot/subsystems/SwerveModule.java | 1 + 6 files changed, 90 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e1faffa..ae01bf8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -31,7 +31,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4; + public static final double ROTATION_SPEED = 0.5; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -40,18 +40,18 @@ public final class Constants { public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant? // IDs - public static final int LEFT_FRONT_STEER_CAN_ID = 2; - public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; - public static final int RIGHT_FRONT_STEER_CAN_ID = 4; - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; - public static final int LEFT_BACK_STEER_CAN_ID = 6; - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; - public static final int RIGHT_BACK_STEER_CAN_ID = 8; - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; + public static final int LEFT_FRONT_STEER_CAN_ID = 2; // + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // + public static final int LEFT_BACK_STEER_CAN_ID = 6; // + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // + public static final int RIGHT_BACK_STEER_CAN_ID = 8; // + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;// + public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // public static final int GYRO_ID = 14; // offsets are in degrees @@ -65,10 +65,10 @@ public final class Constants { // public static final double RIGHT_BACK_ENCODER_OFFSET = 360. + 2.15 - 3.637;// // 180-2.021484375;//0.0;//134.384765625 - public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 - 90) % 360.; - public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 - 90 ) % 360.; - public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50 - 90) % 360.; - public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 90 - 180) % 360.; + public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.58 + 180 ) % 360.; + public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.05 - 180 ) % 360.; + public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.50) % 360.; + public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.31 - 180) % 360.; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; @@ -176,10 +176,10 @@ public final class Constants { // ID public static final int TURRET_MOTOR_CAN_ID = 19; public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double TURRET_FORWARD_LIMIT = 61.7; // TODO: find - public static final double TURRET_REVERSE_LIMIT = -42.3; // TODO: find + public static final double TURRET_FORWARD_LIMIT = 55.0; // TODO: find + public static final double TURRET_REVERSE_LIMIT = -55.0; // TODO: find public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values @@ -187,8 +187,8 @@ public final class Constants { public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final double HOOD_FORWARD_LIMIT = 48.69; // TODO: find - public static final double HOOD_REVERSE_LIMIT = -100; // TODO: find + public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find + public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9e2c886..e9d97b2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -133,8 +133,8 @@ public class RobotContainer { getDriverController().getLeftY(), //getDriverController().getRightX(), getDriverController().getRightX(), - getDriverController().getRightY(), - true), + //getDriverController().getRightY(), + false), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers m_robotIntake.setDefaultCommand( @@ -147,9 +147,9 @@ public class RobotContainer { new RunCommand(() -> m_robotStorage.manageStorage(), m_robotStorage).withName("Storage manageStorage defaultCommand"));*/ // Serializer Management - // m_robotSerializer.setDefaultCommand( - // new RunCommand(() -> m_robotSerializer.setSerializer(0.8),//m_robotSerializer.setSerializerStateWithBeam(), - // m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); + m_robotSerializer.setDefaultCommand( + new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), + m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), @@ -194,11 +194,11 @@ public class RobotContainer { // 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 */ @@ -209,22 +209,39 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(() -> m_robotIntake.runExtender(false));*/ - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0))); - - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); - - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.3))) + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft + // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0))); + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425))) + .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-25))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //12ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.475))) + .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-47.57))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); + + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) //16ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.5))) + .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))).whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.75), m_robotStorage)) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.75), m_robotStorage)) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 7f87f3e..65493c8 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -20,6 +20,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.CSV; @@ -31,6 +32,7 @@ public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconRight; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; + double speed2; double velP; double input; @@ -168,14 +170,15 @@ public class BoomBoom extends SubsystemBase { public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { m_hoodSubsystem = subsystem0; m_turretSubsystem = subsystem1; - } + } /** * Runs the Drum motor at a given speed * @param speed percent output form -1.0 to 1.0 */ public void runDrumShooter(double speed) { - m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); + SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); } @@ -199,4 +202,9 @@ public class BoomBoom extends SubsystemBase { // feedforward.calculate(targetVel)); // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } + + public void increaseSpeed(double amount) + { + speed2 = speed2 + amount; + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 2305c86..8ab8a9f 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -20,8 +20,8 @@ public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; public CANSparkMax m_angleAdjusterMotor; - public SparkMaxLimitSwitch m_hoodUpLimitSwitch; - public SparkMaxLimitSwitch m_hoodDownLimitSwitch; + // public SparkMaxLimitSwitch m_hoodUpLimitSwitch; + // public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains; public RelativeEncoder m_angleEncoder; @@ -41,10 +41,10 @@ public double m_fireAngle; m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodUpLimitSwitch.enableLimitSwitch(true); - m_hoodDownLimitSwitch.enableLimitSwitch(true); + // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodUpLimitSwitch.enableLimitSwitch(true); + // m_hoodDownLimitSwitch.enableLimitSwitch(true); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 36e0f52..d782706 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -38,13 +38,13 @@ public class SwerveDrive extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), - Units.inchesToMeters(halfWidth)); + Units.inchesToMeters(-halfWidth)); Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), - Units.inchesToMeters(-halfWidth)); - Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth)); - Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), + Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth)); + Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), + Units.inchesToMeters(halfWidth)); public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); @@ -77,7 +77,7 @@ public class SwerveDrive extends SubsystemBase { m_rightBack = rightBack; m_gyro = gyro; - modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack }; + modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(), @@ -108,7 +108,7 @@ public class SwerveDrive extends SubsystemBase { ignoreAngles = true; else ignoreAngles = false; - Translation2d speed = new Translation2d(-speedX, speedY); + Translation2d speed = new Translation2d(-speedY, -speedX); double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); @@ -116,9 +116,9 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + rot * SwerveDriveConstants.ROTATION_SPEED * 8, m_gyro.getRotation2d()) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED); + -rot * SwerveDriveConstants.ROTATION_SPEED * 8); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -128,17 +128,19 @@ public class SwerveDrive extends SubsystemBase { Translation2d speed = new Translation2d(-leftX, leftY); speed = speed.times(speed.getNorm() * speedAdjust); if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + rotTarget = new Rotation2d(rightX, -rightY).plus(new Rotation2d(0, 1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); double xSpeedMetersPerSecond = -speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED); + rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getDegrees())) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); setModuleStates(states); + // SmartDashboard.putNumber("rot", rot); + // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); } /** @@ -149,6 +151,7 @@ public class SwerveDrive extends SubsystemBase { public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); + // int i = 2; { for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index bd1d8ee..1a3ebbc 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -120,6 +120,7 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; if (!ignoreAngle) { + angleMotor.set(TalonFXControlMode.Position, desiredTicks); } From e237f14537bf94b695fe1e676ce380d382d32a5c Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 10 Mar 2022 16:47:32 -0700 Subject: [PATCH 037/180] FIX --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 45 ++++++++++++++----- .../frc4388/robot/subsystems/BoomBoom.java | 3 +- .../frc4388/robot/subsystems/Climber.java | 26 +++++++++++ .../frc4388/robot/subsystems/SwerveDrive.java | 28 +++++------- .../DeadbandedRawXboxController.java | 3 +- .../controller/DeadbandedXboxController.java | 3 +- 7 files changed, 80 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ae01bf8..4c4ef0a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -31,7 +31,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 0.5; + public static final double ROTATION_SPEED = 4.0; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -150,6 +150,7 @@ public final class Constants { public static final int XBOX_OPERATOR_ID = 1; public static final double LEFT_AXIS_DEADBAND = 0.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; + public static final boolean SKEW_STICKS = true; } public static final class ShooterConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e9d97b2..ec1630e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,12 +29,16 @@ import java.util.regex.Matcher; import java.util.regex.Pattern; import java.util.stream.Collectors; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; import com.pathplanner.lib.commands.PPSwerveControllerCommand; +import org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM; + import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -62,6 +66,7 @@ import frc4388.robot.commands.AimToCenter; import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -98,8 +103,11 @@ public class RobotContainer { public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - // private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);*/ + private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); + private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); + private final WPI_TalonFX testSoulderMotor = new WPI_TalonFX(31); + private final Climber m_robotClimber = new Climber(testElbowMotor); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -124,6 +132,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); + testSoulderMotor.setNeutralMode(NeutralMode.Brake); /* Default Commands */ // Swerve Drive with Input @@ -133,7 +142,7 @@ public class RobotContainer { getDriverController().getLeftY(), //getDriverController().getRightX(), getDriverController().getRightX(), - //getDriverController().getRightY(), + // getDriverController().getRightY(), false), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers @@ -142,6 +151,9 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) + ); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), @@ -155,7 +167,7 @@ public class RobotContainer { new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY() * 0.1), m_robotHood)); + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -190,6 +202,14 @@ public class RobotContainer { // Right Bumper > Shift Up new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + new JoystickButton(getDriverController(), XboxController.Button.kA.value) + .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); + + new JoystickButton(getDriverController(), XboxController.Button.kB.value) + .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); + // .whileHeld(new RunCommand(() -> testElbowMotor.set(-0.2))) + // .whenReleased(new RunCommand(() -> testElbowMotor.set(0))); // new JoystickButton(getDriverController(), XboxController.Button.kA.value) // .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); @@ -198,7 +218,7 @@ public class RobotContainer { // .whenPressed(() -> m_robotMap.leftFront.reset()) // .whenPressed(() -> m_robotMap.rightFront.reset()) // .whenPressed(() -> m_robotMap.leftBack.reset()) - // .whenPressed(() -> m_robotMap.rightBack.reset()); + // .whenPressed(() -> m_robotMap.rightBack.reset()); /* Operator Buttons */ @@ -231,10 +251,11 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))).whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))) + .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) @@ -251,12 +272,14 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // A > Shoot with Odo /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - // B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));*/ + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ + + //B > Shoot with Lime + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 65493c8..80a7cfd 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -18,6 +18,7 @@ import java.util.stream.IntStream; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -50,7 +51,6 @@ public class BoomBoom extends SubsystemBase { } private ShooterTableEntry[] m_shooterTable; - /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ @@ -165,6 +165,7 @@ public class BoomBoom extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0); } public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { 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..a55ece0 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,26 @@ +// 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 com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + WPI_TalonFX m_climberElbow; + /** Creates a new Climber. */ + public Climber(WPI_TalonFX climberElbow) { + m_climberElbow = climberElbow; + } + + public void runWithInput(double input){ + m_climberElbow.set(input); + } + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d782706..36dc773 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -37,14 +37,10 @@ public class SwerveDrive extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), - Units.inchesToMeters(-halfWidth)); - Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), - Units.inchesToMeters(halfWidth)); - Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), - Units.inchesToMeters(-halfWidth)); - Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), - Units.inchesToMeters(halfWidth)); + Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth)); + Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth)); + Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth)); + Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth)); public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); @@ -108,29 +104,29 @@ public class SwerveDrive extends SubsystemBase { ignoreAngles = true; else ignoreAngles = false; - Translation2d speed = new Translation2d(-speedY, -speedX); + Translation2d speed = new Translation2d(speedX, speedY); double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); - double xSpeedMetersPerSecond = -speed.getX(); + double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED * 8, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * 8); + -rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, + -rot * SwerveDriveConstants.ROTATION_SPEED); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - Translation2d speed = new Translation2d(-leftX, leftY); + Translation2d speed = new Translation2d(leftX, leftY); speed = speed.times(speed.getNorm() * speedAdjust); if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightX, -rightY).plus(new Rotation2d(0, 1)); + rotTarget = new Rotation2d(rightY, rightX); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = -speed.getX(); + double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, diff --git a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java index e1679dd..177ef4e 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java @@ -3,6 +3,7 @@ package frc4388.utility.controller; import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; +import frc4388.robot.Constants.OIConstants; public class DeadbandedRawXboxController extends RawXboxController { public DeadbandedRawXboxController(int port) { super(port); } @@ -13,7 +14,7 @@ public class DeadbandedRawXboxController extends RawXboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (magnitude >= 1) return translation2d.div(magnitude); + if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; } diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 1c6fe5f..5b1bc97 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -4,6 +4,7 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; +import frc4388.robot.Constants.OIConstants; public class DeadbandedXboxController extends XboxController { public DeadbandedXboxController(int port) { super(port); } @@ -14,7 +15,7 @@ public class DeadbandedXboxController extends XboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (magnitude >= 1) return translation2d.div(magnitude); + if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); return translation2d; } From 74d83a620df2fc65cf7672795ec07fc42b315edc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 10 Mar 2022 16:56:27 -0700 Subject: [PATCH 038/180] convert shooter tables to percent output --- src/main/deploy/ShooterData.csv | 6 ++++-- src/main/java/frc4388/robot/commands/TrackTarget.java | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 573b060..5e4b222 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,3 +1,5 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) -0 ,16 ,0 -999 ,28.8 ,1200 \ No newline at end of file +96 ,-25.00 ,0.425 +144 ,-47.57 ,0.475 +192 ,-55.55 ,0.500 +240 ,-96.00 ,0.525 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 4cd9945..041e723 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -78,7 +78,8 @@ public class TrackTarget extends CommandBase { } vel = m_boomBoom.getVelocity(distance); hood = m_boomBoom.getHood(distance); - m_boomBoom.runDrumShooterVelocityPID(vel); + m_boomBoom.runDrumShooter(vel); + // m_boomBoom.runDrumShooterVelocityPID(vel); m_hood.runAngleAdjustPID(hood); //m_turret.runshooterRotatePID(m_targetAngle); } From 2b0dd5336036769afaaefbd8884d4a7192a4749d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 10 Mar 2022 17:06:17 -0700 Subject: [PATCH 039/180] 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 040/180] 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 263e324798c8d0511395dadd1a4c958c5700cc43 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 10 Mar 2022 17:46:51 -0700 Subject: [PATCH 041/180] sum change --- .../java/frc4388/robot/RobotContainer.java | 57 ++++++++----------- .../frc4388/robot/commands/TrackTarget.java | 7 ++- 2 files changed, 28 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ec1630e..93adb1f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -106,7 +106,7 @@ public class RobotContainer { private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); - private final WPI_TalonFX testSoulderMotor = new WPI_TalonFX(31); + private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); private final Climber m_robotClimber = new Climber(testElbowMotor); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -132,7 +132,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - testSoulderMotor.setNeutralMode(NeutralMode.Brake); + testShoulderMotor.setNeutralMode(NeutralMode.Brake); /* Default Commands */ // Swerve Drive with Input @@ -172,13 +172,10 @@ public class RobotContainer { // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); // 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(); - */ + // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand")); + + // autoInit(); + // recordInit(); } /** @@ -229,33 +226,27 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(() -> m_robotIntake.runExtender(false));*/ - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft - // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0))); - // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425))) - .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-25))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425))) + // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-25))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //12ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.475))) - .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-47.57))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // .whenPressed(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0))); - // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //12ft + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.475))) + // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-47.57))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) //16ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.5))) - .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) //16ft + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.5))) + // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))) - .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) - // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))) + // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) @@ -278,8 +269,8 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ //B > Shoot with Lime - // 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)); } /** diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 041e723..093242b 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -46,7 +46,8 @@ public class TrackTarget extends CommandBase { m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; - addRequirements(m_turret, m_drive, m_visionOdometry); + + addRequirements(m_turret, m_boomBoom, m_hood, m_drive, m_visionOdometry); } // Called when the command is initially scheduled. @@ -78,9 +79,9 @@ public class TrackTarget extends CommandBase { } vel = m_boomBoom.getVelocity(distance); hood = m_boomBoom.getHood(distance); - m_boomBoom.runDrumShooter(vel); + // m_boomBoom.runDrumShooter(vel); // m_boomBoom.runDrumShooterVelocityPID(vel); - m_hood.runAngleAdjustPID(hood); + // m_hood.runAngleAdjustPID(hood); //m_turret.runshooterRotatePID(m_targetAngle); } From 03485c1063262dd0e97964b08bab9e643019d6e9 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 12 Mar 2022 11:36:10 -0700 Subject: [PATCH 042/180] current limits --- src/main/java/frc4388/robot/Constants.java | 11 +++++++++-- src/main/java/frc4388/robot/Robot.java | 11 +++++++++++ .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 18 ++++++++++++++++-- .../frc4388/robot/subsystems/BoomBoom.java | 6 ++++++ .../java/frc4388/robot/subsystems/Climber.java | 4 ++++ .../java/frc4388/robot/subsystems/Hood.java | 4 ++++ .../java/frc4388/robot/subsystems/Intake.java | 7 +++++++ .../java/frc4388/robot/subsystems/LED.java | 1 + .../frc4388/robot/subsystems/Serializer.java | 3 +++ .../java/frc4388/robot/subsystems/Storage.java | 3 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 4 ++++ .../frc4388/robot/subsystems/SwerveModule.java | 6 ++++-- .../java/frc4388/robot/subsystems/Turret.java | 4 ++++ 14 files changed, 77 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4c4ef0a..7725175 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,6 +4,7 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import edu.wpi.first.math.controller.PIDController; @@ -129,6 +130,10 @@ public final class Constants { // CAN IDs public static final int INTAKE_MOTOR = 15; public static final int EXTENDER_MOTOR = 16; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE = new SupplyCurrentLimitConfiguration( + false, 10, 0, 0); //Find + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration( + false, 15, 0, 0); } public static final class StorageConstants { public static final int STORAGE_CAN_ID = 18; @@ -160,8 +165,10 @@ public final class Constants { public static final int SHOOTER_TIMEOUT_MS = 32; public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; - public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration( - true, 60, 40, 0.5); + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER = new SupplyCurrentLimitConfiguration( + true, 10, 0, 0); + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_SHOOTER = new StatorCurrentLimitConfiguration( + true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; public static final double TURRET_SPEED_MULTIPLIER = 0.4d; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e9d690b..51ed393 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -43,6 +43,7 @@ public class Robot extends TimedRobot { private SendableChooser odoChooser = new SendableChooser(); private HashMap odoChoices = new HashMap<>(); private Pose2d selectedOdo; + private double current; /** * This function is run when the robot is first started up and should be @@ -130,6 +131,16 @@ public class Robot extends TimedRobot { m_robotTime.updateTimes(); SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); + current = + m_robotContainer.m_robotBoomBoom.getCurrent() + + m_robotContainer.m_robotClimber.getCurrent() + + m_robotContainer.m_robotHood.getCurrent() + + m_robotContainer.m_robotIntake.getCurrent() + + m_robotContainer.m_robotSerializer.getCurrent() + + m_robotContainer.m_robotStorage.getCurrent() + + m_robotContainer.m_robotSwerveDrive.getCurrent() + + m_robotContainer.m_robotTurret.getCurrent(); + SmartDashboard.putNumber("Total Robot Current Draw", current); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled // commands, running already-scheduled commands, removing finished or diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ec1630e..2235b7b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -107,7 +107,7 @@ public class RobotContainer { private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); private final WPI_TalonFX testSoulderMotor = new WPI_TalonFX(31); - private final Climber m_robotClimber = new Climber(testElbowMotor); + public final Climber m_robotClimber = new Climber(testElbowMotor); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 0e13aac..58fa705 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -16,6 +16,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.networktables.NetworkTableType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.IntakeConstants; @@ -186,7 +187,9 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON @@ -201,7 +204,9 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.follow(shooterFalconLeft); @@ -230,6 +235,15 @@ public class RobotMap { public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + void configureIntakeMotors(){ + intakeMotor.configFactoryDefault(); + intakeMotor.setNeutralMode(NeutralMode.Coast); + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE, + ShooterConstants.SHOOTER_TIMEOUT_MS); + intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE, + ShooterConstants.SHOOTER_TIMEOUT_MS); + } + /* Storage Subsystem */ public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); // public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 80a7cfd..b7eaced 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -180,6 +180,8 @@ public class BoomBoom extends SubsystemBase { public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); + SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); + SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); } @@ -208,4 +210,8 @@ public class BoomBoom extends SubsystemBase { { speed2 = speed2 + amount; } + + public double getCurrent(){ + return m_shooterFalconLeft.getSupplyCurrent() + m_shooterFalconRight.getSupplyCurrent(); + } } \ 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 a55ece0..052d024 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -23,4 +23,8 @@ public class Climber extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run } + + public double getCurrent() { + return m_climberElbow.getSupplyCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 8ab8a9f..0a4b39c 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -95,4 +95,8 @@ public double m_fireAngle; public double getAnglePositionDegrees(){ return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; } + + public double getCurrent(){ + return m_angleAdjusterMotor.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index fa11c34..719891e 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkMaxLimitSwitch; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; @@ -52,6 +53,8 @@ public class Intake extends SubsystemBase { */ public void runWithTriggers(double leftTrigger, double rightTrigger) { m_intakeMotor.set((rightTrigger - leftTrigger) * 0.3); + SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } /** * Runs The Extender- @@ -74,4 +77,8 @@ public class Intake extends SubsystemBase { toggle = !toggle; runExtender(toggle); } + + public double getCurrent(){ + return m_intakeMotor.getSupplyCurrent() + m_extenderMotor.getOutputCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e46be14..7a24a0c 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -57,4 +57,5 @@ public class LED extends SubsystemBase { public LEDPatterns getPattern() { return m_currentPattern; } + } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index e3f2468..54df449 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -48,4 +48,7 @@ public class Serializer extends SubsystemBase{ public boolean getSerializerState() { return serializerState; } + public double getCurrent(){ + return m_serializerBelt.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index dcaddd7..0a0dc22 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -65,4 +65,7 @@ public class Storage extends SubsystemBase { public void periodic() { //manageStorage(); } + public double getCurrent(){ + return m_storageMotor.getOutputCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 36dc773..f0419d3 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -258,4 +258,8 @@ public class SwerveDrive extends SubsystemBase { speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; } } + + public double getCurrent(){ + return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1a3ebbc..299ddd4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -120,7 +120,7 @@ public class SwerveModule extends SubsystemBase { double desiredTicks = currentTicks + deltaTicks; if (!ignoreAngle) { - + angleMotor.set(TalonFXControlMode.Position, desiredTicks); } @@ -192,5 +192,7 @@ public class SwerveModule extends SubsystemBase { canCoder.setPositionToAbsolute(); // canCoder.configSensorInitializationStrategy(initializationStrategy) } - + public double getCurrent(){ + return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1a97942..b133d6f 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -109,4 +109,8 @@ public class Turret extends SubsystemBase { / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; } + public double getCurrent(){ + return m_boomBoomRotateMotor.getOutputCurrent(); + } + } \ No newline at end of file From 42f13035ef59101f6499420472cee871dca2f174 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 12:02:38 -0700 Subject: [PATCH 043/180] changed try catch for track target --- .../frc4388/robot/commands/TrackTarget.java | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 093242b..ffe50c5 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -61,21 +61,21 @@ public class TrackTarget extends CommandBase { @Override public void execute() { //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); - m_visionOdometry.setLEDs(true); - points = m_visionOdometry.getTargetPoints(); - double pointTotal = 0; - for(Point point : points) - { - pointTotal = pointTotal + point.x; - } - average = pointTotal/points.size(); - output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP; - m_turret.runTurretWithInput(output); - try{ + try { + m_visionOdometry.setLEDs(true); + points = m_visionOdometry.getTargetPoints(); + double pointTotal = 0; + for(Point point : points) { + pointTotal = pointTotal + point.x; + } + average = pointTotal/points.size(); + output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP; + m_turret.runTurretWithInput(output); pos = m_visionOdometry.getVisionOdometry(); distance = Math.hypot(pos.getX(), pos.getY()); } catch (Exception e){ + System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } vel = m_boomBoom.getVelocity(distance); hood = m_boomBoom.getHood(distance); From 89ba3afb00e61e5bfa47b5adae8332e3830aaf32 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sat, 12 Mar 2022 12:09:10 -0700 Subject: [PATCH 044/180] updated exception --- .../java/frc4388/robot/subsystems/VisionOdometry.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 162a897..d4b60db 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -54,13 +54,14 @@ public class VisionOdometry extends SubsystemBase { * Breaks down targets into 4 corners and uses the top 2 points * * @return Vision points on the rim of the target in screen space + * @throws VisionObscuredException */ - public ArrayList getTargetPoints() { + public ArrayList getTargetPoints() throws VisionObscuredException { PhotonPipelineResult result = m_camera.getLatestResult(); latency = result.getLatencyMillis(); if(!result.hasTargets()) - return new ArrayList(); + throw new VisionObscuredException(); ArrayList points = new ArrayList<>(); @@ -99,7 +100,7 @@ public class VisionOdometry extends SubsystemBase { ArrayList screenPoints = getTargetPoints(); if(screenPoints.size() < 3) - throw new VisionObscuredException("Not enough vision points available"); + throw new VisionObscuredException("Less than 3 vision points available"); ArrayList points3d = get3dPoints(screenPoints); ArrayList points = topView(points3d); @@ -125,6 +126,7 @@ public class VisionOdometry extends SubsystemBase { public double getLatency() { return latency; } + public boolean getLEDs() { if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false; return true; From 032e176efeac426ecd3443053e25911ec6a9b3e4 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:05:33 -0700 Subject: [PATCH 045/180] velocity correction and shooter tables time --- simgui.json | 3 + src/main/deploy/ShooterData.csv | 10 ++-- src/main/java/frc4388/robot/Robot.java | 7 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 1 + src/main/java/frc4388/utility/Vector2D.java | 56 +++++++++++++++++++ .../frc4388/utility/VelocityCorrection.java | 47 ++++++++++++++++ 6 files changed, 119 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc4388/utility/Vector2D.java create mode 100644 src/main/java/frc4388/utility/VelocityCorrection.java diff --git a/simgui.json b/simgui.json index 4e5f439..7037638 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,7 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Climber": "Subsystem", "/LiveWindow/Hood": "Subsystem", "/LiveWindow/Intake": "Subsystem", "/LiveWindow/LED": "Subsystem", @@ -46,6 +47,8 @@ "/LiveWindow/Ungrouped/Talon FX [23]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [24]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [2]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [30]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [31]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [3]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller", diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 5e4b222..b0f3627 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,5 +1,5 @@ -Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) -96 ,-25.00 ,0.425 -144 ,-47.57 ,0.475 -192 ,-55.55 ,0.500 -240 ,-96.00 ,0.525 \ No newline at end of file +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Time (s) +96 ,-25.00 ,0.425 ,0 +144 ,-47.57 ,0. ,0 +192 ,-55.55 ,0.500 ,0 +240 ,-96.00 ,0.525 ,0 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 51ed393..1215bbe 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.RunCommand; import frc4388.utility.RobotTime; +import frc4388.utility.VelocityCorrection; /** * The VM is configured to automatically run this class, and to call the @@ -150,6 +151,12 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom); + System.out.println("Position: " + vc.position); + System.out.println("Velocity: " + vc.cartesianVelocity); + System.out.println("Target: " + vc.target.toString()); + + //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition()); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f0419d3..b5c3176 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -163,6 +163,7 @@ public class SwerveDrive extends SubsystemBase { updateSmartDash(); SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); + SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java new file mode 100644 index 0000000..6a56a21 --- /dev/null +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -0,0 +1,56 @@ +// 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 Vector2D() { + super(); + } + + public Vector2D(double x, double y) { + super(x, y); + + this.x = x; + this.y = y; + } + + public static Vector2D add(Vector2D v1, Vector2D v2) { + return new Vector2D(v1.x + v2.x, v1.y + v2.y); + } + + public static Vector2D subtract(Vector2D v1, Vector2D v2) { + return new Vector2D(v1.x - v2.x, v1.y - v2.y); + } + + public static Vector2D multiply(Vector2D v1, double scalar) { + return new Vector2D(scalar * v1.x, scalar * v1.y); + } + + 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 + ")"); + } + +} diff --git a/src/main/java/frc4388/utility/VelocityCorrection.java b/src/main/java/frc4388/utility/VelocityCorrection.java new file mode 100644 index 0000000..cc11d0d --- /dev/null +++ b/src/main/java/frc4388/utility/VelocityCorrection.java @@ -0,0 +1,47 @@ +// 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.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.SwerveDrive; + +/** Add your docs here. */ +public class VelocityCorrection { + + SwerveDrive swerve; + BoomBoom boomBoom; + + // vectors (in ft and ft/sec) + public Vector2D position; + public Vector2D cartesianVelocity; + + // find + public Vector2D target; + + public VelocityCorrection(SwerveDrive swerve, BoomBoom boomBoom) { + + this.swerve = swerve; + this.boomBoom = boomBoom; + + position = new Vector2D(5, 0);//new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY()); + cartesianVelocity = new Vector2D(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond); + + target = getTargetPoint(); + } + + private Vector2D getTargetPoint() { + double approxShotTime = 1; // TODO: get shot time from shooter tables + + Vector2D targetPoint = Vector2D.multiply(this.cartesianVelocity, -1 * approxShotTime); + + return Vector2D.round(targetPoint, 5); + } + +} From 7e2505f0f377c5c372245a501d5e7b5e9445c840 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:21:03 -0700 Subject: [PATCH 046/180] unit vector method --- src/main/java/frc4388/utility/Vector2D.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 6a56a21..0e194e4 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -35,6 +35,10 @@ public class Vector2D extends Vector2d { return new Vector2D(scalar * v1.x, scalar * v1.y); } + public Vector2D unit() { + return new Vector2D(this.x / this.magnitude(), this.y / this.magnitude()); + } + public static Vector2D round(Vector2D v, int places) { int scale = (int) Math.pow(10, places); @@ -50,6 +54,7 @@ public class Vector2D extends Vector2d { @Override public String toString() { + Vector2d test = new Vector2d(); return ("(" + this.x + ", " + this.y + ")"); } From 06ccba971ef3f4398946d823d5f9387537f1362b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:35:47 -0700 Subject: [PATCH 047/180] vector2d angle --- src/main/java/frc4388/robot/RobotContainer.java | 1 + src/main/java/frc4388/utility/Vector2D.java | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fb53f70..09051b9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -79,6 +79,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 0e194e4..7255d66 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -4,6 +4,7 @@ package frc4388.utility; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.drive.Vector2d; /** Aarav's good vector class (better than WPILib) */ @@ -11,9 +12,11 @@ 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) { @@ -21,6 +24,7 @@ public class Vector2D extends Vector2d { this.x = x; this.y = y; + this.angle = Math.atan2(this.y, this.x); } public static Vector2D add(Vector2D v1, Vector2D v2) { From 35662f72f9ff05490d517feb1997e7a2a8e4e647 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 14:54:42 -0700 Subject: [PATCH 048/180] shooter tables duration --- src/main/deploy/ShooterData.csv | 2 +- .../frc4388/robot/subsystems/BoomBoom.java | 39 +++++++++++++------ 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index b0f3627..16e194d 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,4 +1,4 @@ -Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Time (s) +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s) 96 ,-25.00 ,0.425 ,0 144 ,-47.57 ,0. ,0 192 ,-55.55 ,0.500 ,0 diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index b7eaced..a2e87df 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -47,14 +47,12 @@ public class BoomBoom extends SubsystemBase { // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later public static class ShooterTableEntry { - public Double distance, hoodExt, drumVelocity; + public Double distance, hoodExt, drumVelocity, duration; } private ShooterTableEntry[] m_shooterTable; - /* - * Creates new BoomBoom subsystem, has drum shooter and angle adjuster - */ - /** Creates a new BoomBoom. */ + + /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; @@ -94,20 +92,39 @@ public class BoomBoom extends SubsystemBase { } } + /** + * This is a function that takes a value (distance) and returns a value (drumVelocity) that is a + * linear interpolation of the two values (drumVelocity) at the two closest points in the table + * (m_shooterTable) to the given value (distance). + * @param distance Distance in shooter table + * @return Drum Velocity in units per 100 ms + */ public Double getVelocity(final Double distance) { - // This is a function that takes a value (distance) and returns a value (drumVelocity) that is a - // linear interpolation of the two values (drumVelocity) at the two closest points in the table - // (m_shooterTable) to the given value (distance). return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); } + /** + * This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear + * interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable) + * to the given value (distance). + * @param distance Distance in shooter table + * @return Hood extension in units + */ public Double getHood(final Double distance) { - // This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear - // interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable) - // to the given value (distance). return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } + /** + * This is a function that takes a value (distance) and returns a value (duration) that is a linear + * interpolation of the two values (duration) at the two closest points in the table (m_shooterTable) + * to the given value (distance). + * @param distance Distance in shooter table + * @return Shot duration in seconds + */ + public Double getDuration(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.duration).doubleValue(); + } + /** * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the From 79435399a35ace5dc3f199755a5bd24f9a562dab Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 12 Mar 2022 14:59:21 -0700 Subject: [PATCH 049/180] fixes --- simgui.json | 6 + src/main/java/frc4388/robot/Constants.java | 1 - src/main/java/frc4388/robot/Robot.java | 12 +- .../java/frc4388/robot/RobotContainer.java | 37 +++--- src/main/java/frc4388/robot/RobotMap.java | 107 +++++++++--------- .../java/frc4388/robot/subsystems/Hood.java | 5 +- .../java/frc4388/robot/subsystems/Intake.java | 11 +- .../frc4388/robot/subsystems/Serializer.java | 5 +- .../frc4388/robot/subsystems/Storage.java | 2 +- 9 files changed, 103 insertions(+), 83 deletions(-) diff --git a/simgui.json b/simgui.json index 4e5f439..935c3ae 100644 --- a/simgui.json +++ b/simgui.json @@ -22,6 +22,7 @@ "types": { "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Climber": "Subsystem", "/LiveWindow/Hood": "Subsystem", "/LiveWindow/Intake": "Subsystem", "/LiveWindow/LED": "Subsystem", @@ -46,6 +47,8 @@ "/LiveWindow/Ungrouped/Talon FX [23]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [24]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [2]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [30]": "Motor Controller", + "/LiveWindow/Ungrouped/Talon FX [31]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [3]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [4]": "Motor Controller", "/LiveWindow/Ungrouped/Talon FX [5]": "Motor Controller", @@ -73,5 +76,8 @@ }, "open": true } + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7725175..8ae655f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -105,7 +105,6 @@ public final class Constants { public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double INCHES_PER_METER = 39.370; public static final double METERS_PER_INCH = 1 / INCHES_PER_METER; - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 51ed393..e4fd2ea 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -129,17 +129,17 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); - SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); + // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); + // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); current = - m_robotContainer.m_robotBoomBoom.getCurrent() + + // m_robotContainer.m_robotBoomBoom.getCurrent() + m_robotContainer.m_robotClimber.getCurrent() + - m_robotContainer.m_robotHood.getCurrent() + + // m_robotContainer.m_robotHood.getCurrent() + m_robotContainer.m_robotIntake.getCurrent() + m_robotContainer.m_robotSerializer.getCurrent() + m_robotContainer.m_robotStorage.getCurrent() + - m_robotContainer.m_robotSwerveDrive.getCurrent() + - m_robotContainer.m_robotTurret.getCurrent(); + m_robotContainer.m_robotSwerveDrive.getCurrent(); + // m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fb53f70..3da5b6a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -100,11 +100,10 @@ public class RobotContainer { public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); - public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + // public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + // public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); public final Climber m_robotClimber = new Climber(testElbowMotor); @@ -133,7 +132,7 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - testShoulderMotor.setNeutralMode(NeutralMode.Brake); + // testShoulderMotor.setNeutralMode(NeutralMode.Brake); /* Default Commands */ // Swerve Drive with Input @@ -152,9 +151,9 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) - ); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) + // ); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), @@ -167,8 +166,8 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); + // m_robotHood.setDefaultCommand( + // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -201,11 +200,11 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); + // new JoystickButton(getDriverController(), XboxController.Button.kA.value) + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); - new JoystickButton(getDriverController(), XboxController.Button.kB.value) - .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); + // new JoystickButton(getDriverController(), XboxController.Button.kB.value) + // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(-0.025)); // .whileHeld(new RunCommand(() -> testElbowMotor.set(-0.2))) // .whenReleased(new RunCommand(() -> testElbowMotor.set(0))); @@ -256,6 +255,13 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(1.0), m_robotIntake)) + .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-1.0), m_robotIntake)) + .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) @@ -265,13 +271,14 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + // A > Shoot with Odo /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ //B > Shoot with Lime - 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)); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 58fa705..3aed04d 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -36,7 +36,7 @@ public class RobotMap { public RobotMap() { // configureLEDMotorControllers(); configureSwerveMotorControllers(); - configureShooterMotorControllers(); + // configureShooterMotorControllers(); } /* LED Subsystem */ @@ -162,73 +162,72 @@ public class RobotMap { SwerveDriveConstants.SWERVE_TIMEOUT_MS); } - // Shooter Config - /* Boom Boom Subsystem */ - public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); - public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); +// // Shooter Config +// /* Boom Boom Subsystem */ +// public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); +// public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // turret subsystem +// // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); - // hood subsystem - public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); +// // hood subsystem +// public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - // Create motor CANSparkMax - void configureShooterMotorControllers() { +// // Create motor CANSparkMax +// void configureShooterMotorControllers() { - // LEFT FALCON - shooterFalconLeft.configFactoryDefault(); - shooterFalconLeft.setNeutralMode(NeutralMode.Coast); - shooterFalconLeft.setInverted(true); - shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); +// // LEFT FALCON +// shooterFalconLeft.configFactoryDefault(); +// shooterFalconLeft.setNeutralMode(NeutralMode.Coast); +// shooterFalconLeft.setInverted(true); +// shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); - // RIGHT FALCON - shooterFalconRight.setInverted(false); - shooterFalconRight.setNeutralMode(NeutralMode.Coast); - shooterFalconRight.configFactoryDefault(); - shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - // m_shooterFalconRight.configPeakOutputForward(0, - // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) - shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); +// // RIGHT FALCON +// shooterFalconRight.setInverted(false); +// shooterFalconRight.setNeutralMode(NeutralMode.Coast); +// shooterFalconRight.configFactoryDefault(); +// shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); +// // m_shooterFalconRight.configPeakOutputForward(0, +// // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) +// shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); +// shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, +// ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.follow(shooterFalconLeft); +// shooterFalconRight.follow(shooterFalconLeft); - // /* Turret Subsytem */ - // shooterFalconRight.configStatorCurrentLimit(new - // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers - // out of our ass anymore - // shooterFalconLeft.configSupplyCurrentLimit(new - // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull - // numbers out of our ass anymore +// // /* Turret Subsytem */ +// // shooterFalconRight.configStatorCurrentLimit(new +// // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers +// // out of our ass anymore +// // shooterFalconLeft.configSupplyCurrentLimit(new +// // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull +// // numbers out of our ass anymore - // hood subsystem - angleAdjusterMotor.restoreFactoryDefaults(); - angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - } +// // hood subsystem +// angleAdjusterMotor.restoreFactoryDefaults(); +// angleAdjusterMotor.setIdleMode(IdleMode.kBrake); +// } /* Serializer Subsystem */ public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); -// public final CANSparkMax serializerShooterBelt = new CANSparkMax(SerializerConstants.SERIALIZER_SHOOTER_BELT, MotorType.kBrushless); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); /* Intake Subsytem */ diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 0a4b39c..1da9228 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -79,7 +79,10 @@ public double m_fireAngle; m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } - + /** + * Runs the hood with the given input + * @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle) + */ public void runHood(double input) { m_angleAdjusterMotor.set(input); } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 719891e..2754c6d 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -47,9 +47,9 @@ public class Intake extends SubsystemBase { // This method will be called once per scheduler run } /** - * Runs The Intake With Triggers. - * @param leftTrigger Left Trigger to Run - - * @param rightTrigger Right Trigger to Run + + * Runs The Intake With Triggers as input + * @param leftTrigger Left Trigger to Run Inward + * @param rightTrigger Right Trigger to Run Outward */ public void runWithTriggers(double leftTrigger, double rightTrigger) { m_intakeMotor.set((rightTrigger - leftTrigger) * 0.3); @@ -65,7 +65,10 @@ public class Intake extends SubsystemBase { double extenderMotorSpeed = extended ? 0.25d : -0.25d; m_extenderMotor.set(extenderMotorSpeed); } - + /** + * Moves the extender motor to pull the intake in or out + * @param input A value from -1.0 to 1.0, positive is in + */ public void runExtender(double input) { if (!m_serializer.getBeam() && input < 0.) return; m_extenderMotor.set(input); diff --git a/src/main/java/frc4388/robot/subsystems/Serializer.java b/src/main/java/frc4388/robot/subsystems/Serializer.java index 54df449..99702fc 100644 --- a/src/main/java/frc4388/robot/subsystems/Serializer.java +++ b/src/main/java/frc4388/robot/subsystems/Serializer.java @@ -19,7 +19,10 @@ public class Serializer extends SubsystemBase{ // m_serializerShooterBelt.set(0); } - + /** + * + * @param input from -1.0 to 1.0, positive is inward + */ public void setSerializer(double input){ m_serializerBelt.set(input); } diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 0a0dc22..8f78712 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -36,7 +36,7 @@ public class Storage extends SubsystemBase { /** * Runs The Storage at a Specifyed Speed - * @param input The Specifyed Speed + * @param input The value frm -1.0 to 1.0, positive is inwards (towards the shooter) */ public void runStorage(double input) { m_storageMotor.set(input); From 64391c99c22d0736d71ce42ba7c97e6f22a0e341 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 15:05:32 -0700 Subject: [PATCH 050/180] extender rework new subsystem --- src/main/java/frc4388/robot/Robot.java | 9 +-- .../java/frc4388/robot/RobotContainer.java | 13 ++-- .../frc4388/robot/subsystems/Extender.java | 66 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Intake.java | 46 ++----------- 4 files changed, 83 insertions(+), 51 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Extender.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0255f40..e89f8d9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -137,6 +137,7 @@ public class Robot extends TimedRobot { m_robotContainer.m_robotClimber.getCurrent() + // m_robotContainer.m_robotHood.getCurrent() + m_robotContainer.m_robotIntake.getCurrent() + + m_robotContainer.m_robotExtender.getCurrent() + m_robotContainer.m_robotSerializer.getCurrent() + m_robotContainer.m_robotStorage.getCurrent() + m_robotContainer.m_robotSwerveDrive.getCurrent(); @@ -151,10 +152,10 @@ public class Robot extends TimedRobot { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom); - System.out.println("Position: " + vc.position); - System.out.println("Velocity: " + vc.cartesianVelocity); - System.out.println("Target: " + vc.target.toString()); + // VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom); + // System.out.println("Position: " + vc.position); + // System.out.println("Velocity: " + vc.cartesianVelocity); + // System.out.println("Target: " + vc.target.toString()); //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 45095c2..fc00270 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,6 +67,7 @@ import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Extender; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -98,7 +99,9 @@ public class RobotContainer { // Subsystems public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.extenderMotor, m_robotSerializer); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotSerializer); + public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); + public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); // public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); @@ -258,11 +261,11 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(1.0), m_robotIntake)) - .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-1.0), m_robotIntake)) - .whenReleased(new RunCommand(() -> m_robotIntake.runExtender(0.0), m_robotIntake)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java new file mode 100644 index 0000000..f0749a5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -0,0 +1,66 @@ +// 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 com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Extender extends SubsystemBase { + + CANSparkMax m_extenderMotor; + + private SparkMaxLimitSwitch m_inLimit; + private SparkMaxLimitSwitch m_outLimit; + + public boolean toggle; + + /** Creates a new Extender. */ + public Extender(CANSparkMax extenderMotor) { + + m_extenderMotor = extenderMotor; + + m_extenderMotor.restoreFactoryDefaults(); + m_extenderMotor.setInverted(true); + + m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_inLimit.enableLimitSwitch(true); + m_outLimit.enableLimitSwitch(true); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Runs The Extender- + * @param extended Wether the Extender Is Extended + */ + // public void runExtender(boolean extended) { + // if (!m_serializer.getBeam() && !extended) return; + // double extenderMotorSpeed = extended ? 0.25d : -0.25d; + // m_extenderMotor.set(extenderMotorSpeed); + // } + + public void runExtender(double input) { + // if (!m_serializer.getBeam() && input < 0.) return; + m_extenderMotor.set(input); + } + + public double getCurrent() { + return m_extenderMotor.getOutputCurrent(); + } + + /** + * Toggles The Extender + */ + // public void toggleExtender() { + // toggle = !toggle; + // runExtender(toggle); + // } +} diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 2754c6d..4c086b2 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -17,29 +17,15 @@ import com.revrobotics.CANSparkMax; public class Intake extends SubsystemBase { private WPI_TalonFX m_intakeMotor; - private CANSparkMax m_extenderMotor; private Serializer m_serializer; - private SparkMaxLimitSwitch m_inLimit; - private SparkMaxLimitSwitch m_outLimit; - - public boolean toggle; /** Creates a new Intake. */ - public Intake(WPI_TalonFX intakeMotor, CANSparkMax extenderMotor, Serializer serializer) { + public Intake(WPI_TalonFX intakeMotor, Serializer serializer) { m_intakeMotor = intakeMotor; - m_extenderMotor = extenderMotor; m_serializer = serializer; - m_extenderMotor.restoreFactoryDefaults(); - m_intakeMotor.setNeutralMode(NeutralMode.Brake); m_intakeMotor.setInverted(false); - m_extenderMotor.setInverted(true); - - m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_inLimit.enableLimitSwitch(true); - m_outLimit.enableLimitSwitch(true); } @Override @@ -56,32 +42,8 @@ public class Intake extends SubsystemBase { SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } - /** - * Runs The Extender- - * @param extended Wether the Extender Is Extended - */ - public void runExtender(boolean extended) { - if (!m_serializer.getBeam() && !extended) return; - double extenderMotorSpeed = extended ? 0.25d : -0.25d; - m_extenderMotor.set(extenderMotorSpeed); - } - /** - * Moves the extender motor to pull the intake in or out - * @param input A value from -1.0 to 1.0, positive is in - */ - public void runExtender(double input) { - if (!m_serializer.getBeam() && input < 0.) return; - m_extenderMotor.set(input); - } - /** - * Toggles The Extender - */ - public void toggleExtender() { - toggle = !toggle; - runExtender(toggle); - } - - public double getCurrent(){ - return m_intakeMotor.getSupplyCurrent() + m_extenderMotor.getOutputCurrent(); + + public double getCurrent() { + return m_intakeMotor.getSupplyCurrent(); } } \ No newline at end of file From 281ab62dd7df072c4bae702ebd5d64e1aeaef97f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 17:32:10 -0700 Subject: [PATCH 051/180] current limits for drive --- src/main/java/frc4388/robot/Constants.java | 12 ++++++++++ src/main/java/frc4388/robot/RobotMap.java | 28 ++++++++++++++++++---- 2 files changed, 36 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8ae655f..df6d364 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -112,6 +112,18 @@ public final class Constants { public static final double TICK_TIME_TO_SECONDS = 0.1; public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + // current limits + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_STEER = new SupplyCurrentLimitConfiguration( + false, 10, 0, 0); + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_STEER = new StatorCurrentLimitConfiguration( + false, 15, 0, 0); + + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL = new SupplyCurrentLimitConfiguration( + false, 10, 0, 0); + public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_WHEEL = new StatorCurrentLimitConfiguration( + false, 15, 0, 0); + + // misc public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3aed04d..45e2f37 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -138,6 +138,28 @@ public class RobotMap { rightBackSteerMotor.setNeutralMode(mode); rightBackWheelMotor.setNeutralMode(mode);// Coast + // current limits + + leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + rightFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + leftBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + rightBackSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); + + leftFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); + + leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + rightBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); + + leftFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); + leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, @@ -237,10 +259,8 @@ public class RobotMap { void configureIntakeMotors(){ intakeMotor.configFactoryDefault(); intakeMotor.setNeutralMode(NeutralMode.Coast); - intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE, - ShooterConstants.SHOOTER_TIMEOUT_MS); - intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE, - ShooterConstants.SHOOTER_TIMEOUT_MS); + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); + intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); } /* Storage Subsystem */ From d2381220bb15d0a87ea5df8790e61e8bf040202f Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 12 Mar 2022 17:33:38 -0700 Subject: [PATCH 052/180] Button Box Broken --- simgui-ds.json | 3 ++ simgui.json | 1 + src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 50 +++++++++++++++++-- src/main/java/frc4388/robot/RobotMap.java | 1 + .../frc4388/utility/controller/ButtonBox.java | 23 +++++++++ 6 files changed, 75 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc4388/utility/controller/ButtonBox.java diff --git a/simgui-ds.json b/simgui-ds.json index b16ea5c..1cc0e4e 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,6 +96,9 @@ { "guid": "78696e70757401000000000000000000", "useGamepad": true + }, + { + "guid": "03000000c01600008704000000000000" } ] } diff --git a/simgui.json b/simgui.json index 935c3ae..f3617f0 100644 --- a/simgui.json +++ b/simgui.json @@ -23,6 +23,7 @@ "/FMSInfo": "FMSInfo", "/LiveWindow/BoomBoom": "Subsystem", "/LiveWindow/Climber": "Subsystem", + "/LiveWindow/Extender": "Subsystem", "/LiveWindow/Hood": "Subsystem", "/LiveWindow/Intake": "Subsystem", "/LiveWindow/LED": "Subsystem", diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8ae655f..37d287c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -152,6 +152,7 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; + public static final int BUTTON_FOX_ID = 2; public static final double LEFT_AXIS_DEADBAND = 0.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; public static final boolean SKEW_STICKS = true; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fc00270..e0241b7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -82,6 +82,7 @@ import frc4388.utility.ListeningSendableChooser; import frc4388.utility.PathPlannerUtil; import frc4388.utility.Vector2D; import frc4388.utility.PathPlannerUtil.Path.Waypoint; +import frc4388.utility.controller.ButtonBox; import frc4388.utility.controller.DeadbandedXboxController; /** @@ -115,6 +116,7 @@ public class RobotContainer { /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final ButtonBox m_buttonFox = new ButtonBox(OIConstants.BUTTON_FOX_ID); /* Autonomous */ private PathPlannerTrajectory loadedPathTrajectory = null; @@ -221,6 +223,11 @@ public class RobotContainer { // .whenPressed(() -> m_robotMap.leftBack.reset()) // .whenPressed(() -> m_robotMap.rightBack.reset()); + + + + + /* Operator Buttons */ // X > Extend Intake @@ -263,6 +270,7 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); @@ -283,6 +291,36 @@ public class RobotContainer { //B > Shoot with Lime // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); + + + + + + /* Button Box Buttons */ + new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftSwitch.value) + .whenPressed(new InstantCommand(() -> configureManualButtonBindings())) + .whenReleased(new InstantCommand(() -> configureButtonBindings())); + + new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) + .whileHeld(new RunCommand(() -> System.out.println("MiddleSwitch"))); + + new JoystickButton(getButtonFox(), ButtonBox.Button.kRightSwitch.value) + .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); + + new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftButton.value) + .whileHeld(new RunCommand(() -> System.out.println("LeftButton"))); + + new JoystickButton(getButtonFox(), ButtonBox.Button.kRightButton.value) + .whileHeld(new RunCommand(() -> System.out.println("RightButton"))); + } + + public void configureManualButtonBindings() + { + new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) + .whileHeld(new RunCommand(() -> System.out.println("Deez Nuts"))); + + new JoystickButton(getButtonFox(), ButtonBox.Button.kRightSwitch.value) + .whileHeld(new RunCommand(() -> System.out.println("Inna mouth"))); } /** @@ -317,6 +355,14 @@ public class RobotContainer { return m_driverXbox; } + public XboxController getOperatorController() { + return m_operatorXbox; + } + + public ButtonBox getButtonFox() { + return m_buttonFox; + } + /** * Get odometry. * @@ -335,10 +381,6 @@ public class RobotContainer { m_robotSwerveDrive.resetOdometry(pose); } - public XboxController getOperatorController() { - return m_operatorXbox; - } - /** * Creates a WatchKey for the path planner directory and registers it with the * WatchService. diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3aed04d..5e7360e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -222,6 +222,7 @@ public class RobotMap { // // hood subsystem // angleAdjusterMotor.restoreFactoryDefaults(); // angleAdjusterMotor.setIdleMode(IdleMode.kBrake); +// angleAdjusterMotor.setInverted(true); // } diff --git a/src/main/java/frc4388/utility/controller/ButtonBox.java b/src/main/java/frc4388/utility/controller/ButtonBox.java new file mode 100644 index 0000000..741d79e --- /dev/null +++ b/src/main/java/frc4388/utility/controller/ButtonBox.java @@ -0,0 +1,23 @@ +package frc4388.utility.controller; + +import edu.wpi.first.wpilibj.GenericHID; + +public class ButtonBox extends GenericHID { + public ButtonBox(int port) { + super(port); + } + public enum Button { + kRightSwitch(1), + kMiddleSwitch(2), + kLeftSwitch(3), + kRightButton(4), + kLeftButton(5); + + @SuppressWarnings("MemberName") + public final int value; + + Button(int value) { + this.value = value; + } + } +} From 18624989b71ca281323eb83ccfeb5fb9754d7492 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 12 Mar 2022 17:43:17 -0700 Subject: [PATCH 053/180] funk --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e0241b7..e9249a3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -298,8 +298,8 @@ public class RobotContainer { /* Button Box Buttons */ new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftSwitch.value) - .whenPressed(new InstantCommand(() -> configureManualButtonBindings())) - .whenReleased(new InstantCommand(() -> configureButtonBindings())); + .whenPressed(new RunCommand(() -> configureManualButtonBindings())) + .whenReleased(new RunCommand(() -> configureButtonBindings())); new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) .whileHeld(new RunCommand(() -> System.out.println("MiddleSwitch"))); From 42421b51afb115a0e892fb995b86f60d5f956aab Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 17:43:17 -0700 Subject: [PATCH 054/180] change --- src/main/java/frc4388/robot/Robot.java | 7 ++++ .../java/frc4388/robot/RobotContainer.java | 36 +++++++++++-------- 2 files changed, 29 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e89f8d9..416d090 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -270,6 +270,13 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { + boolean robotManual = m_robotContainer.manual; + + if (robotManual) { + m_robotContainer.configureManualButtonBindings(); + } else { + m_robotContainer.configureAutomaticButtonBindings(); + } } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e0241b7..df8cd63 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -132,6 +132,9 @@ public class RobotContainer { // Function that removes the ".path" from the end of a string. private static final Function PATH_EXTENSION_REMOVER = ((Function) Pattern .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); + + + public boolean manual = false; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -292,17 +295,13 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); - - - - /* Button Box Buttons */ new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftSwitch.value) - .whenPressed(new InstantCommand(() -> configureManualButtonBindings())) - .whenReleased(new InstantCommand(() -> configureButtonBindings())); + .whenPressed(new RunCommand(() -> setManual(true))) + .whenReleased(new RunCommand(() -> setManual(false))); - new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new RunCommand(() -> System.out.println("MiddleSwitch"))); + // new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) + // .whileHeld(new RunCommand(() -> System.out.println("MiddleSwitch"))); new JoystickButton(getButtonFox(), ButtonBox.Button.kRightSwitch.value) .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); @@ -314,13 +313,18 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> System.out.println("RightButton"))); } - public void configureManualButtonBindings() - { + public void configureManualButtonBindings() { + new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new RunCommand(() -> System.out.println("Deez Nuts"))); - - new JoystickButton(getButtonFox(), ButtonBox.Button.kRightSwitch.value) - .whileHeld(new RunCommand(() -> System.out.println("Inna mouth"))); + .whileHeld(new RunCommand(() -> System.out.println("MANUAL"))); + + } + + public void configureAutomaticButtonBindings() { + + new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) + .whileHeld(new RunCommand(() -> System.out.println("AUTOMATIC"))); + } /** @@ -363,6 +367,10 @@ public class RobotContainer { return m_buttonFox; } + public void setManual(boolean set) { + this.manual = set; + } + /** * Get odometry. * From 74b13a3b9d620f03162ea98160e8849078196f0a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 19:01:16 -0700 Subject: [PATCH 055/180] manual and normal button box switching --- src/main/java/frc4388/robot/Robot.java | 10 +-- .../java/frc4388/robot/RobotContainer.java | 33 +++------- .../robot/commands/RunMiddleSwitch.java | 61 +++++++++++++++++++ 3 files changed, 70 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunMiddleSwitch.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 416d090..04a55bc 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -269,15 +269,7 @@ public class Robot extends TimedRobot { * This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - boolean robotManual = m_robotContainer.manual; - - if (robotManual) { - m_robotContainer.configureManualButtonBindings(); - } else { - m_robotContainer.configureAutomaticButtonBindings(); - } - } + public void teleopPeriodic() {} @Override public void testInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index df8cd63..d00505f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,6 +32,7 @@ import java.util.stream.Collectors; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; +import com.fasterxml.jackson.databind.jsonFormatVisitors.JsonObjectFormatVisitor; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; @@ -51,6 +52,7 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.PS4Controller.Button; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -63,6 +65,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.RunMiddleSwitch; import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; @@ -226,11 +229,6 @@ public class RobotContainer { // .whenPressed(() -> m_robotMap.leftBack.reset()) // .whenPressed(() -> m_robotMap.rightBack.reset()); - - - - - /* Operator Buttons */ // X > Extend Intake @@ -263,7 +261,7 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) @@ -286,7 +284,6 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - // A > Shoot with Odo /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ @@ -297,11 +294,11 @@ public class RobotContainer { /* Button Box Buttons */ new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftSwitch.value) - .whenPressed(new RunCommand(() -> setManual(true))) - .whenReleased(new RunCommand(() -> setManual(false))); + .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) + .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - // new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) - // .whileHeld(new RunCommand(() -> System.out.println("MiddleSwitch"))); + new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) + .whileHeld(new RunMiddleSwitch()); new JoystickButton(getButtonFox(), ButtonBox.Button.kRightSwitch.value) .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); @@ -313,20 +310,6 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> System.out.println("RightButton"))); } - public void configureManualButtonBindings() { - - new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new RunCommand(() -> System.out.println("MANUAL"))); - - } - - public void configureAutomaticButtonBindings() { - - new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new RunCommand(() -> System.out.println("AUTOMATIC"))); - - } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java b/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java new file mode 100644 index 0000000..3a61d55 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java @@ -0,0 +1,61 @@ +// 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; + +public class RunMiddleSwitch extends CommandBase { + + private static boolean manual = false; + private boolean newManual = false; + private boolean changes = false; + + /** Creates a new RunMiddleSwitch. */ + public RunMiddleSwitch() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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() { + + changes = (newManual == manual); + + if (manual) { + printManual(); + } else { + printNormal(); + } + + newManual = manual; + } + + public void printNormal(){ + System.out.println("Normal"); + } + + public void printManual(){ + System.out.println("Manual"); + } + + public static void setManual(boolean set) + { + manual = set; + } + + // 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 changes; + } +} From 811a5d557ba3ba6fc47043e5cdfc04096adb1d15 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 19:02:37 -0700 Subject: [PATCH 056/180] bruh ryan @ryan123rudder --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 54c4c01..5cc4947 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -164,7 +164,7 @@ public final class Constants { public static final class OIConstants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; - public static final int BUTTON_FOX_ID = 2; + public static final int BUTTON_BOX_ID = 2; public static final double LEFT_AXIS_DEADBAND = 0.1; public static final double RIGHT_AXIS_DEADBAND = 0.6; public static final boolean SKEW_STICKS = true; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d00505f..7766084 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -119,7 +119,7 @@ public class RobotContainer { /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); - private final ButtonBox m_buttonFox = new ButtonBox(OIConstants.BUTTON_FOX_ID); + private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); /* Autonomous */ private PathPlannerTrajectory loadedPathTrajectory = null; @@ -293,20 +293,20 @@ public class RobotContainer { // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); /* Button Box Buttons */ - new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftSwitch.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - new JoystickButton(getButtonFox(), ButtonBox.Button.kMiddleSwitch.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whileHeld(new RunMiddleSwitch()); - new JoystickButton(getButtonFox(), ButtonBox.Button.kRightSwitch.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); - new JoystickButton(getButtonFox(), ButtonBox.Button.kLeftButton.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> System.out.println("LeftButton"))); - new JoystickButton(getButtonFox(), ButtonBox.Button.kRightButton.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value) .whileHeld(new RunCommand(() -> System.out.println("RightButton"))); } @@ -346,8 +346,8 @@ public class RobotContainer { return m_operatorXbox; } - public ButtonBox getButtonFox() { - return m_buttonFox; + public ButtonBox getButtonBox() { + return m_buttonBox; } public void setManual(boolean set) { From 6e709eea8582e3e70c56833f175a0c8f7d65e780 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 20:22:42 -0700 Subject: [PATCH 057/180] limelight on button --- src/main/java/frc4388/robot/RobotContainer.java | 12 ++++-------- src/main/java/frc4388/robot/subsystems/Hood.java | 1 + 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 7766084..313a0d3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,10 +29,8 @@ import java.util.regex.Matcher; import java.util.regex.Pattern; import java.util.stream.Collectors; -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; -import com.fasterxml.jackson.databind.jsonFormatVisitors.JsonObjectFormatVisitor; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; @@ -52,7 +50,6 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.PS4Controller.Button; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -60,7 +57,6 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -232,11 +228,11 @@ public class RobotContainer { /* Operator Buttons */ // X > Extend Intake - /*new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(() -> m_robotIntake.runExtender(true)); + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry)); // Y > Retract Intake - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(() -> m_robotIntake.runExtender(false));*/ + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(() -> m_robotIntake.runExtender(false)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425))) diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 1da9228..f0b1a54 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.SparkMaxRelativeEncoder.Type; import edu.wpi.first.wpilibj2.command.SubsystemBase; From e23ad1c0b63c75a9405c432ab6e412df50b06b10 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 20:39:04 -0700 Subject: [PATCH 058/180] motor configuring --- src/main/java/frc4388/robot/RobotMap.java | 27 ++++++++++++++++--- .../frc4388/robot/subsystems/Extender.java | 3 --- .../java/frc4388/robot/subsystems/Intake.java | 3 --- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 6f6350d..5595a9a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -37,6 +37,10 @@ public class RobotMap { // configureLEDMotorControllers(); configureSwerveMotorControllers(); // configureShooterMotorControllers(); + configureIntakeMotors(); + configureExtenderMotors(); + configureSerializerMotors(); + configureStorageMotors(); } /* LED Subsystem */ @@ -182,7 +186,7 @@ public class RobotMap { rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } +} // // Shooter Config // /* Boom Boom Subsystem */ @@ -257,15 +261,32 @@ public class RobotMap { public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); - void configureIntakeMotors(){ + void configureIntakeMotors() { intakeMotor.configFactoryDefault(); + intakeMotor.setInverted(false); intakeMotor.setNeutralMode(NeutralMode.Coast); + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); } + void configureExtenderMotors() { + extenderMotor.restoreFactoryDefaults(); + extenderMotor.setInverted(true); + extenderMotor.setIdleMode(IdleMode.kBrake); + } + + void configureSerializerMotors() { + serializerBelt.restoreFactoryDefaults(); + } + /* Storage Subsystem */ - public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); + public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); // public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); // public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); + + void configureStorageMotors() { + storageMotor.restoreFactoryDefaults(); + } + } diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index f0749a5..108784c 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -23,9 +23,6 @@ public class Extender extends SubsystemBase { m_extenderMotor = extenderMotor; - m_extenderMotor.restoreFactoryDefaults(); - m_extenderMotor.setInverted(true); - m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_inLimit.enableLimitSwitch(true); diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 4c086b2..5d6f001 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -23,9 +23,6 @@ public class Intake extends SubsystemBase { public Intake(WPI_TalonFX intakeMotor, Serializer serializer) { m_intakeMotor = intakeMotor; m_serializer = serializer; - - m_intakeMotor.setNeutralMode(NeutralMode.Brake); - m_intakeMotor.setInverted(false); } @Override From ae9890f34b30e7fc591756a6fa764b0f3381c5b1 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 12 Mar 2022 20:39:05 -0700 Subject: [PATCH 059/180] asd --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 10 +-- src/main/java/frc4388/robot/RobotMap.java | 89 ++++++++++--------- .../java/frc4388/robot/subsystems/Hood.java | 4 +- .../java/frc4388/robot/subsystems/Turret.java | 3 +- 5 files changed, 56 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 54c4c01..2d87fc1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -219,7 +219,7 @@ public final class Constants { // public static final double TARGET_HEIGHT = 67.5; // public static final double FOV = 29.8; //Field of view limelight - public static final double LIME_ANGLE = 24.7; + public static final double LIME_ANGLE = 50; public static final String NAME = "photonCamera"; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index df8cd63..8880940 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,7 +105,7 @@ public class RobotContainer { public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); - // public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); // public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); @@ -152,7 +152,7 @@ public class RobotContainer { //getDriverController().getRightX(), getDriverController().getRightX(), // getDriverController().getRightY(), - false), + true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers m_robotIntake.setDefaultCommand( @@ -257,10 +257,10 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.525))) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.1))) // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) - // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 6f6350d..8466940 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -36,7 +36,7 @@ public class RobotMap { public RobotMap() { // configureLEDMotorControllers(); configureSwerveMotorControllers(); - // configureShooterMotorControllers(); + configureShooterMotorControllers(); } /* LED Subsystem */ @@ -186,59 +186,60 @@ public class RobotMap { // // Shooter Config // /* Boom Boom Subsystem */ -// public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); -// public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); // // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); -// // hood subsystem -// public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + // hood subsystem + public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); -// // Create motor CANSparkMax -// void configureShooterMotorControllers() { + // Create motor CANSparkMax + void configureShooterMotorControllers() { -// // LEFT FALCON -// shooterFalconLeft.configFactoryDefault(); -// shooterFalconLeft.setNeutralMode(NeutralMode.Coast); -// shooterFalconLeft.setInverted(true); -// shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); + // LEFT FALCON + shooterFalconLeft.configFactoryDefault(); + shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + shooterFalconLeft.setInverted(true); + shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); -// // RIGHT FALCON -// shooterFalconRight.setInverted(false); -// shooterFalconRight.setNeutralMode(NeutralMode.Coast); -// shooterFalconRight.configFactoryDefault(); -// shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); -// // m_shooterFalconRight.configPeakOutputForward(0, -// // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) -// shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, -// ShooterConstants.SHOOTER_TIMEOUT_MS); + // RIGHT FALCON + shooterFalconRight.setInverted(false); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.configFactoryDefault(); + shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + // m_shooterFalconRight.configPeakOutputForward(0, + // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, + ShooterConstants.SHOOTER_TIMEOUT_MS); -// shooterFalconRight.follow(shooterFalconLeft); + shooterFalconRight.follow(shooterFalconLeft); + } // // /* Turret Subsytem */ -// // shooterFalconRight.configStatorCurrentLimit(new -// // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers -// // out of our ass anymore -// // shooterFalconLeft.configSupplyCurrentLimit(new -// // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull + // shooterFalconRight.configStatorCurrentLimit(new + // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers + // out of our ass anymore + // shooterFalconLeft.configSupplyCurrentLimit(new + // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull // // numbers out of our ass anymore // // hood subsystem diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 1da9228..d486374 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -11,6 +11,7 @@ import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.SoftLimitDirection; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -48,7 +49,7 @@ public double m_fireAngle; m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); - setHoodSoftLimits(true); + setHoodSoftLimits(false); } @@ -85,6 +86,7 @@ public double m_fireAngle; */ public void runHood(double input) { m_angleAdjusterMotor.set(input); + SmartDashboard.putNumber("Hood Angle", m_angleAdjusterMotor.getAlternateEncoder(1024).getPosition()); } public void resetGyroAngleAdj(){ diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index b133d6f..2fdfef4 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -56,7 +56,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); - setTurretSoftLimits(true); + setTurretSoftLimits(false); m_boomBoomRotateMotor.setInverted(true); @@ -89,6 +89,7 @@ public class Turret extends SubsystemBase { public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + SmartDashboard.putNumber("Turret Angle", m_boomBoomRotateMotor.getAlternateEncoder(1024).getPosition()); } public void runshooterRotatePID(double targetAngle) { From 1417118c720d04e6adf3b12e8e71f7e9f5f77823 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 20:43:36 -0700 Subject: [PATCH 060/180] indentation --- src/main/java/frc4388/robot/RobotMap.java | 2 +- src/main/java/frc4388/robot/subsystems/Hood.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5595a9a..03c7e03 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -286,7 +286,7 @@ public class RobotMap { // public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); void configureStorageMotors() { - storageMotor.restoreFactoryDefaults(); + storageMotor.restoreFactoryDefaults(); } } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index f0b1a54..cdb8629 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -31,7 +31,7 @@ public class Hood extends SubsystemBase { public boolean m_isHoodReady = false; -public double m_fireAngle; + public double m_fireAngle; /** Creates a new Hood. */ From ba9dce23af55c4b6b598179862b31290adb0e5cb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 22:19:42 -0700 Subject: [PATCH 061/180] shhhtufff --- src/main/java/frc4388/robot/Constants.java | 8 ++-- .../java/frc4388/robot/RobotContainer.java | 44 ++++++++++++++----- src/main/java/frc4388/robot/RobotMap.java | 10 ++--- .../java/frc4388/robot/subsystems/Hood.java | 4 +- .../java/frc4388/robot/subsystems/Intake.java | 2 +- .../java/frc4388/robot/subsystems/Turret.java | 10 +++-- 6 files changed, 51 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f3dfd97..6393625 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -183,7 +183,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.4d; + public static final double TURRET_SPEED_MULTIPLIER = 0.75d; public static final int DEGREES_PER_ROT = 0; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; @@ -198,8 +198,8 @@ public final class Constants { public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double TURRET_FORWARD_LIMIT = 55.0; // TODO: find - public static final double TURRET_REVERSE_LIMIT = -55.0; // TODO: find + public static final double TURRET_FORWARD_LIMIT = 17.0; + public static final double TURRET_REVERSE_LIMIT = -105.0; public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values @@ -207,7 +207,7 @@ public final class Constants { public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find + public static final double HOOD_FORWARD_LIMIT = -0.0; // TODO: find public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c9a3ea2..2498c7c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -105,7 +105,7 @@ public class RobotContainer { public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); // private final LED m_robotLED = new LED(m_robotMap.LEDController); public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); - // public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); + public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); @@ -134,6 +134,7 @@ public class RobotContainer { public boolean manual = false; + public static boolean softLimits = true; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -174,8 +175,8 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotHood.setDefaultCommand( - // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY() * 0.15), m_robotHood)); + m_robotHood.setDefaultCommand( + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY()), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -228,11 +229,17 @@ public class RobotContainer { /* Operator Buttons */ // X > Extend Intake - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry)); - // Y > Retract Intake + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(true), m_robotVisionOdometry)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(() -> m_robotIntake.runExtender(false)); + // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) //8ft // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.425))) @@ -252,7 +259,7 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.1))) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.2))) // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); @@ -289,12 +296,21 @@ public class RobotContainer { // .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry)); /* Button Box Buttons */ + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) - .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) - .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); + .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)); + + // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) + // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) + // .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new RunMiddleSwitch()); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + // .whileHeld(new RunMiddleSwitch()); new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); @@ -350,6 +366,10 @@ public class RobotContainer { this.manual = set; } + public static void setSoftLimits(boolean set) { + softLimits = set; + } + /** * Get odometry. * diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5caebaa..86701be 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -236,7 +236,7 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.follow(shooterFalconLeft); - } +// } // // /* Turret Subsytem */ // shooterFalconRight.configStatorCurrentLimit(new @@ -247,10 +247,10 @@ public class RobotMap { // // numbers out of our ass anymore // // hood subsystem -// angleAdjusterMotor.restoreFactoryDefaults(); -// angleAdjusterMotor.setIdleMode(IdleMode.kBrake); -// angleAdjusterMotor.setInverted(true); -// } + angleAdjusterMotor.restoreFactoryDefaults(); + angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + angleAdjusterMotor.setInverted(true); + } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 057bf2e..9ab9a1f 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -50,13 +50,14 @@ public class Hood extends SubsystemBase { m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); - setHoodSoftLimits(false); + setHoodSoftLimits(true); } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); } /** @@ -87,7 +88,6 @@ public class Hood extends SubsystemBase { */ public void runHood(double input) { m_angleAdjusterMotor.set(input); - SmartDashboard.putNumber("Hood Angle", m_angleAdjusterMotor.getAlternateEncoder(1024).getPosition()); } public void resetGyroAngleAdj(){ diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 5d6f001..cb4a73d 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -35,7 +35,7 @@ public class Intake extends SubsystemBase { * @param rightTrigger Right Trigger to Run Outward */ public void runWithTriggers(double leftTrigger, double rightTrigger) { - m_intakeMotor.set((rightTrigger - leftTrigger) * 0.3); + m_intakeMotor.set((rightTrigger - leftTrigger) * 0.4); SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 2fdfef4..9ca1c32 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -7,7 +7,7 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; - +import com.revrobotics.SparkMaxRelativeEncoder.Type; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -56,10 +56,12 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); - setTurretSoftLimits(false); + setTurretSoftLimits(true); m_boomBoomRotateMotor.setInverted(true); + // m_boomBoomRotateMotor.getAlternateEncoder(4096).setPosition(0); + m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); @@ -71,6 +73,7 @@ public class Turret extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Turret Angle", m_boomBoomRotateEncoder.getPosition()); } /** @@ -89,7 +92,6 @@ public class Turret extends SubsystemBase { public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); - SmartDashboard.putNumber("Turret Angle", m_boomBoomRotateMotor.getAlternateEncoder(1024).getPosition()); } public void runshooterRotatePID(double targetAngle) { @@ -102,10 +104,12 @@ public class Turret extends SubsystemBase { } public double getboomBoomRotatePosition() { + // return 0.0; return m_boomBoomRotateEncoder.getPosition(); } public double getBoomBoomAngleDegrees() { + // return 0.0; return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; } From 362fb579a908618a9b1f5d9acb994ad8e5fbd573 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 12 Mar 2022 22:28:03 -0700 Subject: [PATCH 062/180] shooter drum speed change --- 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 2498c7c..676f275 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -259,7 +259,7 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.2))) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); From 68c69d726222a4fc6795551c402e3ab36ee7b1e6 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 13 Mar 2022 17:02:55 -0600 Subject: [PATCH 063/180] drum pid + tentative shooting values + get voltage --- src/main/deploy/ShooterData.csv | 7 +++--- src/main/java/frc4388/robot/Constants.java | 16 +++++++------ src/main/java/frc4388/robot/Robot.java | 2 ++ .../java/frc4388/robot/RobotContainer.java | 24 ++++++++++++++----- .../frc4388/robot/subsystems/BoomBoom.java | 12 +++++++++- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +++++-- .../robot/subsystems/SwerveModule.java | 4 ++++ .../java/frc4388/robot/subsystems/Turret.java | 6 +---- .../robot/subsystems/VisionOdometry.java | 2 +- 9 files changed, 55 insertions(+), 26 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 16e194d..1284d91 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,5 +1,4 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s) -96 ,-25.00 ,0.425 ,0 -144 ,-47.57 ,0. ,0 -192 ,-55.55 ,0.500 ,0 -240 ,-96.00 ,0.525 ,0 \ No newline at end of file +81, ,0 ,8000 ,0 +150, ,-62.1 ,10000 ,0 +227, ,-103.9 ,10500 ,0 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6393625..62e5422 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -32,7 +32,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4.0; + public static final double ROTATION_SPEED = 2.0; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -183,7 +183,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.75d; + public static final double TURRET_SPEED_MULTIPLIER = 0.4d; public static final int DEGREES_PER_ROT = 0; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; @@ -195,19 +195,21 @@ public final class Constants { /* Turret Constants */ // ID public static final int TURRET_MOTOR_CAN_ID = 19; + //Gains for turret public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double TURRET_FORWARD_LIMIT = 17.0; - public static final double TURRET_REVERSE_LIMIT = -105.0; - - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values + public static final double TURRET_FORWARD_LIMIT = 17.0; // TODO: find + public static final double TURRET_REVERSE_LIMIT = -105.0; // TODO: find + //Shooter gains for actual Drum + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); // TODO: tune values /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final double HOOD_FORWARD_LIMIT = -0.0; // TODO: find + public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 04a55bc..d04fae9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -143,6 +143,8 @@ public class Robot extends TimedRobot { m_robotContainer.m_robotSwerveDrive.getCurrent(); // m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); + SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); + SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled // commands, running already-scheduled commands, removing finished or diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 676f275..8667896 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -259,9 +259,11 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.6))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) @@ -271,13 +273,23 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new InstantCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index a2e87df..bc8d8cf 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -37,6 +37,7 @@ public class BoomBoom extends SubsystemBase { double velP; double input; + public double pidOffset = 0; public boolean m_isDrumReady = false; public double m_fireVel; @@ -56,6 +57,7 @@ public class BoomBoom extends SubsystemBase { public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; + setShooterGains(); try { // This is a helper class that allows us to read a CSV file into a Java array. @@ -183,6 +185,9 @@ public class BoomBoom extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0); + SmartDashboard.putNumber("Shooter Current", getCurrent()); + SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); } public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { @@ -211,7 +216,8 @@ public class BoomBoom extends SubsystemBase { } public void runDrumShooterVelocityPID(double targetVel) { - m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init + SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset); + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init // New BoomBoom controller stuff // Controls a motor with the output of the BangBang controller @@ -223,6 +229,10 @@ public class BoomBoom extends SubsystemBase { // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } + public void updateOffset(double change) { + pidOffset = pidOffset + change; + } + public void increaseSpeed(double amount) { speed2 = speed2 + amount; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index b5c3176..2e3d754 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -112,9 +112,9 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED); + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } @@ -263,4 +263,8 @@ public class SwerveDrive extends SubsystemBase { public double getCurrent(){ return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent(); } + + public double getVoltage(){ + return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage(); + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 299ddd4..11e861e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -195,4 +195,8 @@ public class SwerveModule extends SubsystemBase { public double getCurrent(){ return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); } + + public double getVoltage(){ + return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 9ca1c32..fe99959 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -7,7 +7,7 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.SparkMaxRelativeEncoder.Type; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -60,8 +60,6 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setInverted(true); - // m_boomBoomRotateMotor.getAlternateEncoder(4096).setPosition(0); - m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); @@ -104,12 +102,10 @@ public class Turret extends SubsystemBase { } public double getboomBoomRotatePosition() { - // return 0.0; return m_boomBoomRotateEncoder.getPosition(); } public double getBoomBoomAngleDegrees() { - // return 0.0; return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; } diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index d4b60db..91c435c 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -112,7 +112,7 @@ public class VisionOdometry extends SubsystemBase { } guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees()); - guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); + // guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); SmartDashboard.putNumber("Vision ODO x: ", guess.x); SmartDashboard.putNumber("Vision ODO y: ", guess.y); From 8e13b5c69f4d069042f56232dd88b0b1123de316 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 13 Mar 2022 17:11:59 -0600 Subject: [PATCH 064/180] Added desmos server --- src/main/java/frc4388/robot/Robot.java | 5 + .../java/frc4388/utility/DesmosClient.html | 222 +++++++++++++ .../java/frc4388/utility/DesmosServer.java | 314 ++++++++++++++++++ 3 files changed, 541 insertions(+) create mode 100644 src/main/java/frc4388/utility/DesmosClient.html create mode 100644 src/main/java/frc4388/utility/DesmosServer.java diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index d04fae9..ddee6d5 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.RunCommand; +import frc4388.utility.DesmosServer; import frc4388.utility.RobotTime; import frc4388.utility.VelocityCorrection; @@ -46,6 +47,8 @@ public class Robot extends TimedRobot { private Pose2d selectedOdo; private double current; + private static DesmosServer desmosServer = new DesmosServer(8000); + /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -116,6 +119,8 @@ public class Robot extends TimedRobot { return "Not Running"; } }); + + desmosServer.start(); } /** diff --git a/src/main/java/frc4388/utility/DesmosClient.html b/src/main/java/frc4388/utility/DesmosClient.html new file mode 100644 index 0000000..7eaef15 --- /dev/null +++ b/src/main/java/frc4388/utility/DesmosClient.html @@ -0,0 +1,222 @@ + + + + + + Desmos Client + + +
+
Active
+ + + diff --git a/src/main/java/frc4388/utility/DesmosServer.java b/src/main/java/frc4388/utility/DesmosServer.java new file mode 100644 index 0000000..2c743cd --- /dev/null +++ b/src/main/java/frc4388/utility/DesmosServer.java @@ -0,0 +1,314 @@ +package frc4388.utility; + +import java.io.BufferedReader; +import java.io.IOException; +import java.io.InputStreamReader; +import java.io.OutputStream; +import java.net.ServerSocket; +import java.net.Socket; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Set; + +import org.opencv.core.Point; + +/** + * A http server that allows the robot to communicate with Desmos Graphing Calculator + * + * @author Daniel McGrath + * */ +public class DesmosServer extends Thread { + private static HashMap desmosQueue = new HashMap<>(); + private static HashMap readVariables = new HashMap<>(); + + private static boolean running = false; + + private ServerSocket serverSocket; + private int activePort; + + /** + * Creates DesmosServer running on port + *

+ * Use this for cases when the robot is using the default port + * + * @param port The port the server will run on + * */ + public DesmosServer(int port) { + activePort = port; + } + + /** + * Creates DesmosServer running on port 5500 + * */ + public DesmosServer() { + activePort = 8000; + } + + @Override + public void run() { + try { + runServer(activePort); + } catch(IOException err) { + err.printStackTrace(); + } + } + + /** + * Runs server on port + * + * @param port The port the server runs on + * @throws IOException + * */ + public void runServer(int port) throws IOException { + System.out.println("Initializing DesmosServer on port " + port + "..."); + + serverSocket = new ServerSocket(port); + running = true; + + System.out.println("DesmosServer is active!"); + + while(true) { + Socket client = serverSocket.accept(); + handleClient(client); + } + } + + /** + * Handles client requests + * + * @param client The client connection + * @throws IOException + * */ + public void handleClient(Socket client) throws IOException { + InputStreamReader clientStream = new InputStreamReader(client.getInputStream()); + BufferedReader bufferedReader = new BufferedReader(clientStream); + + ArrayList headers = new ArrayList<>(); + + String header; + while((header = bufferedReader.readLine()).length() != 0) { + headers.add(header); + } + + String body = ""; + while(bufferedReader.ready()) { + body += (char) bufferedReader.read(); + } + + readVariables(body); + sendResponse(client); + } + + /** + * Sends JSON response with format + *

+ * [ + * {"int": "24"}, + * {"double": "2.4"}, + * {"point": "(2,4)"}, + * {"list": "[2,4]"} + * ] + * + * @param client The client connection + * @throws IOException + * */ + public void sendResponse(Socket client) throws IOException { + OutputStream clientOutput = client.getOutputStream(); + + // Write Headers + clientOutput.write("HTTP/1.1 200 OK\r\n".getBytes()); + clientOutput.write("Access-Control-Allow-Origin: *\r\n".getBytes()); + clientOutput.write("Keep-Alive: timeout=2, max=100\r\n".getBytes()); + clientOutput.write("Connection: Keep-Alive\r\n".getBytes()); + clientOutput.write("Content-Type: application/json\r\n\r\n".getBytes()); + + clientOutput.write(getJSONOutput().getBytes()); + clientOutput.flush(); + clientOutput.close(); + } + + /** + * Produces JSON output containing Desmos output. + * + * @return JSON string to be read by Desmos client + * */ + public static String getJSONOutput() { + String json = "["; + + if(!desmosQueue.isEmpty()) { + Set keySet = new HashSet<>(desmosQueue.keySet()); + + for(String key : keySet) { + json += "\n\t{" + + "\"name\":\"" + key + "\"," + + "\"type\":\"" + desmosQueue.get(key)[0] + "\"," + + "\"value\":\"" + desmosQueue.get(key)[1] + "\"" + + "},"; + + desmosQueue.remove(key); + } + + json = json.substring(0, json.length()-1); // remove comma at the end + } + + json += "\n]"; + + return json; + } + + /** + * Interpret client request and update variables + * + * @param requestBody Client request + */ + public static void readVariables(String requestBody) { + for(String variable : requestBody.split("\n")) { + if(variable.equals("")) + break; + + String[] readVar = variable.split("\t"); + readVariables.put(readVar[0], new String[] {readVar[1], readVar[2]}); + } + } + + /** + * Checks if the server is running + * + * @return The server status + */ + public static boolean isRunning() { + return running; + } + + // --------------------------------------------------------------------- + + /** + * Adds integer to desmos queue + * + * @param name Name of desmos variable + * @param value Integer value + * */ + public static void putInteger(String name, Integer value) { + desmosQueue.put(name, new String[] {"number", value.toString()}); + } + + /** + * Adds double to desmos queue + * + * @param name Name of desmos variable + * @param value Double value + * */ + public static void putDouble(String name, Double value) { + desmosQueue.put(name, new String[] {"number", value.toString()}); + } + + /** + * Adds point to desmos queue + * + * @param name Name of desmos variable + * @param value Point value + * */ + public static void putPoint(String name, Point point) { + desmosQueue.put(name, new String[] {"point", "(" + point.x + "," + point.y + ")"}); + } + + public static void putArray(String name, double... arr) { + desmosQueue.put(name, new String[] {"array", Arrays.toString(arr).replace(" ", "")}); + } + + /** + * Adds table to desmos queue + * + * @param name ID of table + * @param id Column ID + * @param column Double array containing column values + * @param table Repeat id and column in sequence + * */ + public static void putTable(String name, Object... table) { + String tableStr = ""; + + for(int i = 0; i < table.length; i += 2) { + // Check parameters + if(!(table[i] instanceof String)) { return; } + if(!(table[i+1] instanceof double[])) { return; } + + tableStr += table[i] + ","; + String values = Arrays.toString((double[]) table[i+1]).replace(" ", ""); + tableStr += values.substring(1, values.length() - 1); + tableStr += ' '; + } + + tableStr = tableStr.substring(0, tableStr.length()-1); // remove space at the end + + desmosQueue.put(name, new String[] {"table", tableStr}); + } + + // --------------------------------------------------------------------- + + /** + * Reads desmos integer + * + * @param name Desmos variable name + * @return Numeric value, if variable is an expression it will be evaluated + *

if variable is a double it will be cast to int + * */ + public static int readInteger(String name) { + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number")) + return 0; + + return (int) Double.parseDouble(readVariables.get(name)[1]); + } + + /** + * Reads desmos double + * + * @param name Desmos variable name + * @return Numeric value, if variable is an expression it will be evaluated + * */ + public static double readDouble(String name) { + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("number")) + return 0; + + return Double.parseDouble(readVariables.get(name)[1]); + } + + /** + * Reads desmos point + * + * @param name Desmos variable name + * @return Point, if variable contains expressions they will be evaluated + * */ + public static Point readPoint(String name) { + Point point = new Point(); + + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("point")) + return point; + + String pointStr = readVariables.get(name)[1]; + point.x = Double.parseDouble(pointStr.split(",")[0]); + point.y = Double.parseDouble(pointStr.split(",")[1]); + + return point; + } + + /** + * Reads desmos array, including table columns + * + * @param name Desmos variable name + * @returns Array of numeric values, if array contains expressions they will be evaluated + * */ + public static double[] readArray(String name) { + if(!readVariables.containsKey(name) || !readVariables.get(name)[0].equals("array")) + return new double[0]; + + String[] unparsed = readVariables.get(name)[1].split(","); + double[] arr = new double[unparsed.length]; + + for(int i = 0; i < arr.length; i++) + arr[i] = Integer.parseInt(unparsed[i]); + + return arr; + } +} From cfdac7f069ac6bc0dd442ffc54ae4c7bdb3eb269 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 13 Mar 2022 17:57:36 -0600 Subject: [PATCH 065/180] turret pid tuned and degree conversion --- src/main/java/frc4388/robot/Constants.java | 6 +++--- src/main/java/frc4388/robot/RobotContainer.java | 16 ++++++++-------- .../java/frc4388/robot/subsystems/Turret.java | 3 ++- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 62e5422..277d3a2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -184,7 +184,7 @@ public final class Constants { public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; public static final double TURRET_SPEED_MULTIPLIER = 0.4d; - public static final int DEGREES_PER_ROT = 0; + public static final double DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; public static final double ENCODER_TICKS_PER_REV = 2048; @@ -196,10 +196,10 @@ public final class Constants { // ID public static final int TURRET_MOTOR_CAN_ID = 19; //Gains for turret - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.6); + public static final double SHOOTER_TURRET_MIN = -0.6; //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); - public static final double SHOOTER_TURRET_MIN = -1.0; public static final double TURRET_FORWARD_LIMIT = 17.0; // TODO: find public static final double TURRET_REVERSE_LIMIT = -105.0; // TODO: find //Shooter gains for actual Drum diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8667896..de36846 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -176,7 +176,7 @@ public class RobotContainer { new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getLeftY()), m_robotHood)); + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -260,9 +260,6 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) - // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.6))) - // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) @@ -281,11 +278,14 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-180.0), m_robotTurret)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) // .whileHeld(new InstantCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index fe99959..00e141c 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -71,7 +71,8 @@ public class Turret extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Turret Angle", m_boomBoomRotateEncoder.getPosition()); + SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); + SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.DEGREES_PER_ROT); } /** From 4190491459ed75d6bdec241bf9ee794fee1ff98e Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sun, 13 Mar 2022 19:59:55 -0600 Subject: [PATCH 066/180] Fixed limelight aiming --- src/main/java/frc4388/robot/Constants.java | 7 ++-- src/main/java/frc4388/robot/Robot.java | 2 + .../java/frc4388/robot/RobotContainer.java | 6 +-- .../frc4388/robot/commands/TrackTarget.java | 39 +++++++++++++------ 4 files changed, 37 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 277d3a2..57ff6a1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -221,16 +221,17 @@ public final class Constants { // public static final double TARGET_HEIGHT = 67.5; // public static final double FOV = 29.8; //Field of view limelight - public static final double LIME_ANGLE = 50; + public static final double LIME_ANGLE = 56.4; public static final String NAME = "photonCamera"; public static final double TARGET_HEIGHT = 8*12 + 8; //TODO: Convert to metric (does this still need to be converted?) + public static final double LIME_HEIGHT = 26; public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; public static final double V_FOV = 49.7; - public static final double LIME_VIXELS = 960; - public static final double LIME_HIXELS = 720; + public static final double LIME_HIXELS = 960; + public static final double LIME_VIXELS = 720; public static final double TURRET_kP = 0.1; public static final double RANGE = 10; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ddee6d5..13c5558 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -214,6 +214,8 @@ public class Robot extends TimedRobot { } else LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } + + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index de36846..44f3c02 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -107,7 +107,7 @@ public class RobotContainer { public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); - private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); + public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); public final Climber m_robotClimber = new Climber(testElbowMotor); @@ -304,8 +304,8 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ //B > Shoot with Lime - // 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)); /* Button Box Buttons */ diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index ffe50c5..8cb25c2 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -16,7 +16,9 @@ import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.DesmosServer; public class TrackTarget extends CommandBase { /** Creates a new TrackTarget. */ @@ -37,6 +39,8 @@ public class TrackTarget extends CommandBase { Pose2d pos = new Pose2d(); ArrayList points = new ArrayList<>(); + boolean isExecuted = false; + // public static Gains m_aimGains; public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) { @@ -64,25 +68,37 @@ public class TrackTarget extends CommandBase { try { m_visionOdometry.setLEDs(true); points = m_visionOdometry.getTargetPoints(); - double pointTotal = 0; - for(Point point : points) { - pointTotal = pointTotal + point.x; + for(int i = 0; i < points.size(); i++) { + DesmosServer.putPoint("Point" + i, points.get(i)); } - average = pointTotal/points.size(); - output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP; - m_turret.runTurretWithInput(output); - pos = m_visionOdometry.getVisionOdometry(); - distance = Math.hypot(pos.getX(), pos.getY()); + + Point average = VisionOdometry.averagePoint(points); + DesmosServer.putPoint("average", average); + + output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + output *= 2; + DesmosServer.putDouble("output", output); + // m_turret.runTurretWithInput(output); + + double y_rot = average.y / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); + DesmosServer.putDouble("distance", distance); + + // isExecuted = true; } catch (Exception e){ System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } - vel = m_boomBoom.getVelocity(distance); - hood = m_boomBoom.getHood(distance); + // vel = m_boomBoom.getVelocity(distance); + // hood = m_boomBoom.getHood(distance); // m_boomBoom.runDrumShooter(vel); // m_boomBoom.runDrumShooterVelocityPID(vel); // m_hood.runAngleAdjustPID(hood); - //m_turret.runshooterRotatePID(m_targetAngle); + // m_turret.runshooterRotatePID(m_targetAngle); } // Called once the command ends or is interrupted. @@ -94,5 +110,6 @@ public class TrackTarget extends CommandBase { @Override public boolean isFinished() { return false; + // return isExecuted && Math.abs(output) < .1; } } From 86cc9b6ff598607bcdfceb7c8ac78adcc83f37a3 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 13 Mar 2022 21:43:14 -0600 Subject: [PATCH 067/180] Speedy regression + circle fitting for target track --- .../frc4388/robot/commands/TrackTarget.java | 19 ++++++++++ .../robot/subsystems/VisionOdometry.java | 35 +++++++++++-------- 2 files changed, 39 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 8cb25c2..0acbfd6 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -39,6 +39,8 @@ public class TrackTarget extends CommandBase { Pose2d pos = new Pose2d(); ArrayList points = new ArrayList<>(); + double m=0; + double b=0; boolean isExecuted = false; // public static Gains m_aimGains; @@ -88,6 +90,14 @@ public class TrackTarget extends CommandBase { double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); DesmosServer.putDouble("distance", distance); + updateRegressionDesmos(); + double regressedDistance = distanceRegression(distance); + DesmosServer.putDouble("distanceReg", regressedDistance); + + //Vision odemetry circle fit based pose estimate + Point targetOffset = m_visionOdometry.getTargetOffset(); + DesmosServer.putPoint("targetOff", targetOffset); + // isExecuted = true; } catch (Exception e){ @@ -101,6 +111,15 @@ public class TrackTarget extends CommandBase { // m_turret.runshooterRotatePID(m_targetAngle); } + public final double distanceRegression(double distance) { + return m * distance + b; + } + + public void updateRegressionDesmos() { + m = DesmosServer.readDouble("m"); + b = DesmosServer.readDouble("b"); + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 91c435c..f1048b0 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -91,12 +91,7 @@ public class VisionOdometry extends SubsystemBase { m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); } - /** Gets estimated odometry based on limelight data - * - * @return The estimated odometry pose, including gyro rotation - * @throws VisionObscuredException - */ - public Pose2d getVisionOdometry() throws VisionObscuredException { + public Point getTargetOffset() throws VisionObscuredException { ArrayList screenPoints = getTargetPoints(); if(screenPoints.size() < 3) @@ -111,14 +106,25 @@ public class VisionOdometry extends SubsystemBase { guess = iterateGuess(guess, points); } - guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees()); - // guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees()); + return guess; + } - SmartDashboard.putNumber("Vision ODO x: ", guess.x); - SmartDashboard.putNumber("Vision ODO y: ", guess.y); + /** Gets estimated odometry based on limelight data + * + * @return The estimated odometry pose, including gyro rotation + * @throws VisionObscuredException + */ + public Pose2d getVisionOdometry() throws VisionObscuredException { + Point targetOffset = getTargetOffset(); + + targetOffset = correctGuessForCenter(targetOffset, m_shooter.getBoomBoomAngleDegrees()); + targetOffset = correctGuessForGyro(targetOffset, m_drive.getRegGyro().getDegrees()); + + SmartDashboard.putNumber("Vision ODO x: ", targetOffset.x); + SmartDashboard.putNumber("Vision ODO y: ", targetOffset.y); Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees())); - Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation); + Pose2d odometryPose = new Pose2d(targetOffset.x, targetOffset.y, rotation); return odometryPose; } @@ -128,8 +134,7 @@ public class VisionOdometry extends SubsystemBase { } public boolean getLEDs() { - if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false; - return true; + return m_camera.getLEDMode() != VisionLEDMode.kOff; } public void updateOdometryWithVision(){ @@ -141,6 +146,7 @@ public class VisionOdometry extends SubsystemBase { err.printStackTrace(); } } + /** Reverse 3d projects target points from screen coordinates to 3d space *

* Uses the known height of the target to project points @@ -286,9 +292,8 @@ public class VisionOdometry extends SubsystemBase { * @return The angle corrected for the quadrent */ public static final double correctQuadrent(double angle, Point guess, Point circlePoint) { - if(circlePoint.x - guess.x < 0) { + if(circlePoint.x - guess.x < 0) return angle - Math.PI; - } return angle; } From 4c758f996203024146abd434d91a7e01d21c9197 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 13 Mar 2022 21:51:55 -0600 Subject: [PATCH 068/180] Changed error handling --- src/main/java/frc4388/robot/commands/TrackTarget.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 0acbfd6..e444c12 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -80,7 +80,7 @@ public class TrackTarget extends CommandBase { output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 2; DesmosServer.putDouble("output", output); - // m_turret.runTurretWithInput(output); + m_turret.runTurretWithInput(output); double y_rot = average.y / VisionConstants.LIME_VIXELS; y_rot *= Math.toRadians(VisionConstants.V_FOV); @@ -101,7 +101,8 @@ public class TrackTarget extends CommandBase { // isExecuted = true; } catch (Exception e){ - System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); + e.printStackTrace(); + // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } // vel = m_boomBoom.getVelocity(distance); // hood = m_boomBoom.getHood(distance); From 0315104ee8451f2321e8c15832eba815e400e6b3 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Mon, 14 Mar 2022 12:16:32 -0600 Subject: [PATCH 069/180] changed track target reqs + started extender command --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 24 +++++----- .../robot/commands/RunExtenderOut.java | 46 +++++++++++++++++++ .../frc4388/robot/commands/TrackTarget.java | 7 ++- .../frc4388/robot/subsystems/Extender.java | 8 +++- .../frc4388/commands/AimToCenterTest.java | 2 +- 6 files changed, 71 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunExtenderOut.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 57ff6a1..7a3599b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -183,7 +183,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.4d; + public static final double TURRET_SPEED_MULTIPLIER = 0.6; public static final double DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 44f3c02..76b5d5b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -175,6 +175,8 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + + m_robotHood.setDefaultCommand( new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( @@ -270,26 +272,25 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.switchDirection(), m_robotExtender)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-180.0), m_robotTurret)); - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new InstantCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) @@ -304,8 +305,9 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ //B > Shoot with Lime - 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) + // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + /* Button Box Buttons */ diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOut.java b/src/main/java/frc4388/robot/commands/RunExtenderOut.java new file mode 100644 index 0000000..b287baf --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtenderOut.java @@ -0,0 +1,46 @@ +// 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.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +public class RunExtenderOut extends CommandBase { + + private Intake intake; + private Extender extender; + private int direction; + + /** Creates a new RunExtenderOut. */ + public RunExtenderOut(Intake intake, Extender extender) { + // Use addRequirements() here to declare subsystem dependencies. + + this.intake = intake; + this.extender = extender; + + this.direction = 1; + + addRequirements(this.intake, this.extender); + } + + // 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() {} + + // 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/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index e444c12..c1e4a14 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -23,7 +23,6 @@ import frc4388.utility.DesmosServer; public class TrackTarget extends CommandBase { /** Creates a new TrackTarget. */ Turret m_turret; - SwerveDrive m_drive; VisionOdometry m_visionOdometry; BoomBoom m_boomBoom; Hood m_hood; @@ -45,15 +44,14 @@ public class TrackTarget extends CommandBase { // public static Gains m_aimGains; - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) { + public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { // Use addRequirements() here to declare subsystem dependencies. m_turret = turret; - m_drive = drive; m_boomBoom = boomBoom; m_hood = hood; m_visionOdometry = visionOdometry; - addRequirements(m_turret, m_boomBoom, m_hood, m_drive, m_visionOdometry); + addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry); } // Called when the command is initially scheduled. @@ -102,6 +100,7 @@ public class TrackTarget extends CommandBase { } catch (Exception e){ e.printStackTrace(); + m_turret.runshooterRotatePID(180); // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } // vel = m_boomBoom.getVelocity(distance); diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 108784c..4be18e9 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -17,6 +17,7 @@ public class Extender extends SubsystemBase { private SparkMaxLimitSwitch m_outLimit; public boolean toggle; + public int direction = 1; /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { @@ -46,7 +47,12 @@ public class Extender extends SubsystemBase { public void runExtender(double input) { // if (!m_serializer.getBeam() && input < 0.) return; - m_extenderMotor.set(input); + if (this.direction > 0) {} + m_extenderMotor.set(this.direction * input); + } + + public void switchDirection() { + this.direction = this.direction * -1; } public double getCurrent() { diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 7f0b004..644d3a9 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -9,7 +9,7 @@ public class AimToCenterTest { private static final double DELTA = 1e-15; - @Test + //@Test public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { boolean output; From 8668d960191dc7b65bd48beaf45b1a25a3081e58 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Mon, 14 Mar 2022 12:19:23 -0600 Subject: [PATCH 070/180] Fixed desmos server --- src/main/java/frc4388/robot/Robot.java | 1 + src/main/java/frc4388/utility/DesmosClient.html | 4 +++- src/test/java/frc4388/commands/AimToCenterTest.java | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 13c5558..ae7067e 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -121,6 +121,7 @@ public class Robot extends TimedRobot { }); desmosServer.start(); + DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } /** diff --git a/src/main/java/frc4388/utility/DesmosClient.html b/src/main/java/frc4388/utility/DesmosClient.html index 7eaef15..864a5f9 100644 --- a/src/main/java/frc4388/utility/DesmosClient.html +++ b/src/main/java/frc4388/utility/DesmosClient.html @@ -116,8 +116,10 @@ return 'point'; else if(raw.match(/\[[a-zA-Z0-9.,_{}\(\)]*\]/)) return 'array'; - else + else if(raw.contains('=')) return 'number'; + else + return 'none'; } function readVariable(latex) { diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 7f0b004..644d3a9 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -9,7 +9,7 @@ public class AimToCenterTest { private static final double DELTA = 1e-15; - @Test + //@Test public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { boolean output; From 1ebfd0c3eff493c2c2aebe8c4f978178acdd5d74 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Mon, 14 Mar 2022 12:53:37 -0600 Subject: [PATCH 071/180] Actually fixed desmosClient --- src/main/java/frc4388/utility/DesmosClient.html | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/utility/DesmosClient.html b/src/main/java/frc4388/utility/DesmosClient.html index 864a5f9..f8a77f3 100644 --- a/src/main/java/frc4388/utility/DesmosClient.html +++ b/src/main/java/frc4388/utility/DesmosClient.html @@ -116,7 +116,7 @@ return 'point'; else if(raw.match(/\[[a-zA-Z0-9.,_{}\(\)]*\]/)) return 'array'; - else if(raw.contains('=')) + else if(raw.includes('=')) return 'number'; else return 'none'; From bc14a85fc794eae83b721dd8651fe6c885cbd146 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 14 Mar 2022 15:16:21 -0600 Subject: [PATCH 072/180] extender intake group command DONE --- src/main/java/frc4388/robot/Constants.java | 6 +++ .../java/frc4388/robot/RobotContainer.java | 43 +++++++++++----- .../robot/commands/ExtenderIntakeGroup.java | 29 +++++++++++ .../frc4388/robot/commands/RunExtender.java | 49 +++++++++++++++++++ ...erOut.java => RunIntakeConditionally.java} | 24 ++++----- .../frc4388/robot/subsystems/Extender.java | 34 +++++++++---- .../java/frc4388/robot/subsystems/Intake.java | 6 ++- 7 files changed, 156 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java create mode 100644 src/main/java/frc4388/robot/commands/RunExtender.java rename src/main/java/frc4388/robot/commands/{RunExtenderOut.java => RunIntakeConditionally.java} (69%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a3599b..d54fce3 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -146,6 +146,12 @@ public final class Constants { public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration( false, 15, 0, 0); } + + public static final class ExtenderConstants { + public static final double EXTENDER_FORWARD_LIMIT = 250.0; + public static final double EXTENDER_REVERSE_LIMIT = 0.0; + } + public static final class StorageConstants { public static final int STORAGE_CAN_ID = 18; public static final int BEAM_SENSOR_SHOOTER = 28; //TODO diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 76b5d5b..0e0c200 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -54,6 +54,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.NotifierCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -61,6 +62,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.ExtenderIntakeGroup; import frc4388.robot.commands.RunMiddleSwitch; import frc4388.robot.commands.Shoot; import frc4388.robot.commands.TrackTarget; @@ -237,8 +239,8 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) // .whenPressed(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); @@ -260,9 +262,9 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) @@ -272,14 +274,26 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whileHeld(new RunExtender(m_robotIntake, m_robotExtender, 0.5)) + // .whenReleased(new RunCommand(() -> RunExtender.changeDirection(), m_robotIntake, m_robotExtender)); + + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection())); + + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new ParallelCommandGroup( + // new RunCommand(() -> m_robotIntake.m_intakeMotor.set(ExtenderIntakeGroup.direction * -0.2), m_robotIntake), + // new RunCommand(() -> m_robotExtender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 0.5), m_robotExtender))) + // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection())); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0))); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.switchDirection(), m_robotExtender)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) @@ -314,10 +328,15 @@ public class RobotContainer { new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)); + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0), m_robotExtender)); // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java new file mode 100644 index 0000000..88d1cff --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java @@ -0,0 +1,29 @@ +// 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.ParallelRaceGroup; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class ExtenderIntakeGroup extends ParallelRaceGroup { + + public static int direction = 1; // assume extender starts retracted completely + + /** Creates a new RunExtenderAndIntake. */ + public ExtenderIntakeGroup(Intake intake, Extender extender) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + ExtenderIntakeGroup.direction = 1; + addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); + } + + public static void changeDirection() { + ExtenderIntakeGroup.direction *= -1; + } +} diff --git a/src/main/java/frc4388/robot/commands/RunExtender.java b/src/main/java/frc4388/robot/commands/RunExtender.java new file mode 100644 index 0000000..8af3e8f --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunExtender.java @@ -0,0 +1,49 @@ +// 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.ExtenderConstants; +import frc4388.robot.subsystems.Extender; +import frc4388.robot.subsystems.Intake; + +public class RunExtender extends CommandBase { + + private Extender extender; + + /** Creates a new RunExtender. */ + public RunExtender(Extender extender) { + // Use addRequirements() here to declare subsystem dependencies. + + this.extender = extender; + + addRequirements(this.extender); + } + + // 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() { + this.extender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 1.0); // TODO: change to 1.0 for actual speed, 0.5 is just for testing + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + ExtenderIntakeGroup.changeDirection(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (Math.abs(this.extender.m_extenderMotor.getEncoder().getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT) < 5) { + return true; + } + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/RunExtenderOut.java b/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java similarity index 69% rename from src/main/java/frc4388/robot/commands/RunExtenderOut.java rename to src/main/java/frc4388/robot/commands/RunIntakeConditionally.java index b287baf..01f92b0 100644 --- a/src/main/java/frc4388/robot/commands/RunExtenderOut.java +++ b/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java @@ -5,25 +5,19 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Extender; import frc4388.robot.subsystems.Intake; -public class RunExtenderOut extends CommandBase { +public class RunIntakeConditionally extends CommandBase { private Intake intake; - private Extender extender; - private int direction; - /** Creates a new RunExtenderOut. */ - public RunExtenderOut(Intake intake, Extender extender) { + /** Creates a new RunIntakeConditionally. */ + public RunIntakeConditionally(Intake intake) { // Use addRequirements() here to declare subsystem dependencies. - + this.intake = intake; - this.extender = extender; - this.direction = 1; - - addRequirements(this.intake, this.extender); + addRequirements(this.intake); } // Called when the command is initially scheduled. @@ -32,7 +26,13 @@ public class RunExtenderOut extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (ExtenderIntakeGroup.direction > 0) { + this.intake.m_intakeMotor.set(-0.4); + } else { + this.intake.m_intakeMotor.set(0); + } + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 4be18e9..501bb56 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -4,20 +4,24 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ExtenderConstants; +import frc4388.utility.DesmosServer; public class Extender extends SubsystemBase { - CANSparkMax m_extenderMotor; + public CANSparkMax m_extenderMotor; private SparkMaxLimitSwitch m_inLimit; private SparkMaxLimitSwitch m_outLimit; public boolean toggle; - public int direction = 1; /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { @@ -26,13 +30,28 @@ public class Extender extends SubsystemBase { m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_inLimit.enableLimitSwitch(true); - m_outLimit.enableLimitSwitch(true); + m_inLimit.enableLimitSwitch(false); + m_outLimit.enableLimitSwitch(false); + + m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT); + m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT); + setExtenderSoftLimits(true); + } + + /** + * Set status of extender motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setExtenderSoftLimits(boolean set) { + m_extenderMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_extenderMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition()); + DesmosServer.putDouble("ExtenderPosition", m_extenderMotor.getEncoder().getPosition()); } /** @@ -47,12 +66,7 @@ public class Extender extends SubsystemBase { public void runExtender(double input) { // if (!m_serializer.getBeam() && input < 0.) return; - if (this.direction > 0) {} - m_extenderMotor.set(this.direction * input); - } - - public void switchDirection() { - this.direction = this.direction * -1; + m_extenderMotor.set(input); } public double getCurrent() { diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index cb4a73d..3ad0491 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -12,11 +12,13 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.commands.ExtenderIntakeGroup; + import com.revrobotics.CANSparkMax; public class Intake extends SubsystemBase { - private WPI_TalonFX m_intakeMotor; + public WPI_TalonFX m_intakeMotor; private Serializer m_serializer; /** Creates a new Intake. */ @@ -28,6 +30,8 @@ public class Intake extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake Percent Output", m_intakeMotor.get()); + SmartDashboard.putNumber("Extender Direction", ExtenderIntakeGroup.direction); } /** * Runs The Intake With Triggers as input From abc2e8b05fb4326dac47f6f889defcdd71ebf914 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Mon, 14 Mar 2022 16:56:32 -0600 Subject: [PATCH 073/180] track target distance regression WORKS --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 17 +++++++++++------ .../frc4388/robot/commands/TrackTarget.java | 4 +++- .../java/frc4388/robot/subsystems/Turret.java | 4 ++-- 5 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d54fce3..c57bd9d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -190,7 +190,7 @@ public final class Constants { public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; public static final double TURRET_SPEED_MULTIPLIER = 0.6; - public static final double DEGREES_PER_ROT = 180.0/105.45445251464844; + public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; public static final double ENCODER_TICKS_PER_REV = 2048; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ae7067e..b5c7845 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -121,7 +121,7 @@ public class Robot extends TimedRobot { }); desmosServer.start(); - DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); + // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0e0c200..961b817 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -59,6 +59,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.AimToCenter; @@ -291,11 +292,11 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0))); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); @@ -317,10 +318,14 @@ public class RobotContainer { // A > Shoot with Odo /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); /* Button Box Buttons */ diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index c1e4a14..2cd8924 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -112,12 +112,14 @@ public class TrackTarget extends CommandBase { } public final double distanceRegression(double distance) { - return m * distance + b; + return (1.09517561985 * distance + 20.1846165624); } public void updateRegressionDesmos() { m = DesmosServer.readDouble("m"); b = DesmosServer.readDouble("b"); + + DesmosServer.putArray("MB", m, b); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 00e141c..cf10e17 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -72,7 +72,7 @@ public class Turret extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); - SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.DEGREES_PER_ROT); + SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); } /** @@ -94,7 +94,7 @@ public class Turret extends SubsystemBase { } public void runshooterRotatePID(double targetAngle) { - targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); } From 946e7aee4747555b851c49f1bf3070a7c2cab295 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Mon, 14 Mar 2022 17:11:15 -0600 Subject: [PATCH 074/180] yes --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/commands/TrackTarget.java | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 961b817..c6146b4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -320,7 +320,7 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); + .whileHeld(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 2cd8924..d2aa6ec 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -100,7 +100,6 @@ public class TrackTarget extends CommandBase { } catch (Exception e){ e.printStackTrace(); - m_turret.runshooterRotatePID(180); // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } // vel = m_boomBoom.getVelocity(distance); From 8a25b7c1d5d955cf8505262f559deaab4036e1b2 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 14 Mar 2022 18:29:11 -0600 Subject: [PATCH 075/180] switch to pigeon2 + vector2d docs --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 6 ++-- src/main/java/frc4388/robot/RobotMap.java | 4 +-- .../frc4388/robot/commands/TrackTarget.java | 2 +- .../frc4388/robot/subsystems/Extender.java | 2 -- .../frc4388/robot/subsystems/SwerveDrive.java | 22 +++++--------- src/main/java/frc4388/utility/Vector2D.java | 29 ++++++++++++++++++- .../utility/{ => desmos}/DesmosClient.html | 0 .../utility/{ => desmos}/DesmosServer.java | 2 +- 9 files changed, 44 insertions(+), 25 deletions(-) rename src/main/java/frc4388/utility/{ => desmos}/DesmosClient.html (100%) rename src/main/java/frc4388/utility/{ => desmos}/DesmosServer.java (99%) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index b5c7845..af3774a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -24,9 +24,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.RunCommand; -import frc4388.utility.DesmosServer; import frc4388.utility.RobotTime; import frc4388.utility.VelocityCorrection; +import frc4388.utility.desmos.DesmosServer; /** * The VM is configured to automatically run this class, and to call the diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6146b4..0b55c20 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -297,8 +297,10 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0))); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 86701be..91b1854 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -10,7 +10,7 @@ import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; @@ -63,7 +63,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; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index d2aa6ec..8a5f59f 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -18,7 +18,7 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.VisionOdometry; -import frc4388.utility.DesmosServer; +import frc4388.utility.desmos.DesmosServer; public class TrackTarget extends CommandBase { /** Creates a new TrackTarget. */ diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 501bb56..1680b88 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -12,7 +12,6 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ExtenderConstants; -import frc4388.utility.DesmosServer; public class Extender extends SubsystemBase { @@ -51,7 +50,6 @@ public class Extender extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition()); - DesmosServer.putDouble("ExtenderPosition", m_extenderMotor.getEncoder().getPosition()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2e3d754..5f60cdb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,11 +4,9 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; -import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -46,14 +44,8 @@ public class SwerveDrive extends SubsystemBase { m_backLeftLocation, m_backRightLocation); public SwerveModule[] modules; - public WPI_PigeonIMU m_gyro; - protected FusionStatus fstatus = new FusionStatus(); + public WPI_Pigeon2 m_gyro; - /* - * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. - * The numbers used - * below are robot specific, and should be tuned. - */ public SwerveDrivePoseEstimator m_poseEstimator; public SwerveDriveOdometry m_odometry; @@ -65,7 +57,7 @@ public class SwerveDrive extends SubsystemBase { private final Field2d m_field = new Field2d(); public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, - WPI_PigeonIMU gyro) { + WPI_Pigeon2 gyro) { m_leftFront = leftFront; m_leftBack = leftBack; @@ -74,14 +66,14 @@ public class SwerveDrive extends SubsystemBase { m_gyro = gyro; modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack}; - + m_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(), new Pose2d(), m_kinematics, - VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), - VecBuilder.fill(Units.degreesToRadians(1)), - VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune + VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune + VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 7255d66..fe20cfc 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -27,22 +27,50 @@ public class Vector2D extends Vector2d { 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); } + /** + * 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); @@ -58,7 +86,6 @@ public class Vector2D extends Vector2d { @Override public String toString() { - Vector2d test = new Vector2d(); return ("(" + this.x + ", " + this.y + ")"); } diff --git a/src/main/java/frc4388/utility/DesmosClient.html b/src/main/java/frc4388/utility/desmos/DesmosClient.html similarity index 100% rename from src/main/java/frc4388/utility/DesmosClient.html rename to src/main/java/frc4388/utility/desmos/DesmosClient.html diff --git a/src/main/java/frc4388/utility/DesmosServer.java b/src/main/java/frc4388/utility/desmos/DesmosServer.java similarity index 99% rename from src/main/java/frc4388/utility/DesmosServer.java rename to src/main/java/frc4388/utility/desmos/DesmosServer.java index 2c743cd..e3ee55b 100644 --- a/src/main/java/frc4388/utility/DesmosServer.java +++ b/src/main/java/frc4388/utility/desmos/DesmosServer.java @@ -1,4 +1,4 @@ -package frc4388.utility; +package frc4388.utility.desmos; import java.io.BufferedReader; import java.io.IOException; From ed10b1e83f303cdaf56205d3130dda9b1e875d45 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 14 Mar 2022 18:58:44 -0600 Subject: [PATCH 076/180] goto 0 methods for hood + turret --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/commands/AimToCenter.java | 2 +- src/main/java/frc4388/robot/subsystems/Hood.java | 9 ++++++++- src/main/java/frc4388/robot/subsystems/Turret.java | 9 ++++++++- 4 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 0b55c20..e64241f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -322,7 +322,7 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotTurret.runshooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); + .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 4cdce03..9523577 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -43,7 +43,7 @@ public class AimToCenter extends CommandBase { @Override public void execute() { m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); - m_turret.runshooterRotatePID(m_targetAngle); + m_turret.runShooterRotatePID(m_targetAngle); // Check if limelight is within range (comment out to disable vision odo) if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 9ab9a1f..904eb26 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -90,7 +90,14 @@ public class Hood extends SubsystemBase { m_angleAdjusterMotor.set(input); } - public void resetGyroAngleAdj(){ + /** + * Run a PID to go to the zero position. + */ + public void gotoZero() { + runAngleAdjustPID(0); + } + + public void resetGyroAngleAdj() { m_angleEncoder.setPosition(0); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index cf10e17..403b8c4 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -93,7 +93,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); } - public void runshooterRotatePID(double targetAngle) { + public void runShooterRotatePID(double targetAngle) { targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); } @@ -102,6 +102,13 @@ public class Turret extends SubsystemBase { m_boomBoomRotateEncoder.setPosition(0); } + /** + * Run a PID to go to the zero position. + */ + public void gotoZero() { + runShooterRotatePID(0); + } + public double getboomBoomRotatePosition() { return m_boomBoomRotateEncoder.getPosition(); } From 8a902e635f592cb2bcb83c3d7c85c8a52333a93b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 14 Mar 2022 20:10:12 -0600 Subject: [PATCH 077/180] extender and intake changes --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 47 ++++++++----------- .../ExtenderIntakeGroup.java | 2 +- .../RunExtender.java | 31 +++++++++--- .../RunIntakeConditionally.java | 7 +-- .../{ => ShooterCommands}/AimToCenter.java | 2 +- .../commands/{ => ShooterCommands}/Shoot.java | 2 +- .../{ => ShooterCommands}/TrackTarget.java | 2 +- .../frc4388/robot/subsystems/Extender.java | 30 ++++-------- .../java/frc4388/robot/subsystems/Intake.java | 25 +++++----- .../frc4388/commands/AimToCenterTest.java | 3 +- 11 files changed, 77 insertions(+), 75 deletions(-) rename src/main/java/frc4388/robot/commands/{ => ExtenderIntakeCommands}/ExtenderIntakeGroup.java (95%) rename src/main/java/frc4388/robot/commands/{ => ExtenderIntakeCommands}/RunExtender.java (62%) rename src/main/java/frc4388/robot/commands/{ => ExtenderIntakeCommands}/RunIntakeConditionally.java (86%) rename src/main/java/frc4388/robot/commands/{ => ShooterCommands}/AimToCenter.java (98%) rename src/main/java/frc4388/robot/commands/{ => ShooterCommands}/Shoot.java (99%) rename src/main/java/frc4388/robot/commands/{ => ShooterCommands}/TrackTarget.java (98%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c57bd9d..c20669b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -145,6 +145,7 @@ public final class Constants { false, 10, 0, 0); //Find public static final StatorCurrentLimitConfiguration STATOR_CURRENT_LIMIT_CONFIG_INTAKE = new StatorCurrentLimitConfiguration( false, 15, 0, 0); + public static final double INTAKE_SPEED_MULTIPLIER = 0.4; } public static final class ExtenderConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e64241f..ffcc52b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,11 +62,11 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.commands.AimToCenter; -import frc4388.robot.commands.ExtenderIntakeGroup; import frc4388.robot.commands.RunMiddleSwitch; -import frc4388.robot.commands.Shoot; -import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; +import frc4388.robot.commands.ShooterCommands.AimToCenter; +import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Extender; @@ -96,13 +96,14 @@ import frc4388.utility.controller.DeadbandedXboxController; */ public class RobotContainer { private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName()); - /* RobotMap */ + + // RobotMap public final RobotMap m_robotMap = new RobotMap(); // Subsystems public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotSerializer); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor); public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor); @@ -115,12 +116,12 @@ public class RobotContainer { private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); public final Climber m_robotClimber = new Climber(testElbowMotor); - /* Controllers */ + // Controllers private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); - /* Autonomous */ + // Autonomous private PathPlannerTrajectory loadedPathTrajectory = null; private final ListeningSendableChooser autoChooser = new ListeningSendableChooser<>(this::loadPath); private final List pathPoints = new ArrayList<>(); @@ -275,20 +276,8 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new RunExtender(m_robotIntake, m_robotExtender, 0.5)) - // .whenReleased(new RunCommand(() -> RunExtender.changeDirection(), m_robotIntake, m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection())); - - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whenPressed(new ParallelCommandGroup( - // new RunCommand(() -> m_robotIntake.m_intakeMotor.set(ExtenderIntakeGroup.direction * -0.2), m_robotIntake), - // new RunCommand(() -> m_robotExtender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 0.5), m_robotExtender))) - // .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.changeDirection())); - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0))); @@ -297,10 +286,14 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) - // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0))); + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); @@ -321,8 +314,8 @@ public class RobotContainer { /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) @@ -343,7 +336,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotExtender.m_extenderMotor.getEncoder().setPosition(0), m_robotExtender)); + .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)); // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java similarity index 95% rename from src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java rename to src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 88d1cff..2e9748f 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ExtenderIntakeCommands; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import frc4388.robot.subsystems.Extender; diff --git a/src/main/java/frc4388/robot/commands/RunExtender.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java similarity index 62% rename from src/main/java/frc4388/robot/commands/RunExtender.java rename to src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java index 8af3e8f..4f4183e 100644 --- a/src/main/java/frc4388/robot/commands/RunExtender.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunExtender.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ExtenderIntakeCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ExtenderConstants; @@ -13,23 +13,38 @@ public class RunExtender extends CommandBase { private Extender extender; + private double error; + private double tolerance; + /** Creates a new RunExtender. */ public RunExtender(Extender extender) { // Use addRequirements() here to declare subsystem dependencies. - this.extender = extender; - + + updateError(); + tolerance = 5.0; + addRequirements(this.extender); } - + + public void updateError() { + if (ExtenderIntakeGroup.direction > 0) { + this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT); + } else { + this.error = Math.abs(this.extender.getPosition() - ExtenderConstants.EXTENDER_REVERSE_LIMIT); + } + } + // 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() { - this.extender.m_extenderMotor.set(ExtenderIntakeGroup.direction * 1.0); // TODO: change to 1.0 for actual speed, 0.5 is just for testing + System.out.println("RunExtender is working"); + this.extender.runExtender(ExtenderIntakeGroup.direction * 1.0); + updateError(); } // Called once the command ends or is interrupted. @@ -41,7 +56,9 @@ public class RunExtender extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(this.extender.m_extenderMotor.getEncoder().getPosition() - ExtenderConstants.EXTENDER_FORWARD_LIMIT) < 5) { + if (error < tolerance) { + System.out.println("RunExtender finished"); + this.extender.runExtender(0.0); return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java similarity index 86% rename from src/main/java/frc4388/robot/commands/RunIntakeConditionally.java rename to src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java index 01f92b0..9584967 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeConditionally.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/RunIntakeConditionally.java @@ -2,9 +2,10 @@ // 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; +package frc4388.robot.commands.ExtenderIntakeCommands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.IntakeConstants; import frc4388.robot.subsystems.Intake; public class RunIntakeConditionally extends CommandBase { @@ -28,9 +29,9 @@ public class RunIntakeConditionally extends CommandBase { @Override public void execute() { if (ExtenderIntakeGroup.direction > 0) { - this.intake.m_intakeMotor.set(-0.4); + this.intake.runAtOutput(-1); } else { - this.intake.m_intakeMotor.set(0); + this.intake.runAtOutput(0); } } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java similarity index 98% rename from src/main/java/frc4388/robot/commands/AimToCenter.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 9523577..7f8a8db 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ShooterCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java similarity index 99% rename from src/main/java/frc4388/robot/commands/Shoot.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index b2a574f..94bb04f 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ShooterCommands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java similarity index 98% rename from src/main/java/frc4388/robot/commands/TrackTarget.java rename to src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 8a5f59f..04d607c 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ShooterCommands; import java.util.ArrayList; diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 1680b88..34cd555 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -15,13 +15,11 @@ import frc4388.robot.Constants.ExtenderConstants; public class Extender extends SubsystemBase { - public CANSparkMax m_extenderMotor; + private CANSparkMax m_extenderMotor; private SparkMaxLimitSwitch m_inLimit; private SparkMaxLimitSwitch m_outLimit; - public boolean toggle; - /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { @@ -52,30 +50,20 @@ public class Extender extends SubsystemBase { SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition()); } - /** - * Runs The Extender- - * @param extended Wether the Extender Is Extended - */ - // public void runExtender(boolean extended) { - // if (!m_serializer.getBeam() && !extended) return; - // double extenderMotorSpeed = extended ? 0.25d : -0.25d; - // m_extenderMotor.set(extenderMotorSpeed); - // } - public void runExtender(double input) { // if (!m_serializer.getBeam() && input < 0.) return; m_extenderMotor.set(input); } + public double getPosition() { + return m_extenderMotor.getEncoder().getPosition(); + } + + public void setEncoder(double position) { + m_extenderMotor.getEncoder().setPosition(position); + } + public double getCurrent() { return m_extenderMotor.getOutputCurrent(); } - - /** - * Toggles The Extender - */ - // public void toggleExtender() { - // toggle = !toggle; - // runExtender(toggle); - // } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 3ad0491..a98bc05 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -4,27 +4,20 @@ package frc4388.robot.subsystems; -//Imported Limit switch ONLY -import com.revrobotics.SparkMaxLimitSwitch; - -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.commands.ExtenderIntakeGroup; - -import com.revrobotics.CANSparkMax; +import frc4388.robot.Constants.IntakeConstants; +import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; public class Intake extends SubsystemBase { - public WPI_TalonFX m_intakeMotor; - private Serializer m_serializer; + private WPI_TalonFX m_intakeMotor; /** Creates a new Intake. */ - public Intake(WPI_TalonFX intakeMotor, Serializer serializer) { + public Intake(WPI_TalonFX intakeMotor) { m_intakeMotor = intakeMotor; - m_serializer = serializer; } @Override @@ -39,10 +32,18 @@ public class Intake extends SubsystemBase { * @param rightTrigger Right Trigger to Run Outward */ public void runWithTriggers(double leftTrigger, double rightTrigger) { - m_intakeMotor.set((rightTrigger - leftTrigger) * 0.4); + m_intakeMotor.set((rightTrigger - leftTrigger) * IntakeConstants.INTAKE_SPEED_MULTIPLIER); SmartDashboard.putNumber("Intake Current Supply", m_intakeMotor.getSupplyCurrent()); SmartDashboard.putNumber("Intake Current Stator", m_intakeMotor.getStatorCurrent()); } + + public void runAtOutput(double output, double multiplier) { + m_intakeMotor.set(output * multiplier); + } + + public void runAtOutput(double output) { + m_intakeMotor.set(output * IntakeConstants.INTAKE_SPEED_MULTIPLIER); + } public double getCurrent() { return m_intakeMotor.getSupplyCurrent(); diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 644d3a9..0bc247d 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -2,7 +2,8 @@ package frc4388.commands; import org.junit.Test; -import frc4388.robot.commands.AimToCenter; +import frc4388.robot.commands.ShooterCommands.AimToCenter; + import org.junit.Assert; public class AimToCenterTest { 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 078/180] 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 565d5d1d628540340082620e11e7d6cbb22950f2 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 15 Mar 2022 13:55:31 -0600 Subject: [PATCH 079/180] shooter tuning --- src/main/deploy/ShooterData.csv | 14 ++++-- .../java/frc4388/robot/RobotContainer.java | 46 ++++++++++--------- .../commands/ShooterCommands/TrackTarget.java | 1 + 3 files changed, 36 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 1284d91..bdd5dc9 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,4 +1,10 @@ -Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s) -81, ,0 ,8000 ,0 -150, ,-62.1 ,10000 ,0 -227, ,-103.9 ,10500 ,0 +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), +78.5 ,-29.5 ,8000 ,0, +99 ,-39.62 ,8500 ,0, +127.25 ,-49.12 ,9500 ,0, +150 ,-66.22 ,10000 ,0, +164.5 ,-75.52 ,10000 ,0, +186 ,-76.24 ,10000 ,0, +207 ,-104.07 ,11000 ,0, +227 ,-105.32 ,11500 ,0, +255.5 ,-97.8 ,14500 ,0, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ffcc52b..cec97f5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -264,18 +264,19 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft - // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) - // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //20ft + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + //Toggles extender in and out new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -285,21 +286,21 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0))); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); + // .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); + + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) @@ -318,9 +319,10 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + // .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); + // .whileHeld% /* Button Box Buttons */ @@ -349,10 +351,12 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) - .whileHeld(new RunCommand(() -> System.out.println("LeftButton"))); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value) - .whileHeld(new RunCommand(() -> System.out.println("RightButton"))); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } /** diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 04d607c..1b62d45 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -86,6 +86,7 @@ public class TrackTarget extends CommandBase { y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); + DesmosServer.putDouble("distance", distance); updateRegressionDesmos(); From b97f993e7d54cf4ba7540784900fe3b46c67c853 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 15 Mar 2022 14:23:37 -0600 Subject: [PATCH 080/180] desmos shi --- src/main/java/frc4388/robot/Constants.java | 3 +- .../ExtenderIntakeGroup.java | 4 +- .../commands/ShooterCommands/TrackTarget.java | 13 +++-- .../frc4388/utility/desmos/DesmosClient.html | 58 +++++++++++++++++-- .../frc4388/utility/desmos/DesmosServer.java | 38 ++++++++++++ 5 files changed, 103 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c20669b..bd25aab 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -243,7 +243,8 @@ public final class Constants { public static final double RANGE = 10; - public static final double LIMELIGHT_RADIUS = 1.d; + public static final double EDGE_TO_CENTER = 20; + public static final double LIMELIGHT_RADIUS = 8; public static final double SHOOTER_CORRECTION = 1.d; } } diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 2e9748f..7eb3572 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -19,11 +19,11 @@ public class ExtenderIntakeGroup extends ParallelRaceGroup { public ExtenderIntakeGroup(Intake intake, Extender extender) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - ExtenderIntakeGroup.direction = 1; + ExtenderIntakeGroup.direction = 1; // Does this make sense? It kind of defeats the purpose of making it static, does it work without? addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); } - public static void changeDirection() { + public static void changeDirection() { // Never implemented? ExtenderIntakeGroup.direction *= -1; } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 1b62d45..4fee5c8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -86,28 +86,29 @@ public class TrackTarget extends CommandBase { y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); - DesmosServer.putDouble("distance", distance); updateRegressionDesmos(); double regressedDistance = distanceRegression(distance); + regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; DesmosServer.putDouble("distanceReg", regressedDistance); //Vision odemetry circle fit based pose estimate Point targetOffset = m_visionOdometry.getTargetOffset(); DesmosServer.putPoint("targetOff", targetOffset); + vel = m_boomBoom.getVelocity(distance); + hood = m_boomBoom.getHood(distance); + // m_boomBoom.runDrumShooter(vel); + m_boomBoom.runDrumShooterVelocityPID(vel); + m_hood.runAngleAdjustPID(hood); + // isExecuted = true; } catch (Exception e){ e.printStackTrace(); // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } - // vel = m_boomBoom.getVelocity(distance); - // hood = m_boomBoom.getHood(distance); - // m_boomBoom.runDrumShooter(vel); - // m_boomBoom.runDrumShooterVelocityPID(vel); - // m_hood.runAngleAdjustPID(hood); // m_turret.runshooterRotatePID(m_targetAngle); } diff --git a/src/main/java/frc4388/utility/desmos/DesmosClient.html b/src/main/java/frc4388/utility/desmos/DesmosClient.html index f8a77f3..ca3170c 100644 --- a/src/main/java/frc4388/utility/desmos/DesmosClient.html +++ b/src/main/java/frc4388/utility/desmos/DesmosClient.html @@ -27,7 +27,8 @@ const EXPRESSION_TYPES = { 'expression': ['number', 'point', 'array'], 'note': ['note'], - 'table': ['table'] + 'table': ['table'], + 'clear': ['all', 'except', 'single'] }; let status = document.getElementById('status'); @@ -35,6 +36,8 @@ let elt = document.getElementById('calculator'); let calculator = Desmos.GraphingCalculator(elt); + const clearState = calculator.getState(); + let helperExpressions = {}; let variables = []; @@ -72,6 +75,8 @@ } else if(EXPRESSION_TYPES['table'].includes(variable['type'])) { let cols = getColumns(variable['value'].split(' ')); calculator.setExpression({ type: 'table', id: variable['name'], columns: cols}); + } else if(EXPRESSION_TYPES['clear'].includes(variable['type'])) { + clear(variable['value'], variable['type']); } else console.log('Invalid input type : ' + variable['type']); } @@ -92,6 +97,47 @@ return cols; } + function clear(lname, type) { + let state = calculator.getState(); + let expressions = state.expressions.list; + + if(expressions.length === 0) + return; + + let lnames = lname.split(','); + + switch(type) { + case 'all': + state = clearState; + break; + case 'except': + for(let i = 0; i < expressions.length; i++) { + if(!expressions[i].latex.includes('=')) + return; + + let expressionName = regularName(expressions[i].latex.split('=')[0]); + if(!lnames.includes(expressionName)) { + expressions.splice(i, 1); + i--; + } + } + break; + case 'single': + for(let i = 0; i < expressions.length; i++) { + if(!expressions[i].latex.includes('=')) + return; + + if(lnames.includes(regularName(expressions[i].split('=')[0]))) { + expressions.splice(i, 1); + i--; + } + } + break; + } + + calculator.setState(state); + } + function getVariables() { let vars = []; let expressions = calculator.getExpressions(); @@ -116,13 +162,14 @@ return 'point'; else if(raw.match(/\[[a-zA-Z0-9.,_{}\(\)]*\]/)) return 'array'; - else if(raw.includes('=')) - return 'number'; else - return 'none'; + return 'number'; } function readVariable(latex) { + if(!latex.includes('=')) + return []; + let vars = []; let lname = latex.split('=')[0]; let lvalue = latex.split('=')[1]; @@ -191,6 +238,9 @@ } function regularName(lname) { + if(lname.length == 1) + return lname; + return lname.substring(0, 1) + lname.substring(3, lname.length - 1); } diff --git a/src/main/java/frc4388/utility/desmos/DesmosServer.java b/src/main/java/frc4388/utility/desmos/DesmosServer.java index e3ee55b..cefe858 100644 --- a/src/main/java/frc4388/utility/desmos/DesmosServer.java +++ b/src/main/java/frc4388/utility/desmos/DesmosServer.java @@ -23,6 +23,8 @@ public class DesmosServer extends Thread { private static HashMap desmosQueue = new HashMap<>(); private static HashMap readVariables = new HashMap<>(); + private static int clearCount = 0; + private static boolean running = false; private ServerSocket serverSocket; @@ -127,6 +129,8 @@ public class DesmosServer extends Thread { clientOutput.write(getJSONOutput().getBytes()); clientOutput.flush(); clientOutput.close(); + + clearCount = 0; } /** @@ -245,6 +249,40 @@ public class DesmosServer extends Thread { desmosQueue.put(name, new String[] {"table", tableStr}); } + /** + * Clears entire expression panel + */ + public static void clearAll() { + desmosQueue.put("clear" + clearCount, new String[] {"all", "n/a"}); + clearCount++; + } + + /** + * Clears entire expression panel except for expressions in {@code names} + * + * @param names Names which should be preserved in clear + */ + public static void clearExcept(String... names) { + String namesStr = Arrays.toString(names).replace(" ", ""); + namesStr = namesStr.substring(1, namesStr.length()-1); + + desmosQueue.put("clear" + clearCount, new String[] {"except", namesStr}); + clearCount++; + } + + /** + * Removes expressions in {@code names} + * + * @param names Names which should be removed + */ + public static void clearSpecific(String... names) { + String namesStr = Arrays.toString(names).replace(" ", ""); + namesStr = namesStr.substring(1, namesStr.length()-1); + + desmosQueue.put("clear" + clearCount, new String[] {"specific", namesStr}); + clearCount++; + } + // --------------------------------------------------------------------- /** From 98a82fb9de3ff008dbc457bdbfd9012b0346c2eb Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 15 Mar 2022 18:27:37 -0600 Subject: [PATCH 081/180] Added buttons --- src/main/java/frc4388/robot/RobotContainer.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cec97f5..8b43872 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -264,9 +264,9 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-55.55))) // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //20ft - .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) //20ft + // .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) @@ -319,9 +319,9 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); //B > Shoot with Lime - // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - // .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); // .whileHeld% From 7091d61515d15076876724d061ff3e5ed7569a47 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Tue, 15 Mar 2022 20:02:36 -0600 Subject: [PATCH 082/180] dfgn --- .../frc4388/robot/subsystems/Storage.java | 35 +++++++++---------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 8f78712..5823c96 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -7,31 +7,30 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.DigitalInput; +import com.revrobotics.ColorSensorV3; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor; - private DigitalInput m_beamShooter; - private DigitalInput m_beamIntake; + public ColorSensorV3 m_colorSensor; + /** Creates a new Storage. */ - public Storage(CANSparkMax storageMotor, DigitalInput beamShooter, DigitalInput beamIntake) { + public Storage(CANSparkMax storageMotor, ColorSensorV3 colorSensor) { m_storageMotor = storageMotor; - m_beamShooter = beamShooter; - m_beamIntake = beamIntake; + m_colorSensor = colorSensor; } public Storage(CANSparkMax storageMotor) { m_storageMotor = storageMotor; - m_beamShooter = null; - m_beamIntake = null; + m_colorSensor = null; } /** * If The Beam Is Broken, Run Storage * If Else, Stop Running Storage */ public void manageStorage() { - if (getBeamIntake()) runStorage(0.d); - else runStorage(1.d); + if (getColorBroken()) runStorage(0.d); + else runStorage(0.9); } /** @@ -42,19 +41,19 @@ public class Storage extends SubsystemBase { m_storageMotor.set(input); } /** - * Gets The Beam State On The Shooter + * Gets the state of the colorsensor as a beam break * @return The State Of The Beam on the Shooter */ - public boolean getBeamShooter(){ - return m_beamShooter.get();//True if open + public boolean getColorBroken(){ + return (getRed() || getBlue()); } - /** - * Gets The Beam State Of The Intake - * @return The Beam State Of The Intake - */ - public boolean getBeamIntake(){ - return m_beamIntake.get(); //True if open + public boolean getRed(){ + return (m_colorSensor.getRed() >= 200); + } + + public boolean getBlue(){ + return (m_colorSensor.getBlue() >= 200); } From ca4d87977ad205d5b2a5a720d55e22a5ec3e87d4 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 16 Mar 2022 10:32:15 -0600 Subject: [PATCH 083/180] Track target working --- src/main/deploy/ShooterData.csv | 4 +++- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 3 ++- .../commands/ShooterCommands/TrackTarget.java | 20 +++++++++++++------ .../frc4388/robot/subsystems/BoomBoom.java | 2 +- .../robot/subsystems/VisionOdometry.java | 2 ++ 7 files changed, 25 insertions(+), 12 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index bdd5dc9..52e1e46 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,4 +1,5 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), +0 ,-29.5 ,8000 ,0, 78.5 ,-29.5 ,8000 ,0, 99 ,-39.62 ,8500 ,0, 127.25 ,-49.12 ,9500 ,0, @@ -7,4 +8,5 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), 186 ,-76.24 ,10000 ,0, 207 ,-104.07 ,11000 ,0, 227 ,-105.32 ,11500 ,0, -255.5 ,-97.8 ,14500 ,0, +255.5 ,-105.8 ,13500 ,0, +999 ,-105.8 ,13500 ,0, \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bd25aab..6ecf795 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -236,8 +236,8 @@ public final class Constants { public static final double LIME_HEIGHT = 26; public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?) public static final double H_FOV = 59.6; - public static final double V_FOV = 49.7; - public static final double LIME_HIXELS = 960; + public static final double V_FOV = 45.7; + public static final double LIME_HIXELS = 920; public static final double LIME_VIXELS = 720; public static final double TURRET_kP = 0.1; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index af3774a..30e77dd 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -216,7 +216,7 @@ public class Robot extends TimedRobot { LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } - m_robotContainer.m_robotVisionOdometry.setLEDs(false); + m_robotContainer.m_robotVisionOdometry.setLEDs(true); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8b43872..a68376e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -321,7 +321,8 @@ public class RobotContainer { //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))); + .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); // .whileHeld% diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 4fee5c8..b313fa8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -9,6 +9,7 @@ import java.util.ArrayList; import org.opencv.core.Point; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.VisionConstants; @@ -76,7 +77,8 @@ public class TrackTarget extends CommandBase { DesmosServer.putPoint("average", average); output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 2; + output *= 4; + // output *= 0.5; DesmosServer.putDouble("output", output); m_turret.runTurretWithInput(output); @@ -94,15 +96,21 @@ public class TrackTarget extends CommandBase { DesmosServer.putDouble("distanceReg", regressedDistance); //Vision odemetry circle fit based pose estimate - Point targetOffset = m_visionOdometry.getTargetOffset(); - DesmosServer.putPoint("targetOff", targetOffset); + // Point targetOffset = m_visionOdometry.getTargetOffset(); + // DesmosServer.putPoint("targetOff", targetOffset); - vel = m_boomBoom.getVelocity(distance); - hood = m_boomBoom.getHood(distance); + vel = m_boomBoom.getVelocity(regressedDistance + 30); + hood = m_boomBoom.getHood(regressedDistance + 30); // m_boomBoom.runDrumShooter(vel); m_boomBoom.runDrumShooterVelocityPID(vel); m_hood.runAngleAdjustPID(hood); + SmartDashboard.putNumber("Regressed Distance", regressedDistance); + SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("Hood Target Angle Track", hood); + SmartDashboard.putNumber("Vel Target Track", vel); + + // isExecuted = true; } catch (Exception e){ @@ -113,7 +121,7 @@ public class TrackTarget extends CommandBase { } public final double distanceRegression(double distance) { - return (1.09517561985 * distance + 20.1846165624); + return (79.6078 * Math.pow(1.01343, distance) - 56.6671); } public void updateRegressionDesmos() { diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index bc8d8cf..9e7d4b8 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -217,7 +217,7 @@ public class BoomBoom extends SubsystemBase { public void runDrumShooterVelocityPID(double targetVel) { SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset); - m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init // New BoomBoom controller stuff // Controls a motor with the output of the BangBang controller diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index f1048b0..2a0abd2 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -59,6 +59,8 @@ public class VisionOdometry extends SubsystemBase { public ArrayList getTargetPoints() throws VisionObscuredException { PhotonPipelineResult result = m_camera.getLatestResult(); latency = result.getLatencyMillis(); + + System.out.println("Result: " + result.hasTargets() + ", latency: " + latency); if(!result.hasTargets()) throw new VisionObscuredException(); From 35e6515a74b858ef870ea0c0b75f83dd4c3c5111 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 11:22:51 -0600 Subject: [PATCH 084/180] random code cleanup shtuff --- .../java/frc4388/robot/RobotContainer.java | 4 +- src/main/java/frc4388/robot/RobotMap.java | 40 ++++++--------- .../RunMiddleSwitch.java | 2 +- .../ExtenderIntakeGroup.java | 5 +- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- .../java/frc4388/robot/subsystems/Turret.java | 49 ++++++++++++------- .../java/frc4388/robot/subsystems/Vision.java | 4 +- 7 files changed, 56 insertions(+), 50 deletions(-) rename src/main/java/frc4388/robot/commands/{ => ButtonBoxCommands}/RunMiddleSwitch.java (96%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a68376e..1e6cd5b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,7 +62,7 @@ import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; -import frc4388.robot.commands.RunMiddleSwitch; +import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Shoot; @@ -316,7 +316,7 @@ public class RobotContainer { .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT), m_robotTurret)); + // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kB.value) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 91b1854..065ec83 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -188,16 +188,16 @@ public class RobotMap { SwerveDriveConstants.SWERVE_TIMEOUT_MS); } -// // Shooter Config -// /* Boom Boom Subsystem */ + // Shooter Config + /* Boom Boom Subsystem */ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); -// // turret subsystem + // turret subsystem public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // hood subsystem - public CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); // Create motor CANSparkMax void configureShooterMotorControllers() { @@ -219,9 +219,9 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON - shooterFalconRight.setInverted(false); - shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.configFactoryDefault(); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.setInverted(false); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); // m_shooterFalconRight.configPeakOutputForward(0, @@ -236,24 +236,18 @@ public class RobotMap { ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.follow(shooterFalconLeft); -// } -// // /* Turret Subsytem */ - // shooterFalconRight.configStatorCurrentLimit(new - // StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers - // out of our ass anymore - // shooterFalconLeft.configSupplyCurrentLimit(new - // SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull -// // numbers out of our ass anymore + // turret + shooterTurret.restoreFactoryDefaults(); + shooterTurret.setIdleMode(IdleMode.kBrake); + shooterTurret.setInverted(true); -// // hood subsystem + // hood subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setInverted(true); } - - - + /* Serializer Subsystem */ public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); @@ -277,14 +271,12 @@ public class RobotMap { extenderMotor.setIdleMode(IdleMode.kBrake); } - void configureSerializerMotors() { - serializerBelt.restoreFactoryDefaults(); - } + void configureSerializerMotors() { + serializerBelt.restoreFactoryDefaults(); + } - /* Storage Subsystem */ + /* Storage Subsystem */ public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); -// public final DigitalInput beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); -// public final DigitalInput beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); void configureStorageMotors() { storageMotor.restoreFactoryDefaults(); diff --git a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java similarity index 96% rename from src/main/java/frc4388/robot/commands/RunMiddleSwitch.java rename to src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java index 3a61d55..095cc9f 100644 --- a/src/main/java/frc4388/robot/commands/RunMiddleSwitch.java +++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/RunMiddleSwitch.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ButtonBoxCommands; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 7eb3572..02ce30a 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -13,13 +13,14 @@ import frc4388.robot.subsystems.Intake; // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class ExtenderIntakeGroup extends ParallelRaceGroup { - public static int direction = 1; // assume extender starts retracted completely + public static int direction; /** Creates a new RunExtenderAndIntake. */ public ExtenderIntakeGroup(Intake intake, Extender extender) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - ExtenderIntakeGroup.direction = 1; // Does this make sense? It kind of defeats the purpose of making it static, does it work without? + + ExtenderIntakeGroup.direction = 1; // assume extender starts retracted completely addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 5f60cdb..ee63473 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -65,7 +65,7 @@ public class SwerveDrive extends SubsystemBase { m_rightBack = rightBack; m_gyro = gyro; - modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack}; + modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = new SwerveDrivePoseEstimator( m_gyro.getRotation2d(), diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 403b8c4..7698464 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -29,28 +29,20 @@ public class Turret extends SubsystemBase { public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, // MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; - // SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; - public Gyro m_turretGyro; + SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; - public double m_targetDistance = 0; + SparkMaxPIDController m_boomBoomRotatePIDController; + public RelativeEncoder m_boomBoomRotateEncoder; - public boolean m_isAimReady = false; - - SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); - public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); - - // Variables - public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit.enableLimitSwitch(true); - // m_boomBoomLeftLimit.enableLimitSwitch(true); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + setTurretLimitSwitches(false); // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); @@ -58,8 +50,13 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); setTurretSoftLimits(true); - m_boomBoomRotateMotor.setInverted(true); - + setTurretPIDGains(); + } + + /** + * Set gains for turret PIDs. + */ + public void setTurretPIDGains() { m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); @@ -84,6 +81,15 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } + /** + * Set status of turret limit switches. + * @param set Boolean to set limit switches to. + */ + public void setTurretLimitSwitches(boolean set) { + m_boomBoomRightLimit.enableLimitSwitch(set); + m_boomBoomLeftLimit.enableLimitSwitch(set); + } + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1; @@ -109,6 +115,13 @@ public class Turret extends SubsystemBase { runShooterRotatePID(0); } + /** + * Run a PID to go to the midpoint position, between the two soft limits. + */ + public void gotoMidpoint() { + runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + public double getboomBoomRotatePosition() { return m_boomBoomRotateEncoder.getPosition(); } @@ -116,7 +129,7 @@ public class Turret extends SubsystemBase { public double getBoomBoomAngleDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; - } + } // TODO: does this method work? public double getCurrent(){ return m_boomBoomRotateMotor.getOutputCurrent(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 748bcd5..08d3d5e 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -86,10 +86,10 @@ public void track(){ public void checkFinished(){ if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ - m_turret.m_isAimReady = true; + // m_turret.m_isAimReady = true; } else{ - m_turret.m_isAimReady = false; + // m_turret.m_isAimReady = false; } } From db61c85202286ea5435fd6db0d20c20727745034 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 11:47:40 -0600 Subject: [PATCH 085/180] spit out wrong color command --- .../ShooterCommands/SpitOutWrongColor.java | 73 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Turret.java | 2 +- 2 files changed, 74 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java b/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java new file mode 100644 index 0000000..7be2329 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java @@ -0,0 +1,73 @@ +// 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.ShooterCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Turret; +import frc4388.utility.RobotTime; + +public class SpitOutWrongColor extends CommandBase { + + // subsystems + private BoomBoom drum; + private Turret turret; + private Storage storage; + + // time (in milliseconds) + private long initialTime; + private long elapsedTime; + private long threshold; + + private double initialTurret; + private int spitVelocity; + + /** Creates a new SpitOutWrongColor. */ + public SpitOutWrongColor(BoomBoom drum, Turret turret, Storage storage) { + // Use addRequirements() here to declare subsystem dependencies. + + this.drum = drum; + this.turret = turret; + this.storage = storage; + + initialTime = System.currentTimeMillis(); + elapsedTime = 0; + threshold = 2000; + + initialTurret = this.turret.getEncoderPosition(); + spitVelocity = 2000; + + addRequirements(this.drum, this.turret, this.storage); + } + + // 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() { + elapsedTime = System.currentTimeMillis() - initialTime; + + this.storage.runStorage(0.9); + this.turret.gotoMidpoint(); + this.drum.runDrumShooterVelocityPID(spitVelocity); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.storage.runStorage(0.0); + this.turret.runShooterRotatePID(initialTurret * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (elapsedTime >= threshold); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 7698464..446bb02 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -122,7 +122,7 @@ public class Turret extends SubsystemBase { runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); } - public double getboomBoomRotatePosition() { + public double getEncoderPosition() { return m_boomBoomRotateEncoder.getPosition(); } From a67e3ebd4bcd54eaa878ec568a471eb4751941e5 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 16 Mar 2022 12:05:34 -0600 Subject: [PATCH 086/180] storage and turret shtuff --- src/main/deploy/ShooterData.csv | 4 ++-- src/main/java/frc4388/robot/Constants.java | 8 ++++---- .../frc4388/robot/subsystems/Storage.java | 7 +++++-- .../java/frc4388/robot/subsystems/Turret.java | 20 ++++++++++++------- 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index 52e1e46..c11dc0f 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -8,5 +8,5 @@ Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), 186 ,-76.24 ,10000 ,0, 207 ,-104.07 ,11000 ,0, 227 ,-105.32 ,11500 ,0, -255.5 ,-105.8 ,13500 ,0, -999 ,-105.8 ,13500 ,0, \ No newline at end of file +255.5 ,-105.8 ,13500 ,0, +999 ,-105.8 ,13500 ,0, \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6ecf795..2f43844 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -203,12 +203,12 @@ public final class Constants { // ID public static final int TURRET_MOTOR_CAN_ID = 19; //Gains for turret - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.6); - public static final double SHOOTER_TURRET_MIN = -0.6; + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, TURRET_SPEED_MULTIPLIER); + public static final double SHOOTER_TURRET_MIN = -TURRET_SPEED_MULTIPLIER; //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); - public static final double TURRET_FORWARD_LIMIT = 17.0; // TODO: find - public static final double TURRET_REVERSE_LIMIT = -105.0; // TODO: find + public static final double TURRET_FORWARD_LIMIT = 0.0; + public static final double TURRET_REVERSE_LIMIT = -95.0; //Find //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); // TODO: tune values diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5823c96..adcda74 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.DigitalInput; import com.revrobotics.ColorSensorV3; +import edu.wpi.first.wpilibj.util.Color; public class Storage extends SubsystemBase { public CANSparkMax m_storageMotor; @@ -49,11 +50,13 @@ public class Storage extends SubsystemBase { } public boolean getRed(){ - return (m_colorSensor.getRed() >= 200); + // return (m_colorSensor.getRed() >= 200 && m_colorSensor.getBlue() < 100 && m_colorSensor.getGreen() < 100); + return (m_colorSensor.getColor() == Color.kRed); } public boolean getBlue(){ - return (m_colorSensor.getBlue() >= 200); + // return (m_colorSensor.getBlue() >= 200 && m_colorSensor.getRed() < 100 && m_colorSensor.getGreen() < 100); + return (m_colorSensor.getColor() == Color.kBlue); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 403b8c4..d6aae1a 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.utility.Gains; public class Turret extends SubsystemBase { @@ -38,6 +39,8 @@ public class Turret extends SubsystemBase { SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + SparkMaxLimitSwitch m_boomBoomLeftLimit; + SparkMaxLimitSwitch m_boomBoomRightLimit; // Variables public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument @@ -47,12 +50,10 @@ public class Turret extends SubsystemBase { m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit.enableLimitSwitch(true); - // m_boomBoomLeftLimit.enableLimitSwitch(true); - // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); @@ -73,6 +74,8 @@ public class Turret extends SubsystemBase { // This method will be called once per scheduler run SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2); + if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2); } /** @@ -88,7 +91,10 @@ public class Turret extends SubsystemBase { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1; } - + /** + * Move the turret with an input + * @param input from -1.0 to 1.0, positive is clockwise + */ public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); } From 7ae7591951d16ec46e903c92b4da3eb6501330cb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 13:59:10 -0600 Subject: [PATCH 087/180] manage storage + alliance (needs testing) --- src/main/java/frc4388/robot/Robot.java | 8 ++ .../java/frc4388/robot/RobotContainer.java | 7 ++ .../robot/commands/StorageCommands/Drum.java | 0 .../StorageCommands/ManageStorage.java | 76 +++++++++++++++++++ .../SpitOutWrongColor.java | 23 +++--- .../frc4388/robot/subsystems/Storage.java | 10 ++- .../java/frc4388/robot/subsystems/Turret.java | 10 +-- 7 files changed, 114 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/StorageCommands/Drum.java create mode 100644 src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java rename src/main/java/frc4388/robot/commands/{ShooterCommands => StorageCommands}/SpitOutWrongColor.java (88%) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 30e77dd..cb70709 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -49,6 +50,8 @@ public class Robot extends TimedRobot { private static DesmosServer desmosServer = new DesmosServer(8000); + public static Alliance alliance; + /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -231,6 +234,8 @@ public class Robot extends TimedRobot { public void autonomousInit() { LOGGER.fine("autonomousInit()"); + Robot.alliance = DriverStation.getAlliance(); + selectedOdo = odoChooser.getSelected(); if (selectedOdo == null) { selectedOdo = m_robotContainer.getOdometry(); @@ -256,6 +261,9 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { LOGGER.fine("teleopInit()"); + + Robot.alliance = DriverStation.getAlliance(); + m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); selectedOdo = odoChooser.getSelected(); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1e6cd5b..ebd9715 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -67,6 +67,7 @@ import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.robot.commands.ShooterCommands.TrackTarget; +import frc4388.robot.commands.StorageCommands.ManageStorage; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Extender; @@ -164,6 +165,12 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + + m_robotStorage.setDefaultCommand( + new ManageStorage(m_robotStorage, + m_robotBoomBoom, + m_robotTurret).withName("Storage ManageStorage defaultCommand")); + // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) // ); diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java b/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java new file mode 100644 index 0000000..0639307 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java @@ -0,0 +1,76 @@ +// 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.StorageCommands; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; +import frc4388.robot.Robot; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Storage; +import frc4388.robot.subsystems.Turret; + +public class ManageStorage extends CommandBase { + + // subsystems + private Storage storage; + private BoomBoom drum; + private Turret turret; + + private Alliance alliance; + private boolean rightColor; + + /** Creates a new ManageStorage. */ + public ManageStorage(Storage storage, BoomBoom drum, Turret turret) { + // Use addRequirements() here to declare subsystem dependencies. + + this.storage = storage; + this.drum = drum; + this.turret = turret; + + rightColor = true; + + addRequirements(this.storage, this.drum, this.turret); + } + + // 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() { + checkColor(); + + if (rightColor) { + this.storage.manageStorage(); + } else { + + // * CommandScheduler.getInstance().schedule(new ExampleCommand()); + // * new ExampleCommand().schedule(); + // * new ExampleCommand().execute(); + + new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command + } + } + + private void checkColor() { + this.alliance = this.storage.getColor(); + rightColor = this.alliance.equals(Robot.alliance); + } + + // 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/commands/ShooterCommands/SpitOutWrongColor.java b/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java similarity index 88% rename from src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java rename to src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java index 7be2329..5c79247 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/SpitOutWrongColor.java +++ b/src/main/java/frc4388/robot/commands/StorageCommands/SpitOutWrongColor.java @@ -2,21 +2,20 @@ // 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.ShooterCommands; +package frc4388.robot.commands.StorageCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.Turret; -import frc4388.utility.RobotTime; public class SpitOutWrongColor extends CommandBase { // subsystems + private Storage storage; private BoomBoom drum; private Turret turret; - private Storage storage; // time (in milliseconds) private long initialTime; @@ -27,27 +26,27 @@ public class SpitOutWrongColor extends CommandBase { private int spitVelocity; /** Creates a new SpitOutWrongColor. */ - public SpitOutWrongColor(BoomBoom drum, Turret turret, Storage storage) { + public SpitOutWrongColor(Storage storage, BoomBoom drum, Turret turret) { // Use addRequirements() here to declare subsystem dependencies. + this.storage = storage; this.drum = drum; this.turret = turret; - this.storage = storage; + addRequirements(this.storage, this.drum, this.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { initialTime = System.currentTimeMillis(); elapsedTime = 0; threshold = 2000; initialTurret = this.turret.getEncoderPosition(); spitVelocity = 2000; - - addRequirements(this.drum, this.turret, this.storage); } - - // 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() { diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index adcda74..ee1311e 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -6,7 +6,12 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; + +import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + import com.revrobotics.ColorSensorV3; import edu.wpi.first.wpilibj.util.Color; @@ -58,7 +63,10 @@ public class Storage extends SubsystemBase { // return (m_colorSensor.getBlue() >= 200 && m_colorSensor.getRed() < 100 && m_colorSensor.getGreen() < 100); return (m_colorSensor.getColor() == Color.kBlue); } - + + public Alliance getColor() { + return (getRed() ? Alliance.Red : Alliance.Blue); + } @Override /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index cb07d7f..c0bf99b 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -31,10 +31,7 @@ public class Turret extends SubsystemBase { // MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; - SparkMaxPIDController m_boomBoomRotatePIDController; - public RelativeEncoder m_boomBoomRotateEncoder; - - SparkMaxPIDController m_boomBoomRotatePIDController; + public SparkMaxPIDController m_boomBoomRotatePIDController; public RelativeEncoder m_boomBoomRotateEncoder; SparkMaxLimitSwitch m_boomBoomLeftLimit; @@ -141,9 +138,8 @@ public class Turret extends SubsystemBase { } public double getBoomBoomAngleDegrees() { - return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 - / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; - } // TODO: does this method work? + return (getEncoderPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + } public double getCurrent(){ return m_boomBoomRotateMotor.getOutputCurrent(); From 38459ba40c3d27f69082352187ef719e485c1cb2 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 14:32:41 -0600 Subject: [PATCH 088/180] small changes shtuf --- src/main/java/frc4388/robot/Robot.java | 15 --------------- .../commands/StorageCommands/ManageStorage.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 6 +++--- .../java/frc4388/robot/subsystems/Turret.java | 9 ++++++--- src/main/java/frc4388/utility/Vector2D.java | 10 ++++++++++ 5 files changed, 20 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index cb70709..fbb7cfa 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -162,15 +162,6 @@ public class Robot extends TimedRobot { // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - - // VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom); - // System.out.println("Position: " + vc.position); - // System.out.println("Velocity: " + vc.cartesianVelocity); - // System.out.println("Target: " + vc.target.toString()); - - - //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition()); - //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition()); // odo chooser stuff addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), @@ -178,12 +169,6 @@ public class Robot extends TimedRobot { new Pose2d(1, 3, new Rotation2d(Math.PI/4))); updateOdoChooser(); SmartDashboard.putData("Odometry Chooser", odoChooser); - - // 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()); } public void updateOdoChooser() { diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java index 0639307..44cc062 100644 --- a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java +++ b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java @@ -53,7 +53,7 @@ public class ManageStorage extends CommandBase { // * CommandScheduler.getInstance().schedule(new ExampleCommand()); // * new ExampleCommand().schedule(); - // * new ExampleCommand().execute(); + // * new ExampleCommand().execute(); (accompanied by initialize and onFinished) new SpitOutWrongColor(this.storage, this.drum, this.turret); // ? is this how you run a command inside a command } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ee63473..262c950 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -169,9 +169,9 @@ public class SwerveDrive extends SubsystemBase { // chassis speeds // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds - SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); + // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); + // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); + // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index c0bf99b..d2dc34f 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -45,10 +45,8 @@ public class Turret extends SubsystemBase { m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit.enableLimitSwitch(true); - m_boomBoomLeftLimit.enableLimitSwitch(true); + setTurretLimitSwitches(true); - // setTurretLimitSwitches(false); // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); @@ -74,8 +72,13 @@ public class Turret extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + + SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); + SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2); if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2); } diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index fe20cfc..8ce1968 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -57,6 +57,16 @@ public class Vector2D extends Vector2d { 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. From 8aedbd33dcbf26fbc6b0f7f51db3a5a928bb5d17 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 14:33:40 -0600 Subject: [PATCH 089/180] 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 090/180] 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 091/180] 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 092/180] 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 093/180] 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 094/180] 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 095/180] 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 096/180] 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 2f23aed7f1997beff59907637e8477239741a78b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 15:35:38 -0600 Subject: [PATCH 097/180] vel correction setup --- src/main/deploy/ShooterData.csv | 24 ++++++------ src/main/deploy/VelocityCorrectionData.csv | 12 ++++++ src/main/java/frc4388/robot/Constants.java | 2 +- .../robot/commands/ShooterCommands/Shoot.java | 39 +------------------ .../frc4388/robot/subsystems/BoomBoom.java | 13 +------ 5 files changed, 27 insertions(+), 63 deletions(-) create mode 100644 src/main/deploy/VelocityCorrectionData.csv diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index c11dc0f..ccef197 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -1,12 +1,12 @@ -Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Duration (s), -0 ,-29.5 ,8000 ,0, -78.5 ,-29.5 ,8000 ,0, -99 ,-39.62 ,8500 ,0, -127.25 ,-49.12 ,9500 ,0, -150 ,-66.22 ,10000 ,0, -164.5 ,-75.52 ,10000 ,0, -186 ,-76.24 ,10000 ,0, -207 ,-104.07 ,11000 ,0, -227 ,-105.32 ,11500 ,0, -255.5 ,-105.8 ,13500 ,0, -999 ,-105.8 ,13500 ,0, \ No newline at end of file +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) +0 ,-29.5 ,8000 +78.5 ,-29.5 ,8000 +99 ,-39.62 ,8500 +127.25 ,-49.12 ,9500 +150 ,-66.22 ,10000 +164.5 ,-75.52 ,10000 +186 ,-76.24 ,10000 +207 ,-104.07 ,11000 +227 ,-105.32 ,11500 +255.5 ,-105.8 ,13500 +999 ,-105.8 ,13500 \ No newline at end of file diff --git a/src/main/deploy/VelocityCorrectionData.csv b/src/main/deploy/VelocityCorrectionData.csv new file mode 100644 index 0000000..21d13a2 --- /dev/null +++ b/src/main/deploy/VelocityCorrectionData.csv @@ -0,0 +1,12 @@ +Distance (in) ,Duration (s) +0 ,0 +78.5 ,0 +99 ,0 +127.25 ,0 +150 ,0 +164.5 ,0 +186 ,0 +207 ,0 +227 ,0 +255.5 ,0 +999 ,0 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2f43844..17d6f24 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -210,7 +210,7 @@ public final class Constants { public static final double TURRET_FORWARD_LIMIT = 0.0; public static final double TURRET_REVERSE_LIMIT = -95.0; //Find //Shooter gains for actual Drum - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); // TODO: tune values + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 20; diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 94bb04f..d39f4ce 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -96,7 +96,7 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - m_targetPoint = new Pose2d(hTargetDistanceFromHub(), vTargetDistanceFromHub(), SwerveDriveConstants.HUB_POSE.getRotation()); + m_targetPoint = SwerveDriveConstants.HUB_POSE; m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); error = (m_targetAngle - turretDummy.get() + 360) % 360; @@ -152,43 +152,6 @@ public class Shoot extends CommandBase { output = kP * proportional + kI * integral + kD * derivative; normOutput = output/360 * inverted; } - // TODO: horizontal velocity correction - public double hTargetDistanceFromHub() { - - double hVel = m_swerve.getChassisSpeeds()[0]; - double velBeforeCorrection = m_boomBoom.getVelocity(m_distance); - double vDistanceFromHub = m_odoY; - double approxTravelTime = vDistanceFromHub / velBeforeCorrection; - double hTargetDistanceFromHub = hVel * approxTravelTime; - - // return hTargetDistanceFromHub; - return 0.0; // this is for no velocity correction - } - - public Pose2d findTargetPoint() { - - // position vector and radius - Translation2d position = new Translation2d(m_odoX, m_odoY); - double radius = position.getNorm(); - - // equation of circle = x^2 + y^2 = m_distance^2 - // derivative of circle = 2x + 2y * y' = 0 --> y' = -x/y - - // velocity vector (x, y) - Translation2d cartesianVelocity = new Translation2d(m_swerve.getChassisSpeeds()[0], m_swerve.getChassisSpeeds()[1]); - - // unit tangential vector - Translation2d tangential = new Translation2d(0, 0); - // velocity vector (tangential, radial) - Translation2d polarVelocity = new Translation2d(0, 0); - - return SwerveDriveConstants.HUB_POSE; - } - - // TODO: vertical velocity correction - public double vTargetDistanceFromHub() { - return 0.0; // this is for no velocity correction - } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 9e7d4b8..c81fd38 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -48,7 +48,7 @@ public class BoomBoom extends SubsystemBase { // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later public static class ShooterTableEntry { - public Double distance, hoodExt, drumVelocity, duration; + public Double distance, hoodExt, drumVelocity; } private ShooterTableEntry[] m_shooterTable; @@ -116,17 +116,6 @@ public class BoomBoom extends SubsystemBase { return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); } - /** - * This is a function that takes a value (distance) and returns a value (duration) that is a linear - * interpolation of the two values (duration) at the two closest points in the table (m_shooterTable) - * to the given value (distance). - * @param distance Distance in shooter table - * @return Shot duration in seconds - */ - public Double getDuration(final Double distance) { - return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.duration).doubleValue(); - } - /** * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the 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 098/180] 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 627812e4fc0e72810757f656cccb3bec8800abde Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Wed, 16 Mar 2022 18:58:58 -0600 Subject: [PATCH 099/180] Added point filtering + added AaraVector2D --- src/main/java/frc4388/robot/Constants.java | 2 + .../commands/ShooterCommands/TrackTarget.java | 18 ++++- src/main/java/frc4388/utility/Vector2D.java | 67 +++++++++++++++++-- 3 files changed, 79 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 17d6f24..aa5b0c8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -241,6 +241,8 @@ public final class Constants { public static final double LIME_VIXELS = 720; public static final double TURRET_kP = 0.1; + public static final double POINTS_THRESHOLD = 400; + public static final double RANGE = 10; public static final double EDGE_TO_CENTER = 20; diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index b313fa8..bf524e6 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -19,6 +19,7 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.VisionOdometry; +import frc4388.utility.Vector2D; import frc4388.utility.desmos.DesmosServer; public class TrackTarget extends CommandBase { @@ -69,13 +70,24 @@ public class TrackTarget extends CommandBase { try { m_visionOdometry.setLEDs(true); points = m_visionOdometry.getTargetPoints(); + + Point average = VisionOdometry.averagePoint(points); + + for(Point point : points) { + Vector2D difference = new Vector2D(point); + difference.subtract(new Vector2D(average)); + + if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD) + points.remove(point); + } + + average = VisionOdometry.averagePoint(points); + DesmosServer.putPoint("average", average); + for(int i = 0; i < points.size(); i++) { DesmosServer.putPoint("Point" + i, points.get(i)); } - Point average = VisionOdometry.averagePoint(points); - DesmosServer.putPoint("average", average); - output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 4; // output *= 0.5; diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index 8ce1968..cc04125 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -4,10 +4,16 @@ package frc4388.utility; +import java.security.InvalidParameterException; + +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 { public double x; @@ -15,8 +21,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) { @@ -27,6 +32,22 @@ public class Vector2D extends Vector2d { this.angle = Math.atan2(this.y, this.x); } + public Vector2D(double[] vec) { + this(vec[0], vec[1]); + + if (vec.length != 2) { + throw new InvalidParameterException(); + } + } + + public Vector2D(Point p) { + this(p.x, p.y); + } + + public Vector2D(Translation2d t) { + this(t.getX(), t.getY()); + } + /** * Add two vectors, component-wise. * @param v1 First vector in the addition. @@ -37,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. @@ -47,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. @@ -57,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. @@ -67,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. @@ -96,7 +153,7 @@ public class Vector2D extends Vector2d { @Override public String toString() { - return ("(" + this.x + ", " + this.y + ")"); + return "<" + this.x + ", " + this.y + ">"; } -} +} \ No newline at end of file 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 100/180] 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 411e06211c79462e58f2f1307ff0d258d21949e9 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Wed, 16 Mar 2022 21:18:10 -0600 Subject: [PATCH 101/180] lots of weird turret limit shtuff, NOT COMPLETE --- src/main/java/frc4388/robot/Constants.java | 11 ++- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 8 +-- .../commands/ShooterCommands/AimToCenter.java | 2 +- .../frc4388/robot/subsystems/Extender.java | 12 ++-- .../java/frc4388/robot/subsystems/Turret.java | 72 +++++++++++++++---- 6 files changed, 80 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 17d6f24..2ef6506 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -207,8 +207,15 @@ public final class Constants { public static final double SHOOTER_TURRET_MIN = -TURRET_SPEED_MULTIPLIER; //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); - public static final double TURRET_FORWARD_LIMIT = 0.0; - public static final double TURRET_REVERSE_LIMIT = -95.0; //Find + + //#region test start + + //#endregion test end + public static final double TURRET_FORWARD_HARD_LIMIT = 0.0; + public static final double TURRET_REVERSE_HARD_LIMIT = -85.0; + + public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2; + public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index fbb7cfa..3704dd7 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -204,7 +204,7 @@ public class Robot extends TimedRobot { LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } - m_robotContainer.m_robotVisionOdometry.setLEDs(true); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ebd9715..d9b8e08 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -166,10 +166,10 @@ public class RobotContainer { getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotStorage.setDefaultCommand( - new ManageStorage(m_robotStorage, - m_robotBoomBoom, - m_robotTurret).withName("Storage ManageStorage defaultCommand")); + // m_robotStorage.setDefaultCommand( + // new ManageStorage(m_robotStorage, + // m_robotBoomBoom, + // m_robotTurret).withName("Storage ManageStorage defaultCommand")); // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 7f8a8db..38a8888 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -69,7 +69,7 @@ public class AimToCenter extends CommandBase { if (angle == Double.NaN) { return false; } - return !((ShooterConstants.TURRET_REVERSE_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_LIMIT)); + return !((ShooterConstants.TURRET_REVERSE_SOFT_LIMIT <= angle) && (angle <= ShooterConstants.TURRET_FORWARD_SOFT_LIMIT)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 34cd555..001208f 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -17,18 +17,18 @@ public class Extender extends SubsystemBase { private CANSparkMax m_extenderMotor; - private SparkMaxLimitSwitch m_inLimit; - private SparkMaxLimitSwitch m_outLimit; + // private SparkMaxLimitSwitch m_inLimit; + // private SparkMaxLimitSwitch m_outLimit; /** Creates a new Extender. */ public Extender(CANSparkMax extenderMotor) { m_extenderMotor = extenderMotor; - m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_inLimit.enableLimitSwitch(false); - m_outLimit.enableLimitSwitch(false); + // m_inLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_outLimit = m_extenderMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_inLimit.enableLimitSwitch(false); + // m_outLimit.enableLimitSwitch(false); m_extenderMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ExtenderConstants.EXTENDER_FORWARD_LIMIT); m_extenderMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ExtenderConstants.EXTENDER_REVERSE_LIMIT); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index d2dc34f..87e74d9 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -37,23 +37,35 @@ public class Turret extends SubsystemBase { SparkMaxLimitSwitch m_boomBoomLeftLimit; SparkMaxLimitSwitch m_boomBoomRightLimit; + boolean hasLeftSwitchChanged = false; + boolean hasRightSwitchChanged = false; + + boolean leftPrevState = false; + boolean rightPrevState = false; + boolean leftState; + boolean rightState; + + long leftCurrentTime; + long rightCurrentTime; + long leftElapsedTime; + long rightElapsedTime; + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_boomBoomLeftLimit.enableLimitSwitch(true); + // m_boomBoomRightLimit.enableLimitSwitch(true); setTurretLimitSwitches(true); - // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_LIMIT); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); setTurretSoftLimits(true); - + setTurretPIDGains(); } @@ -68,19 +80,53 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone); m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput); } - + @Override public void periodic() { // This method will be called once per scheduler run - + + SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); - if (m_boomBoomLeftLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_LIMIT - 2); - if (m_boomBoomRightLimit.isPressed()) m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_LIMIT + 2); + // limit switch annoying time thing + leftState = m_boomBoomLeftLimit.isPressed(); + rightState = m_boomBoomRightLimit.isPressed(); + + hasLeftSwitchChanged = (leftState != leftPrevState); + hasRightSwitchChanged = (rightState != rightPrevState); + + if (leftState && hasLeftSwitchChanged) { + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } + + if (rightState && hasRightSwitchChanged) { + rightCurrentTime = System.currentTimeMillis(); + rightElapsedTime = 0; + } + + if (leftState && !hasLeftSwitchChanged) { + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (rightState && !hasRightSwitchChanged) { + rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/); + } + if (rightState && (rightElapsedTime > 500)) { + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + } + + leftPrevState = leftState; + rightPrevState = rightState; } /** @@ -110,7 +156,7 @@ public class Turret extends SubsystemBase { * @param input from -1.0 to 1.0, positive is clockwise */ public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * 0.5); } public void runShooterRotatePID(double targetAngle) { From c972577066cd93d84f50ecae0b239ba5cac5cfb9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 21:20:18 -0600 Subject: [PATCH 102/180] turret manual boilerplate NOT COMPLETE --- .../java/frc4388/robot/RobotContainer.java | 25 ++++---- .../ButtonBoxCommands/TurretManual.java | 62 +++++++++++++++++++ 2 files changed, 73 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ebd9715..ce9f0c1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -63,6 +63,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; +import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Shoot; @@ -137,8 +138,6 @@ public class RobotContainer { private static final Function PATH_EXTENSION_REMOVER = ((Function) Pattern .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); - - public boolean manual = false; public static boolean softLimits = true; /** @@ -166,10 +165,10 @@ public class RobotContainer { getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotStorage.setDefaultCommand( - new ManageStorage(m_robotStorage, - m_robotBoomBoom, - m_robotTurret).withName("Storage ManageStorage defaultCommand")); + // m_robotStorage.setDefaultCommand( + // new ManageStorage(m_robotStorage, + // m_robotBoomBoom, + // m_robotTurret).withName("Storage ManageStorage defaultCommand")); // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) @@ -352,11 +351,13 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) // .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - // .whileHeld(new RunMiddleSwitch()); + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + .whileHeld(new TurretManual(m_robotTurret)); - new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whileHeld(new RunCommand(() -> System.out.println("RightSwitch"))); + // control turret manual mode + // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) + // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) @@ -407,10 +408,6 @@ public class RobotContainer { return m_buttonBox; } - public void setManual(boolean set) { - this.manual = set; - } - public static void setSoftLimits(boolean set) { softLimits = set; } diff --git a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java new file mode 100644 index 0000000..53d772a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java @@ -0,0 +1,62 @@ +// 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.ButtonBoxCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Turret; + +public class TurretManual extends CommandBase { + + // subsystems + private Turret turret; + + // booleans + private static boolean manual = false; + private boolean newManual = false; + private boolean changes = false; + + /** Creates a new TurretManual. */ + public TurretManual(Turret turret) { + // Use addRequirements() here to declare subsystem dependencies. + this.turret = turret; + + addRequirements(this.turret); + } + + // 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() { + changes = (newManual != manual); + + if (manual) { + System.out.println("Manual Turret"); // TODO: turret manual controls + } else { + System.out.println("Auto Turret"); // TODO: turret auto controls; + } + + newManual = manual; + } + + public static void setManual(boolean set) + { + manual = set; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + manual = !manual; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return changes; + } +} From d5ccd5e46ebc61be25a07c1d0826c601daba61e8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 22:32:57 -0600 Subject: [PATCH 103/180] 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 ebd78f2543d1017f45094cab112c34510c9442a6 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 23:44:20 -0600 Subject: [PATCH 104/180] weird ass limit stuff shtuff (IDK WHY IT WORKS) --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 4 + .../java/frc4388/robot/subsystems/Turret.java | 97 ++++++++++++------- 3 files changed, 66 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5e559e0..f87a507 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -191,6 +191,7 @@ public final class Constants { public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; public static final double TURRET_SPEED_MULTIPLIER = 0.6; + public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5; public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; @@ -212,7 +213,7 @@ public final class Constants { //#endregion test end public static final double TURRET_FORWARD_HARD_LIMIT = 0.0; - public static final double TURRET_REVERSE_HARD_LIMIT = -85.0; + public static final double TURRET_REVERSE_HARD_LIMIT = -123.0; public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ce9f0c1..d5967b9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -359,6 +359,10 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whileHeld(new RunCommand(() -> m_robotTurret.turnOnLeftLimitSwitch(), m_robotTurret)) + .whenReleased(new RunCommand(() -> m_robotTurret.turnOffLeftLimitSwitch(), m_robotTurret)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 87e74d9..f94b44d 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -56,19 +56,42 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - m_boomBoomLeftLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomLeftLimit.enableLimitSwitch(true); - // m_boomBoomRightLimit.enableLimitSwitch(true); - setTurretLimitSwitches(true); + // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_boomBoomRightLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_boomBoomLeftLimit.enableLimitSwitch(false); + // m_boomBoomRightLimit.enableLimitSwitch(false); + // setTurretLimitSwitches(true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - setTurretSoftLimits(true); + setTurretSoftLimits(false); setTurretPIDGains(); } + + public void toggleLeftLimitSwitch() { + // TODO: find better way to do this, but im in a hurry + + + // if (leftSwitch.isLimitSwitchEnabled()) { + // leftSwitch.enableLimitSwitch(false); + // } else { + // leftSwitch.enableLimitSwitch(true); + } + // } + public void turnOnLeftLimitSwitch() { + SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + System.out.println("Left Switch ENABLED"); + leftSwitch.enableLimitSwitch(true); + } + + public void turnOffLeftLimitSwitch() { + SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + System.out.println("Left Switch DISABLED"); + leftSwitch.enableLimitSwitch(false); + } + /** * Set gains for turret PIDs. */ @@ -85,48 +108,48 @@ public class Turret extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); - SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); - SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + // SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); + // SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); - // limit switch annoying time thing - leftState = m_boomBoomLeftLimit.isPressed(); - rightState = m_boomBoomRightLimit.isPressed(); + // limit switch annoying time thing but actually worked first try wtf + // leftState = m_boomBoomLeftLimit.isPressed(); + // rightState = m_boomBoomRightLimit.isPressed(); - hasLeftSwitchChanged = (leftState != leftPrevState); - hasRightSwitchChanged = (rightState != rightPrevState); + // hasLeftSwitchChanged = (leftState != leftPrevState); + // hasRightSwitchChanged = (rightState != rightPrevState); - if (leftState && hasLeftSwitchChanged) { - leftCurrentTime = System.currentTimeMillis(); - leftElapsedTime = 0; - } + // if (leftState && hasLeftSwitchChanged) { + // leftCurrentTime = System.currentTimeMillis(); + // leftElapsedTime = 0; + // } - if (rightState && hasRightSwitchChanged) { - rightCurrentTime = System.currentTimeMillis(); - rightElapsedTime = 0; - } + // if (rightState && hasRightSwitchChanged) { + // rightCurrentTime = System.currentTimeMillis(); + // rightElapsedTime = 0; + // } - if (leftState && !hasLeftSwitchChanged) { - leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - } + // if (leftState && !hasLeftSwitchChanged) { + // leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + // } - if (rightState && !hasRightSwitchChanged) { - rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; - } + // if (rightState && !hasRightSwitchChanged) { + // rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; + // } - if (leftState && (leftElapsedTime > 500)) { - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/); - } - if (rightState && (rightElapsedTime > 500)) { - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); - } + // if (leftState && (leftElapsedTime > 500)) { + // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/); + // } + // if (rightState && (rightElapsedTime > 500)) { + // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + // } - leftPrevState = leftState; - rightPrevState = rightState; + // leftPrevState = leftState; + // rightPrevState = rightState; } /** From 87a6707cc636ec5de2749b86024b13dd7e28c908 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:05:43 -0600 Subject: [PATCH 105/180] docs for weird timed switch thing --- .../java/frc4388/robot/subsystems/Turret.java | 53 ++++++------------- 1 file changed, 16 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index f94b44d..f91a5ec 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -37,18 +37,12 @@ public class Turret extends SubsystemBase { SparkMaxLimitSwitch m_boomBoomLeftLimit; SparkMaxLimitSwitch m_boomBoomRightLimit; - boolean hasLeftSwitchChanged = false; - boolean hasRightSwitchChanged = false; - - boolean leftPrevState = false; - boolean rightPrevState = false; boolean leftState; - boolean rightState; + boolean leftPrevState = false; + boolean hasLeftSwitchChanged = false; long leftCurrentTime; - long rightCurrentTime; long leftElapsedTime; - long rightElapsedTime; public Turret(CANSparkMax boomBoomRotateMotor) { @@ -117,39 +111,24 @@ public class Turret extends SubsystemBase { // SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); // limit switch annoying time thing but actually worked first try wtf - // leftState = m_boomBoomLeftLimit.isPressed(); - // rightState = m_boomBoomRightLimit.isPressed(); + leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). - // hasLeftSwitchChanged = (leftState != leftPrevState); - // hasRightSwitchChanged = (rightState != rightPrevState); + hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. - // if (leftState && hasLeftSwitchChanged) { - // leftCurrentTime = System.currentTimeMillis(); - // leftElapsedTime = 0; - // } + if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } + + if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } - // if (rightState && hasRightSwitchChanged) { - // rightCurrentTime = System.currentTimeMillis(); - // rightElapsedTime = 0; - // } + if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } - // if (leftState && !hasLeftSwitchChanged) { - // leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - // } - - // if (rightState && !hasRightSwitchChanged) { - // rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; - // } - - // if (leftState && (leftElapsedTime > 500)) { - // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/); - // } - // if (rightState && (rightElapsedTime > 500)) { - // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); - // } - - // leftPrevState = leftState; - // rightPrevState = rightState; + leftPrevState = leftState; // * Update the state of the left limit switch. } /** From 2c18973dfaad0ffea193804a89c5c79810fed3f9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:18:08 -0600 Subject: [PATCH 106/180] fixed --- src/main/java/frc4388/robot/Constants.java | 6 +- src/main/java/frc4388/robot/Robot.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 6 +- .../java/frc4388/robot/subsystems/Turret.java | 94 +++++++++---------- 4 files changed, 54 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f87a507..0273228 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -190,7 +190,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.6; + public static final double TURRET_SPEED_MULTIPLIER = 0.8; public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5; public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; @@ -212,8 +212,8 @@ public final class Constants { //#region test start //#endregion test end - public static final double TURRET_FORWARD_HARD_LIMIT = 0.0; - public static final double TURRET_REVERSE_HARD_LIMIT = -123.0; + public static final double TURRET_FORWARD_HARD_LIMIT = 18.4; + public static final double TURRET_REVERSE_HARD_LIMIT = -104.3; public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 3704dd7..1d2b91f 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -142,15 +142,15 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); current = - // m_robotContainer.m_robotBoomBoom.getCurrent() + + m_robotContainer.m_robotBoomBoom.getCurrent() + m_robotContainer.m_robotClimber.getCurrent() + - // m_robotContainer.m_robotHood.getCurrent() + + m_robotContainer.m_robotHood.getCurrent() + m_robotContainer.m_robotIntake.getCurrent() + m_robotContainer.m_robotExtender.getCurrent() + m_robotContainer.m_robotSerializer.getCurrent() + m_robotContainer.m_robotStorage.getCurrent() + m_robotContainer.m_robotSwerveDrive.getCurrent(); - // m_robotContainer.m_robotTurret.getCurrent(); + m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d5967b9..7253a31 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -359,9 +359,9 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); - new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whileHeld(new RunCommand(() -> m_robotTurret.turnOnLeftLimitSwitch(), m_robotTurret)) - .whenReleased(new RunCommand(() -> m_robotTurret.turnOffLeftLimitSwitch(), m_robotTurret)); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + // .whileHeld(new RunCommand(() -> m_robotTurret.turnOnLeftLimitSwitch(), m_robotTurret)) + // .whenReleased(new RunCommand(() -> m_robotTurret.turnOffLeftLimitSwitch(), m_robotTurret)); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index f94b44d..1d01271 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -44,6 +44,7 @@ public class Turret extends SubsystemBase { boolean rightPrevState = false; boolean leftState; boolean rightState; + boolean recentlyPressed; long leftCurrentTime; long rightCurrentTime; @@ -56,20 +57,22 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - // m_boomBoomLeftLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomRightLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_boomBoomLeftLimit.enableLimitSwitch(false); - // m_boomBoomRightLimit.enableLimitSwitch(false); - // setTurretLimitSwitches(true); - + // setTurretSoftLimits(true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - setTurretSoftLimits(false); + + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // setTurretLimitSwitches(true); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); setTurretPIDGains(); } - public void toggleLeftLimitSwitch() { + // public void toggleLeftLimitSwitch() { // TODO: find better way to do this, but im in a hurry @@ -77,20 +80,20 @@ public class Turret extends SubsystemBase { // leftSwitch.enableLimitSwitch(false); // } else { // leftSwitch.enableLimitSwitch(true); - } + // } // } - public void turnOnLeftLimitSwitch() { - SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - System.out.println("Left Switch ENABLED"); - leftSwitch.enableLimitSwitch(true); - } + // public void turnOnLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch ENABLED"); + // leftSwitch.enableLimitSwitch(true); + // } - public void turnOffLeftLimitSwitch() { - SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - System.out.println("Left Switch DISABLED"); - leftSwitch.enableLimitSwitch(false); - } + // public void turnOffLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch DISABLED"); + // leftSwitch.enableLimitSwitch(false); + // } /** * Set gains for turret PIDs. @@ -113,42 +116,35 @@ public class Turret extends SubsystemBase { SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); - // SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); - // SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); + SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); // limit switch annoying time thing but actually worked first try wtf - // leftState = m_boomBoomLeftLimit.isPressed(); - // rightState = m_boomBoomRightLimit.isPressed(); + leftState = m_boomBoomLeftLimit.isPressed(); - // hasLeftSwitchChanged = (leftState != leftPrevState); - // hasRightSwitchChanged = (rightState != rightPrevState); + hasLeftSwitchChanged = (leftState != leftPrevState); - // if (leftState && hasLeftSwitchChanged) { - // leftCurrentTime = System.currentTimeMillis(); - // leftElapsedTime = 0; - // } - - // if (rightState && hasRightSwitchChanged) { - // rightCurrentTime = System.currentTimeMillis(); - // rightElapsedTime = 0; - // } + if (leftState && hasLeftSwitchChanged) { + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } - // if (leftState && !hasLeftSwitchChanged) { - // leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - // } - - // if (rightState && !hasRightSwitchChanged) { - // rightElapsedTime = System.currentTimeMillis() - rightCurrentTime; - // } + if (leftState && !hasLeftSwitchChanged) { + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } + if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; - // if (leftState && (leftElapsedTime > 500)) { - // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// -95/*ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - 2*/); - // } - // if (rightState && (rightElapsedTime > 500)) { - // m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); - // } + if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ + recentlyPressed = true; + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + } + SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); - // leftPrevState = leftState; + leftPrevState = leftState; // rightPrevState = rightState; } @@ -179,7 +175,7 @@ public class Turret extends SubsystemBase { * @param input from -1.0 to 1.0, positive is clockwise */ public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * 0.5); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); } public void runShooterRotatePID(double targetAngle) { From 23a208fbe3f058aa9f87639b14fcb348a8a35523 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:21:29 -0600 Subject: [PATCH 107/180] idk --- .../frc4388/robot/subsystems/Turret.java.bak | 234 ++++++++++++++++++ 1 file changed, 234 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/Turret.java.bak diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java.bak b/src/main/java/frc4388/robot/subsystems/Turret.java.bak new file mode 100644 index 0000000..e98d8b6 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Turret.java.bak @@ -0,0 +1,234 @@ +// 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 com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxPIDController; + +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.utility.Gains; + +public class Turret extends SubsystemBase { + + /** Creates a new Turret. */ + public BoomBoom m_boomBoomSubsystem; + public SwerveDrive m_sDriveSubsystem; + + public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, + // MotorType.kBrushless); + public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; + + public SparkMaxPIDController m_boomBoomRotatePIDController; + public RelativeEncoder m_boomBoomRotateEncoder; + + SparkMaxLimitSwitch m_boomBoomLeftLimit; + SparkMaxLimitSwitch m_boomBoomRightLimit; + + boolean leftState; +<<<<<<< HEAD + boolean rightState; + boolean recentlyPressed; +======= + boolean leftPrevState = false; + boolean hasLeftSwitchChanged = false; +>>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 + + long leftCurrentTime; + long leftElapsedTime; + + public Turret(CANSparkMax boomBoomRotateMotor) { + + m_boomBoomRotateMotor = boomBoomRotateMotor; + m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + + // setTurretSoftLimits(true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); + + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // setTurretLimitSwitches(true); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); + + setTurretPIDGains(); + } + + // public void toggleLeftLimitSwitch() { + // TODO: find better way to do this, but im in a hurry + + + // if (leftSwitch.isLimitSwitchEnabled()) { + // leftSwitch.enableLimitSwitch(false); + // } else { + // leftSwitch.enableLimitSwitch(true); + // } + // } + + // public void turnOnLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch ENABLED"); + // leftSwitch.enableLimitSwitch(true); + // } + + // public void turnOffLeftLimitSwitch() { + // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // System.out.println("Left Switch DISABLED"); + // leftSwitch.enableLimitSwitch(false); + // } + + /** + * Set gains for turret PIDs. + */ + public void setTurretPIDGains() { + m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); + + SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); + SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); + SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); + + // limit switch annoying time thing but actually worked first try wtf +<<<<<<< HEAD + leftState = m_boomBoomLeftLimit.isPressed(); + + hasLeftSwitchChanged = (leftState != leftPrevState); + + if (leftState && hasLeftSwitchChanged) { +======= + leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). + + hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. + + if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. +>>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 + leftCurrentTime = System.currentTimeMillis(); + leftElapsedTime = 0; + } + +<<<<<<< HEAD + if (leftState && !hasLeftSwitchChanged) { + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } + if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; + + if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ + recentlyPressed = true; + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); + } + SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); + + leftPrevState = leftState; + // rightPrevState = rightState; +======= + if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. + leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; + } + + if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. + m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); + } + + leftPrevState = leftState; // * Update the state of the left limit switch. +>>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 + } + + /** + * Set status of turret motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setTurretSoftLimits(boolean set) { + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + + /** + * Set status of turret limit switches. + * @param set Boolean to set limit switches to. + */ + public void setTurretLimitSwitches(boolean set) { + m_boomBoomRightLimit.enableLimitSwitch(set); + m_boomBoomLeftLimit.enableLimitSwitch(set); + } + + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { + m_boomBoomSubsystem = subsystem0; + m_sDriveSubsystem = subsystem1; + } + /** + * Move the turret with an input + * @param input from -1.0 to 1.0, positive is clockwise + */ + public void runTurretWithInput(double input) { + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + } + + public void runShooterRotatePID(double targetAngle) { + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; + m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void resetGyroShooterRotate() { + m_boomBoomRotateEncoder.setPosition(0); + } + + /** + * Run a PID to go to the zero position. + */ + public void gotoZero() { + runShooterRotatePID(0); + } + + /** + * Run a PID to go to the midpoint position, between the two soft limits. + */ + public void gotoMidpoint() { + runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + public double getEncoderPosition() { + return m_boomBoomRotateEncoder.getPosition(); + } + + public double getBoomBoomAngleDegrees() { + return (getEncoderPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); + } + + public double getCurrent(){ + return m_boomBoomRotateMotor.getOutputCurrent(); + } + +} \ No newline at end of file From fc3a61f75c2c089ab1c426fda4b83c4986497264 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:21:55 -0600 Subject: [PATCH 108/180] ik --- .../frc4388/robot/subsystems/Turret.java.bak | 234 ------------------ 1 file changed, 234 deletions(-) delete mode 100644 src/main/java/frc4388/robot/subsystems/Turret.java.bak diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java.bak b/src/main/java/frc4388/robot/subsystems/Turret.java.bak deleted file mode 100644 index e98d8b6..0000000 --- a/src/main/java/frc4388/robot/subsystems/Turret.java.bak +++ /dev/null @@ -1,234 +0,0 @@ -// 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 com.revrobotics.CANSparkMax.ControlType; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxLimitSwitch; -import com.revrobotics.SparkMaxPIDController; - -import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.commands.ShooterCommands.Shoot; -import frc4388.utility.Gains; - -public class Turret extends SubsystemBase { - - /** Creates a new Turret. */ - public BoomBoom m_boomBoomSubsystem; - public SwerveDrive m_sDriveSubsystem; - - public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, - // MotorType.kBrushless); - public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; - - public SparkMaxPIDController m_boomBoomRotatePIDController; - public RelativeEncoder m_boomBoomRotateEncoder; - - SparkMaxLimitSwitch m_boomBoomLeftLimit; - SparkMaxLimitSwitch m_boomBoomRightLimit; - - boolean leftState; -<<<<<<< HEAD - boolean rightState; - boolean recentlyPressed; -======= - boolean leftPrevState = false; - boolean hasLeftSwitchChanged = false; ->>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 - - long leftCurrentTime; - long leftElapsedTime; - - public Turret(CANSparkMax boomBoomRotateMotor) { - - m_boomBoomRotateMotor = boomBoomRotateMotor; - m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); - m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); - - // setTurretSoftLimits(true); - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - - m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // setTurretLimitSwitches(true); - m_boomBoomRightLimit.enableLimitSwitch(true); - m_boomBoomLeftLimit.enableLimitSwitch(true); - - setTurretPIDGains(); - } - - // public void toggleLeftLimitSwitch() { - // TODO: find better way to do this, but im in a hurry - - - // if (leftSwitch.isLimitSwitchEnabled()) { - // leftSwitch.enableLimitSwitch(false); - // } else { - // leftSwitch.enableLimitSwitch(true); - // } - // } - - // public void turnOnLeftLimitSwitch() { - // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // System.out.println("Left Switch ENABLED"); - // leftSwitch.enableLimitSwitch(true); - // } - - // public void turnOffLeftLimitSwitch() { - // SparkMaxLimitSwitch leftSwitch = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // System.out.println("Left Switch DISABLED"); - // leftSwitch.enableLimitSwitch(false); - // } - - /** - * Set gains for turret PIDs. - */ - public void setTurretPIDGains() { - m_boomBoomRotatePIDController.setP(m_shooterTGains.kP); - m_boomBoomRotatePIDController.setI(m_shooterTGains.kI); - m_boomBoomRotatePIDController.setD(m_shooterTGains.kD); - m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF); - m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone); - m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - // SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); - // SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - - SmartDashboard.putNumber("Turret Angle Rotations", m_boomBoomRotateEncoder.getPosition()); - SmartDashboard.putNumber("Turret Angle Degrees", m_boomBoomRotateEncoder.getPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); - SmartDashboard.putBoolean("Left Limit Switch Pressed", m_boomBoomLeftLimit.isPressed()); - SmartDashboard.putBoolean("Right Limit Switch Pressed", m_boomBoomRightLimit.isPressed()); - - // limit switch annoying time thing but actually worked first try wtf -<<<<<<< HEAD - leftState = m_boomBoomLeftLimit.isPressed(); - - hasLeftSwitchChanged = (leftState != leftPrevState); - - if (leftState && hasLeftSwitchChanged) { -======= - leftState = m_boomBoomLeftLimit.isPressed(); // * Get the state of the left limit switch (true for pressed). - - hasLeftSwitchChanged = (leftState != leftPrevState); // * Get whether the state of the left limit switch has changed, based on its previous state. - - if (leftState && hasLeftSwitchChanged) { // * If the left limit switch is pressed, and it recently changed, start the time. ->>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 - leftCurrentTime = System.currentTimeMillis(); - leftElapsedTime = 0; - } - -<<<<<<< HEAD - if (leftState && !hasLeftSwitchChanged) { - leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - } - - if (leftState && (leftElapsedTime > 500)) { - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); - } - if (!m_boomBoomRightLimit.isPressed()) recentlyPressed = false; - - if(m_boomBoomRightLimit.isPressed() && !recentlyPressed){ - recentlyPressed = true; - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_FORWARD_HARD_LIMIT);// 0/*ShooterConstants.TURRET_REVERSE_LIMIT + 2*/); - } - SmartDashboard.putBoolean("Recently Pressed", recentlyPressed); - - leftPrevState = leftState; - // rightPrevState = rightState; -======= - if (leftState && !hasLeftSwitchChanged) { // * If the left limit switch is still pressed, but the state hasn't changed, then calculate elapsed time. - leftElapsedTime = System.currentTimeMillis() - leftCurrentTime; - } - - if (leftState && (leftElapsedTime > 500)) { // * If the left limit switch is pressed for more than half a second, update the encoder position. - m_boomBoomRotateEncoder.setPosition(ShooterConstants.TURRET_REVERSE_HARD_LIMIT); - } - - leftPrevState = leftState; // * Update the state of the left limit switch. ->>>>>>> 87a6707cc636ec5de2749b86024b13dd7e28c908 - } - - /** - * Set status of turret motor soft limits. - * @param set Boolean to set soft limits to. - */ - public void setTurretSoftLimits(boolean set) { - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); - } - - /** - * Set status of turret limit switches. - * @param set Boolean to set limit switches to. - */ - public void setTurretLimitSwitches(boolean set) { - m_boomBoomRightLimit.enableLimitSwitch(set); - m_boomBoomLeftLimit.enableLimitSwitch(set); - } - - public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { - m_boomBoomSubsystem = subsystem0; - m_sDriveSubsystem = subsystem1; - } - /** - * Move the turret with an input - * @param input from -1.0 to 1.0, positive is clockwise - */ - public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); - } - - public void runShooterRotatePID(double targetAngle) { - targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; - m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); - } - - public void resetGyroShooterRotate() { - m_boomBoomRotateEncoder.setPosition(0); - } - - /** - * Run a PID to go to the zero position. - */ - public void gotoZero() { - runShooterRotatePID(0); - } - - /** - * Run a PID to go to the midpoint position, between the two soft limits. - */ - public void gotoMidpoint() { - runShooterRotatePID(-44 * ShooterConstants.TURRET_DEGREES_PER_ROT); - } - - public double getEncoderPosition() { - return m_boomBoomRotateEncoder.getPosition(); - } - - public double getBoomBoomAngleDegrees() { - return (getEncoderPosition() * ShooterConstants.TURRET_DEGREES_PER_ROT); - } - - public double getCurrent(){ - return m_boomBoomRotateMotor.getOutputCurrent(); - } - -} \ No newline at end of file From 25426d7f330423fe04f3386eed8e4baabf0d0ecc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 11:54:05 -0600 Subject: [PATCH 109/180] turret soft + hard limits + zero WORKS --- src/main/java/frc4388/robot/Constants.java | 10 ++++----- .../java/frc4388/robot/RobotContainer.java | 4 +++- .../java/frc4388/robot/subsystems/Turret.java | 22 +++++++++++++++++-- 3 files changed, 28 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0273228..cb10859 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -204,18 +204,18 @@ public final class Constants { // ID public static final int TURRET_MOTOR_CAN_ID = 19; //Gains for turret - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, TURRET_SPEED_MULTIPLIER); - public static final double SHOOTER_TURRET_MIN = -TURRET_SPEED_MULTIPLIER; + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.1, 0.0, 0.1, 0.0, 0, 0.3/*TURRET_SPEED_MULTIPLIER*/); + public static final double SHOOTER_TURRET_MIN = -0.3;//-TURRET_SPEED_MULTIPLIER; //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); //#region test start //#endregion test end - public static final double TURRET_FORWARD_HARD_LIMIT = 18.4; - public static final double TURRET_REVERSE_HARD_LIMIT = -104.3; + public static final double TURRET_FORWARD_HARD_LIMIT = 18.429; + public static final double TURRET_REVERSE_HARD_LIMIT = -106.454; - public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 2; + public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 4; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7253a31..80aae83 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -303,7 +303,9 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom)); + // .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));\ + .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) + .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom)); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index cc96886..4c04332 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -45,6 +45,8 @@ public class Turret extends SubsystemBase { long leftCurrentTime; long leftElapsedTime; + double speedLimiter; + public Turret(CANSparkMax boomBoomRotateMotor) { m_boomBoomRotateMotor = boomBoomRotateMotor; @@ -59,9 +61,10 @@ public class Turret extends SubsystemBase { m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // setTurretLimitSwitches(true); m_boomBoomRightLimit.enableLimitSwitch(true); m_boomBoomLeftLimit.enableLimitSwitch(true); + + this.speedLimiter = 1.0; setTurretPIDGains(); } @@ -99,6 +102,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF); m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone); m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput); + // TODO: add 0.1 } @Override @@ -140,6 +144,20 @@ public class Turret extends SubsystemBase { } leftPrevState = leftState; // * Update the state of the left limit switch. + + + // speed limiting near hard limits + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); + + if (forwardDistance < 20.0) { + this.speedLimiter = 0.2 + (forwardDistance * 0.05); + } + + if (reverseDistance < 20.0) { + this.speedLimiter = 0.2 + (reverseDistance * 0.05); + } } /** @@ -169,7 +187,7 @@ public class Turret extends SubsystemBase { * @param input from -1.0 to 1.0, positive is clockwise */ public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter); } public void runShooterRotatePID(double targetAngle) { From 8212c2c70a6133aabd26f7f3a96137a9d239b326 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 12:43:54 -0600 Subject: [PATCH 110/180] sum chang --- src/main/java/frc4388/robot/Constants.java | 7 +++---- src/main/java/frc4388/robot/subsystems/Turret.java | 6 +++--- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cb10859..ff182de 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -209,14 +209,13 @@ public final class Constants { //Gains for hood public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); - //#region test start - - //#endregion test end public static final double TURRET_FORWARD_HARD_LIMIT = 18.429; public static final double TURRET_REVERSE_HARD_LIMIT = -106.454; - public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 4; + public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 5; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; + + public static final double TURRET_HARD_LIMIT_TOLERANCE = 20.0; //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 4c04332..17f05f2 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -146,16 +146,16 @@ public class Turret extends SubsystemBase { leftPrevState = leftState; // * Update the state of the left limit switch. - // speed limiting near hard limits + // * speed limiting near hard limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). double currentPos = this.getEncoderPosition(); double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - if (forwardDistance < 20.0) { + if (forwardDistance < ShooterConstants.TURRET_HARD_LIMIT_TOLERANCE) { this.speedLimiter = 0.2 + (forwardDistance * 0.05); } - if (reverseDistance < 20.0) { + if (reverseDistance < ShooterConstants.TURRET_HARD_LIMIT_TOLERANCE) { this.speedLimiter = 0.2 + (reverseDistance * 0.05); } } From 0a79361a9781f21b46a232820c2c7da9e26de2cf Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 12:44:15 -0600 Subject: [PATCH 111/180] *this* is important get it ? --- src/main/java/frc4388/utility/Vector2D.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/utility/Vector2D.java b/src/main/java/frc4388/utility/Vector2D.java index cc04125..9ee4fc5 100644 --- a/src/main/java/frc4388/utility/Vector2D.java +++ b/src/main/java/frc4388/utility/Vector2D.java @@ -63,8 +63,8 @@ public class Vector2D extends Vector2d { * @param v Vector to add */ public void add(Vector2D v) { - x += v.x; - y += v.x; + this.x += v.x; + this.y += v.x; } /** @@ -82,8 +82,8 @@ public class Vector2D extends Vector2d { * @param v Vector to subtract */ public void subtract(Vector2D v) { - x -= v.x; - y -= v.x; + this.x -= v.x; + this.y -= v.x; } /** @@ -101,8 +101,8 @@ public class Vector2D extends Vector2d { * @param scalar Scalar to multiply */ public void multiply(double scalar) { - x *= scalar; - y *= scalar; + this.x *= scalar; + this.y *= scalar; } /** @@ -120,8 +120,8 @@ public class Vector2D extends Vector2d { * @param scalar Scalar to divide */ public void divide(double scalar) { - x /= scalar; - y /= scalar; + this.x /= scalar; + this.y /= scalar; } /** From ba8de492b3b4216ce097c26df1f41299f53f2574 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 13:49:13 -0600 Subject: [PATCH 112/180] speed ramp fixes for hood + turret --- src/main/java/frc4388/robot/Constants.java | 9 +++-- .../java/frc4388/robot/RobotContainer.java | 6 ++-- .../java/frc4388/robot/subsystems/Hood.java | 33 ++++++++++++++++--- .../java/frc4388/robot/subsystems/Turret.java | 10 ++++-- 4 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ff182de..5376fd2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,6 +4,8 @@ package frc4388.robot; +import java.security.PublicKey; + import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; @@ -215,7 +217,7 @@ public final class Constants { public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 5; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; - public static final double TURRET_HARD_LIMIT_TOLERANCE = 20.0; + public static final double TURRET_SOFT_LIMIT_TOLERANCE = 20.0; //Shooter gains for actual Drum public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); @@ -223,8 +225,9 @@ public final class Constants { public static final int SHOOTER_ANGLE_ADJUST_ID = 20; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; // TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; // TODO: Find - public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find - public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find + public static final double HOOD_FORWARD_SOFT_LIMIT = 0.0; // TODO: find + public static final double HOOD_REVERSE_SOFT_LIMIT = -150; // TODO: find + public static final double HOOD_SOFT_LIMIT_TOLERANCE = 20.0; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 80aae83..b028826 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -63,7 +63,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; -import frc4388.robot.commands.ButtonBoxCommands.TurretManual; +// import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Shoot; @@ -353,8 +353,8 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) // .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false))); - new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whileHeld(new TurretManual(m_robotTurret)); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + // .whileHeld(new TurretManual(m_robotTurret)); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 904eb26..0652f4b 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -34,6 +34,7 @@ public class Hood extends SubsystemBase { public double m_fireAngle; + public double speedLimiter; /** Creates a new Hood. */ public Hood(CANSparkMax angleAdjusterMotor) { @@ -48,9 +49,11 @@ public class Hood extends SubsystemBase { // m_hoodUpLimitSwitch.enableLimitSwitch(true); // m_hoodDownLimitSwitch.enableLimitSwitch(true); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_LIMIT); - m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, (float) ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, (float) ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); setHoodSoftLimits(true); + + this.speedLimiter = 1.0; } @@ -58,6 +61,26 @@ public class Hood extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); + + // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); + + if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (forwardDistance * 0.05); + } + + if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * 0.05); + } + + if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } + + double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent(); + } /** @@ -87,7 +110,7 @@ public class Hood extends SubsystemBase { * @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle) */ public void runHood(double input) { - m_angleAdjusterMotor.set(input); + m_angleAdjusterMotor.set(input * this.speedLimiter); } /** @@ -101,8 +124,8 @@ public class Hood extends SubsystemBase { m_angleEncoder.setPosition(0); } - public double getAnglePosition(){ - return 0.0;//m_angleEncoder.getPosition(); + public double getEncoderPosition(){ + return m_angleEncoder.getPosition(); } public double getAnglePositionDegrees(){ diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 17f05f2..de1c9eb 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -146,18 +146,22 @@ public class Turret extends SubsystemBase { leftPrevState = leftState; // * Update the state of the left limit switch. - // * speed limiting near hard limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). double currentPos = this.getEncoderPosition(); double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - if (forwardDistance < ShooterConstants.TURRET_HARD_LIMIT_TOLERANCE) { + if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { this.speedLimiter = 0.2 + (forwardDistance * 0.05); } - if (reverseDistance < ShooterConstants.TURRET_HARD_LIMIT_TOLERANCE) { + if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { this.speedLimiter = 0.2 + (reverseDistance * 0.05); } + + if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } } /** From b4bf4296de24de8d1bc030b820e3b40d01b209e8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 15:09:46 -0600 Subject: [PATCH 113/180] driving field relative works and nathans turning works --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/subsystems/SwerveDrive.java | 12 +++++++----- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5376fd2..549a128 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -34,7 +34,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 2.0; + public static final double ROTATION_SPEED = 4.0; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b028826..559fcaf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -155,7 +155,7 @@ public class RobotContainer { getDriverController().getLeftY(), //getDriverController().getRightX(), getDriverController().getRightX(), - // getDriverController().getRightY(), + getDriverController().getRightY(), true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 262c950..e1b8fae 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -113,16 +113,16 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - Translation2d speed = new Translation2d(leftX, leftY); + Translation2d speed = new Translation2d(-leftX, leftY); speed = speed.times(speed.getNorm() * speedAdjust); if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightY, rightX); + rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = speed.getX(); + double xSpeedMetersPerSecond = -speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getDegrees())) + rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getRadians() + (Math.PI/2))) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); @@ -154,6 +154,8 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); updateSmartDash(); + SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees()); + SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle()); SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); @@ -165,7 +167,7 @@ public class SwerveDrive extends SubsystemBase { // odometry SmartDashboard.putNumber("Odometry: X", getOdometry().getX()); SmartDashboard.putNumber("Odometry: Y", getOdometry().getY()); - SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees()); + SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees()); // chassis speeds // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds From f1ea7cc84c1b9892ff412d24824c343593d7dc1a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 17:03:39 -0600 Subject: [PATCH 114/180] 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 d9b49b96736b054e2f7f9952c641952eba5d1b4b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 19:41:20 -0600 Subject: [PATCH 115/180] target lock thing on smart dash --- .../robot/commands/ShooterCommands/Shoot.java | 8 ++-- .../commands/ShooterCommands/TrackTarget.java | 37 ++++++++++++------- 2 files changed, 28 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index d39f4ce..aabac8f 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -187,9 +187,9 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (simMode) { - return isAimedInTolerance; - } - return false; + // if (simMode) { + return isAimedInTolerance; + // } + // return false; } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index bf524e6..426bcf2 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -9,6 +9,7 @@ import java.util.ArrayList; import org.opencv.core.Point; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -40,6 +41,10 @@ public class TrackTarget extends CommandBase { Pose2d pos = new Pose2d(); ArrayList points = new ArrayList<>(); + private boolean targetLocked = false; + private double velocityTolerance = 100.0; + private double hoodTolerance = 5.0; + double m=0; double b=0; boolean isExecuted = false; @@ -67,62 +72,68 @@ public class TrackTarget extends CommandBase { @Override public void execute() { //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); + SmartDashboard.putBoolean("Target Locked", this.targetLocked); + try { m_visionOdometry.setLEDs(true); points = m_visionOdometry.getTargetPoints(); - + Point average = VisionOdometry.averagePoint(points); - + for(Point point : points) { Vector2D difference = new Vector2D(point); difference.subtract(new Vector2D(average)); if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD) - points.remove(point); + points.remove(point); } - + average = VisionOdometry.averagePoint(points); DesmosServer.putPoint("average", average); - + for(int i = 0; i < points.size(); i++) { DesmosServer.putPoint("Point" + i, points.get(i)); } - + output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 4; // output *= 0.5; DesmosServer.putDouble("output", output); m_turret.runTurretWithInput(output); - + double y_rot = average.y / VisionConstants.LIME_VIXELS; y_rot *= Math.toRadians(VisionConstants.V_FOV); y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); - + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); DesmosServer.putDouble("distance", distance); - + updateRegressionDesmos(); double regressedDistance = distanceRegression(distance); regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; DesmosServer.putDouble("distanceReg", regressedDistance); - - //Vision odemetry circle fit based pose estimate + + //Vision odometry circle fit based pose estimate // Point targetOffset = m_visionOdometry.getTargetOffset(); // DesmosServer.putPoint("targetOff", targetOffset); - + vel = m_boomBoom.getVelocity(regressedDistance + 30); hood = m_boomBoom.getHood(regressedDistance + 30); // m_boomBoom.runDrumShooter(vel); m_boomBoom.runDrumShooterVelocityPID(vel); m_hood.runAngleAdjustPID(hood); + + double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); + double currentHood = this.m_hood.getEncoderPosition(); + + this.targetLocked = (Math.abs(currentDrumVel - vel) < velocityTolerance) && (Math.abs(currentHood - hood) < hoodTolerance); SmartDashboard.putNumber("Regressed Distance", regressedDistance); SmartDashboard.putNumber("Distance", distance); SmartDashboard.putNumber("Hood Target Angle Track", hood); SmartDashboard.putNumber("Vel Target Track", vel); - // isExecuted = true; } catch (Exception e){ From 470552c575b22a7b7f0bd0addf94978cfda7e1e9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Mar 2022 20:01:32 -0600 Subject: [PATCH 116/180] basic climber code (DRIVE STILL NOT WORKING) --- .../java/frc4388/robot/RobotContainer.java | 36 ++++++++++++------- .../frc4388/robot/subsystems/Climber.java | 24 +++++++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- 3 files changed, 46 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 559fcaf..1e59a82 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,6 +29,7 @@ import java.util.regex.Matcher; import java.util.regex.Pattern; import java.util.stream.Collectors; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.diffplug.common.base.Errors; import com.pathplanner.lib.PathPlanner; @@ -114,9 +115,10 @@ public class RobotContainer { public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor); public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30); - private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31); - public final Climber m_robotClimber = new Climber(testElbowMotor); + + private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); + private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); + public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -145,7 +147,6 @@ public class RobotContainer { */ public RobotContainer() { configureButtonBindings(); - // testShoulderMotor.setNeutralMode(NeutralMode.Brake); /* Default Commands */ // Swerve Drive with Input @@ -170,9 +171,9 @@ public class RobotContainer { // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber) - // ); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) + ); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), @@ -186,7 +187,6 @@ public class RobotContainer { new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotHood.setDefaultCommand( new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( @@ -215,11 +215,19 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kStart.value) .whenPressed(m_robotSwerveDrive::resetGyro); // Left Bumper > Shift Down - new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); - // Right Bumper > Shift Up - new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + // new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // // Right Bumper > Shift Up + // new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + // .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + new JoystickButton(getDriverController(), XboxController.Axis.kLeftTrigger.value) + .whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.1), m_robotClimber)) + .whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0))); + + new JoystickButton(getDriverController(), XboxController.Axis.kRightTrigger.value) + .whileHeld(new RunCommand(() -> m_robotClimber.runShoulderWithInput(-0.1), m_robotClimber)) + .whenReleased(new RunCommand(() -> m_robotClimber.runShoulderWithInput(0.0))); // new JoystickButton(getDriverController(), XboxController.Button.kA.value) // .whenPressed(() -> m_robotBoomBoom.increaseSpeed(0.025)); @@ -238,6 +246,8 @@ public class RobotContainer { // .whenPressed(() -> m_robotMap.leftBack.reset()) // .whenPressed(() -> m_robotMap.rightBack.reset()); + + /* Operator Buttons */ // X > Extend Intake diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 052d024..7425346 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,21 +4,41 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { + + WPI_TalonFX m_climberShoulder; WPI_TalonFX m_climberElbow; + /** Creates a new Climber. */ - public Climber(WPI_TalonFX climberElbow) { + public Climber(WPI_TalonFX climberShoulder, WPI_TalonFX climberElbow) { + m_climberShoulder = climberShoulder; m_climberElbow = climberElbow; + + m_climberShoulder.configFactoryDefault(); + m_climberElbow.configFactoryDefault(); + m_climberShoulder.setNeutralMode(NeutralMode.Brake); + m_climberElbow.setNeutralMode(NeutralMode.Brake); } - public void runWithInput(double input){ + public void runShoulderWithInput(double input) { + m_climberShoulder.set(input); + } + + public void runElbowWithInput(double input){ m_climberElbow.set(input); } + + public void runBothMotorsWithInputs(double shoulderInput, double elbowInput) { + m_climberShoulder.set(shoulderInput); + m_climberElbow.set(elbowInput); + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e1b8fae..9a4a5e9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -122,7 +122,7 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getRadians() + (Math.PI/2))) + rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2))) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); From 71c9cd2e40d523699c397ab3c66f1318f8dea605 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 10:45:08 -0600 Subject: [PATCH 117/180] 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 18e7c336ec6feddc3bd131cd5465ccab6af24329 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 10:52:48 -0600 Subject: [PATCH 118/180] drive stuff --- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- .../java/frc4388/robot/subsystems/SwerveDrive.java | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1e59a82..af796f7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -171,9 +171,9 @@ public class RobotContainer { // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) - ); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) + // ); // Storage Management /*m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9a4a5e9..555ef83 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -113,17 +113,17 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; - Translation2d speed = new Translation2d(-leftX, leftY); + Translation2d speed = new Translation2d(leftX, leftY); speed = speed.times(speed.getNorm() * speedAdjust); if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1)); + rotTarget = new Rotation2d(rightX, rightY); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); - double xSpeedMetersPerSecond = -speed.getX(); + double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2))) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8); + rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); setModuleStates(states); From fbd74f72cee42a488c55953317e56b79db4db821 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 11:31:28 -0600 Subject: [PATCH 119/180] extender fix --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 3 ++- .../commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java | 4 ++++ 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 549a128..37137a4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -151,7 +151,7 @@ public final class Constants { } public static final class ExtenderConstants { - public static final double EXTENDER_FORWARD_LIMIT = 250.0; + public static final double EXTENDER_FORWARD_LIMIT = 235.0;//250.0; public static final double EXTENDER_REVERSE_LIMIT = 0.0; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index af796f7..ed4d006 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -357,7 +357,8 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)); + .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); // new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) // .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true))) diff --git a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java index 02ce30a..21fb95d 100644 --- a/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java +++ b/src/main/java/frc4388/robot/commands/ExtenderIntakeCommands/ExtenderIntakeGroup.java @@ -24,6 +24,10 @@ public class ExtenderIntakeGroup extends ParallelRaceGroup { addCommands(new RunIntakeConditionally(intake), new RunExtender(extender)); } + public static void setDirectionToOut() { + ExtenderIntakeGroup.direction = 1; + } + public static void changeDirection() { // Never implemented? ExtenderIntakeGroup.direction *= -1; } From f250c8dfceda6d60e7b43f962decb2e6db7b37fc Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 12:05:29 -0600 Subject: [PATCH 120/180] 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 121/180] 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 122/180] 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 123/180] 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); } /** From 1d9940f81b1c09429406b083758100cfb697c2db Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:09:25 -0600 Subject: [PATCH 124/180] gAtEkEePiNg --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 211 +++++++++--------- .../robot/subsystems/ClimberRewrite.java | 8 +- 3 files changed, 114 insertions(+), 107 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7930670..d9bf54d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -173,6 +173,8 @@ public final class Constants { public static final int SHOULDER_ID = 30; public static final int ELBOW_ID = 31; public static final int GYRO_ID = 14; + + public static final double INPUT_MULTIPLIER = 0.7; // 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 791fcbc..5b4f7b7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -22,6 +22,7 @@ import java.util.Comparator; import java.util.List; import java.util.Objects; import java.util.Optional; +import java.util.function.BooleanSupplier; import java.util.function.Function; import java.util.logging.Level; import java.util.logging.Logger; @@ -64,9 +65,6 @@ import frc4388.robot.subsystems.Claws; import frc4388.robot.commands.RunClaw; import frc4388.robot.subsystems.ClimberRewrite; import frc4388.robot.subsystems.Claws.ClawType; -import frc4388.robot.commands.AimToCenter; -import frc4388.robot.commands.Shoot; -import frc4388.robot.commands.TrackTarget; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; @@ -113,7 +111,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public 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); @@ -130,8 +128,8 @@ public class RobotContainer { public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); - private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); + // private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); + // private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers @@ -156,19 +154,15 @@ public class RobotContainer { public static boolean softLimits = true; + public static enum CurrentMode {TURRET, CLIMBER}; + public CurrentMode currentMode = CurrentMode.TURRET; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); /* 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.7, getOperatorController().getRightY() * 0.7), - m_robotClimber)); - - // IK command // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), @@ -196,21 +190,28 @@ public class RobotContainer { // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) - // ); - // Storage Management - /*m_robotStorage.setDefaultCommand( - new RunCommand(() -> m_robotStorage.manageStorage(), - m_robotStorage).withName("Storage manageStorage 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.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), + new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); - // Turret Manual + + // Turret Manual + m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret) + .until(() -> this.currentMode.equals(CurrentMode.CLIMBER))); + // .withName("Turret runTurretWithInput defaultCommand"); + + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(getDriverController().getLeftY(), getDriverController().getRightY()), m_robotClimber) + .until(() -> this.currentMode.equals(CurrentMode.TURRET))); + + // EnableTurret() m_robotHood.setDefaultCommand( new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); @@ -247,10 +248,6 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - - - - /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -262,9 +259,6 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); - - - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); @@ -321,15 +315,16 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - // .whileHeld(new TurretManual(m_robotTurret)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + // .whenReleased(EnableClimber())); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); - new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); @@ -339,6 +334,12 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } + //EnableTurret() + //set turret default to joysticks, set climber default to null + + //EnableClimber() + //set climber default to joysticks set turret defaults to null + /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -389,18 +390,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); + } /** * Creates a WatchKey for the path planner directory and registers it with the @@ -409,26 +410,26 @@ 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 @@ -458,35 +459,35 @@ public class RobotContainer { * @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."); + // 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) { @@ -498,22 +499,22 @@ public class RobotContainer { } 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()); + // // 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() { diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index ef6e973..4d81b38 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -128,8 +128,8 @@ public class ClimberRewrite extends SubsystemBase { } public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput); - m_elbow.set(elbowOutput); + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); } public double[] getJointAngles() { @@ -282,4 +282,8 @@ public class ClimberRewrite extends SubsystemBase { public static Point getClimberPosition(double[] jointAngles) { return getClimberPosition(jointAngles[0], jointAngles[1]); } + + public double getCurrent() { + return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); + } } From 6db9636f222944a66db6b060bde2594923b684c3 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:13:37 -0600 Subject: [PATCH 125/180] softlims --- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/Robot.java | 1 + .../java/frc4388/robot/RobotContainer.java | 49 ++++++++++++++++--- .../robot/subsystems/ClimberRewrite.java | 32 +++++++++--- 4 files changed, 71 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d9bf54d..7a06fd9 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -194,9 +194,9 @@ public final class Constants { 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_FORWARD = 53869; 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_FORWARD = 281717; public static final double ELBOW_SOFT_LIMIT_REVERSE = 0; // PID Constants diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 759e2c9..f01d45a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -124,6 +124,7 @@ public class Robot extends TimedRobot { }); desmosServer.start(); + m_robotContainer.m_robotVisionOdometry.setLEDs(false); // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0}); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5b4f7b7..5cb54c8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,10 +112,14 @@ public class RobotContainer { /* Subsystems */ public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); +<<<<<<< Updated upstream private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); // Subsystems +======= + public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); +>>>>>>> Stashed changes public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); @@ -139,7 +143,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"); @@ -199,6 +203,7 @@ public class RobotContainer { m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); +<<<<<<< Updated upstream // Turret Manual @@ -212,9 +217,15 @@ public class RobotContainer { .until(() -> this.currentMode.equals(CurrentMode.TURRET))); // EnableTurret() +======= + // Turret Manual + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); +>>>>>>> Stashed changes - m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); + // m_robotHood.setDefaultCommand( + // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -268,10 +279,10 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); //Toggles extender in and out - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); @@ -292,7 +303,7 @@ public class RobotContainer { // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); //B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); @@ -305,15 +316,18 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) @@ -411,6 +425,7 @@ public class RobotContainer { * Finally, adds the existing path files to the auto chooser */ private void autoInit() { +<<<<<<< Updated upstream // try { // WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), // StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, @@ -429,6 +444,26 @@ public class RobotContainer { // .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) // .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); // SmartDashboard.putData("Auto Chooser", autoChooser); +======= + 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); +>>>>>>> Stashed changes } /** diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index 4d81b38..a9d208a 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -17,6 +17,7 @@ import com.ctre.phoenix.sensors.WPI_Pigeon2; import org.opencv.core.Point; import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; @@ -51,16 +52,16 @@ public class ClimberRewrite extends SubsystemBase { // 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_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); - m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); - m_elbow.configReverseSoftLimitEnable(false); + m_elbow.configForwardSoftLimitEnable(true); + // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); + // m_elbow.configReverseSoftLimitEnable(true); m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); - m_shoulder.configForwardSoftLimitEnable(false); + m_shoulder.configForwardSoftLimitEnable(true); m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); m_shoulder.configReverseSoftLimitEnable(false); @@ -174,6 +175,8 @@ public class ClimberRewrite extends SubsystemBase { @Override public void periodic() { + SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); + SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // setJointAngles(jointAngles); } @@ -283,7 +286,24 @@ public class ClimberRewrite extends SubsystemBase { return getClimberPosition(jointAngles[0], jointAngles[1]); } +<<<<<<< Updated upstream public double getCurrent() { return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); } +======= + public void setClimberSoftLimits(boolean set){ + m_elbow.configForwardSoftLimitEnable(set); + m_shoulder.configForwardSoftLimitEnable(set); + } + + public void setEncoders(double value){ + m_elbow.setSelectedSensorPosition(value); + m_shoulder.setSelectedSensorPosition(value); + } + + public double getCurrent(){ + return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); + } + +>>>>>>> Stashed changes } From fc1076c5e9dc66acc837eddb99bfffd7a393443c Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:15:55 -0600 Subject: [PATCH 126/180] merge --- .../java/frc4388/robot/RobotContainer.java | 36 +------------------ .../robot/subsystems/ClimberRewrite.java | 6 ---- 2 files changed, 1 insertion(+), 41 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5cb54c8..d94e2d8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,14 +112,7 @@ public class RobotContainer { /* Subsystems */ public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); -<<<<<<< Updated upstream - - private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); - - // Subsystems -======= public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); ->>>>>>> Stashed changes public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); @@ -203,7 +196,6 @@ public class RobotContainer { m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); -<<<<<<< Updated upstream // Turret Manual @@ -217,12 +209,6 @@ public class RobotContainer { .until(() -> this.currentMode.equals(CurrentMode.TURRET))); // EnableTurret() -======= - // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); ->>>>>>> Stashed changes // m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); @@ -425,26 +411,7 @@ public class RobotContainer { * Finally, adds the existing path files to the auto chooser */ private void autoInit() { -<<<<<<< Updated upstream - // 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); -======= + try { WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, @@ -463,7 +430,6 @@ public class RobotContainer { .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); SmartDashboard.putData("Auto Chooser", autoChooser); ->>>>>>> Stashed changes } /** diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index a9d208a..83b6e2b 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -286,11 +286,6 @@ public class ClimberRewrite extends SubsystemBase { return getClimberPosition(jointAngles[0], jointAngles[1]); } -<<<<<<< Updated upstream - public double getCurrent() { - return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); - } -======= public void setClimberSoftLimits(boolean set){ m_elbow.configForwardSoftLimitEnable(set); m_shoulder.configForwardSoftLimitEnable(set); @@ -305,5 +300,4 @@ public class ClimberRewrite extends SubsystemBase { return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); } ->>>>>>> Stashed changes } From 1b6265c65adf858ad7cd818dd74e0aa98a6bbab8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:26:03 -0600 Subject: [PATCH 127/180] new try at turret or climber thing --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 45 +++++-------------- .../robot/subsystems/ClimberRewrite.java | 7 +-- 3 files changed, 13 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a06fd9..b2ba4d6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,6 +175,7 @@ public final class Constants { public static final int GYRO_ID = 14; public static final double INPUT_MULTIPLIER = 0.7; + public static final double CLIMBER_SOFT_LIMIT_TOLERANCE = 20.0; // 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5cb54c8..29d213b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,14 +112,10 @@ public class RobotContainer { /* Subsystems */ public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); -<<<<<<< Updated upstream private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); // Subsystems -======= - public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); ->>>>>>> Stashed changes public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); @@ -203,7 +199,6 @@ public class RobotContainer { m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); -<<<<<<< Updated upstream // Turret Manual @@ -217,12 +212,6 @@ public class RobotContainer { .until(() -> this.currentMode.equals(CurrentMode.TURRET))); // EnableTurret() -======= - // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); ->>>>>>> Stashed changes // m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); @@ -330,8 +319,17 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + + .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) + .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftY(), getOperatorController().getRightY()), m_robotClimber)))) + + .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) + .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))); + + // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) + // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); // .whenReleased(EnableClimber())); // control turret manual mode @@ -425,26 +423,6 @@ public class RobotContainer { * Finally, adds the existing path files to the auto chooser */ private void autoInit() { -<<<<<<< Updated upstream - // 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); -======= try { WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, @@ -463,7 +441,6 @@ public class RobotContainer { .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); SmartDashboard.putData("Auto Chooser", autoChooser); ->>>>>>> Stashed changes } /** diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index a9d208a..621b213 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -179,6 +179,7 @@ public class ClimberRewrite extends SubsystemBase { SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // setJointAngles(jointAngles); + } /** @@ -286,11 +287,6 @@ public class ClimberRewrite extends SubsystemBase { return getClimberPosition(jointAngles[0], jointAngles[1]); } -<<<<<<< Updated upstream - public double getCurrent() { - return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); - } -======= public void setClimberSoftLimits(boolean set){ m_elbow.configForwardSoftLimitEnable(set); m_shoulder.configForwardSoftLimitEnable(set); @@ -305,5 +301,4 @@ public class ClimberRewrite extends SubsystemBase { return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); } ->>>>>>> Stashed changes } From cb3f1942b611ce5a1e360174cdac455e0f0c1b18 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:45:27 -0600 Subject: [PATCH 128/180] speed limiters for elbow and shoulder --- src/main/java/frc4388/robot/Constants.java | 11 +++-- .../robot/subsystems/ClimberRewrite.java | 48 ++++++++++++++++--- .../java/frc4388/robot/subsystems/Hood.java | 7 +-- .../java/frc4388/robot/subsystems/Turret.java | 4 +- 4 files changed, 52 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b2ba4d6..9c2e1a0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,7 +175,8 @@ public final class Constants { public static final int GYRO_ID = 14; public static final double INPUT_MULTIPLIER = 0.7; - public static final double CLIMBER_SOFT_LIMIT_TOLERANCE = 20.0; + public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0; + public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm @@ -195,10 +196,10 @@ public final class Constants { 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 = 53869; - public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0; - public static final double ELBOW_SOFT_LIMIT_FORWARD = 281717; - public static final double ELBOW_SOFT_LIMIT_REVERSE = 0; + public static final double SHOULDER_FORWARD_SOFT_LIMIT = 53869; + public static final double SHOULDER_REVERSE_SOFT_LIMIT = 0; + public static final double ELBOW_FORWARD_SOFT_LIMIT = 281717; + public static final double ELBOW_REVERSE_SOFT_LIMIT = 0; // PID Constants public static final int SHOULDER_SLOT_IDX = 0; diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index 621b213..78cc123 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -37,6 +37,8 @@ public class ClimberRewrite extends SubsystemBase { private Point tPoint; private boolean groundRelative; + private double shoulderSpeedLimiter; + private double elbowSpeedLimiter; /** Creates a new ClimberRewrite. */ public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) { @@ -55,14 +57,14 @@ public class ClimberRewrite extends SubsystemBase { // 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.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); m_elbow.configForwardSoftLimitEnable(true); // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); // m_elbow.configReverseSoftLimitEnable(true); - m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); + m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); m_shoulder.configForwardSoftLimitEnable(true); - m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); + m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); m_shoulder.configReverseSoftLimitEnable(false); m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); @@ -78,6 +80,7 @@ public class ClimberRewrite extends SubsystemBase { m_gyro = gyro; groundRelative = _groundRelative; + this.elbowSpeedLimiter = 1.0; } public void setClimberGains() { @@ -129,8 +132,8 @@ public class ClimberRewrite extends SubsystemBase { } public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); } public double[] getJointAngles() { @@ -179,7 +182,40 @@ public class ClimberRewrite extends SubsystemBase { SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // setJointAngles(jointAngles); - + + // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). + double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); + double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); + double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); + + if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { + this.elbowSpeedLimiter = 1.0; + } + + // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). + double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); + double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); + double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); + + if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { + this.shoulderSpeedLimiter = 1.0; + } } /** diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 0652f4b..5f720d4 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -68,19 +68,16 @@ public class Hood extends SubsystemBase { double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * 0.05); + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); } if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * 0.05); + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); } if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { this.speedLimiter = 1.0; } - - double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent(); - } /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index de1c9eb..9d9e35c 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -152,11 +152,11 @@ public class Turret extends SubsystemBase { double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * 0.05); + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); } if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * 0.05); + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); } if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { From 517053ea50da372d969366a9db52eef156d719ed Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 15:05:07 -0600 Subject: [PATCH 129/180] file change --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/commands/{ => ClimberCommands}/RunClaw.java | 2 +- .../commands/{climber => ClimberCommands}/RunClimberPath.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename src/main/java/frc4388/robot/commands/{ => ClimberCommands}/RunClaw.java (96%) rename src/main/java/frc4388/robot/commands/{climber => ClimberCommands}/RunClimberPath.java (97%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 19c9887..5b1b59f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,7 +62,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; 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.ClimberRewrite; import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.Constants.OIConstants; @@ -70,6 +69,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; +import frc4388.robot.commands.ClimberCommands.RunClaw; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java similarity index 96% rename from src/main/java/frc4388/robot/commands/RunClaw.java rename to src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java index 0c4e8b7..fdcd151 100644 --- a/src/main/java/frc4388/robot/commands/RunClaw.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ClimberCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ClawConstants; diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java similarity index 97% rename from src/main/java/frc4388/robot/commands/climber/RunClimberPath.java rename to src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java index 38ad651..23f37e6 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ClimberCommands; import org.opencv.core.Point; From 1693d320e523a525bf179b724a7ceb28235dbdaf Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 15:09:08 -0600 Subject: [PATCH 130/180] asdfghjkl --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 154 +++++++++--------- .../robot/subsystems/ClimberRewrite.java | 4 +- 3 files changed, 78 insertions(+), 83 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a06fd9..5408f11 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -173,8 +173,6 @@ public final class Constants { public static final int SHOULDER_ID = 30; public static final int ELBOW_ID = 31; public static final int GYRO_ID = 14; - - public static final double INPUT_MULTIPLIER = 0.7; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm @@ -267,6 +265,7 @@ public final class Constants { public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; public static final double ENCODER_TICKS_PER_REV = 2048; + public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d94e2d8..947f92a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -22,7 +22,6 @@ import java.util.Comparator; import java.util.List; import java.util.Objects; import java.util.Optional; -import java.util.function.BooleanSupplier; import java.util.function.Function; import java.util.logging.Level; import java.util.logging.Logger; @@ -125,8 +124,8 @@ public class RobotContainer { public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - // private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); - // private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); + private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); + private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers @@ -151,15 +150,19 @@ public class RobotContainer { public static boolean softLimits = true; - public static enum CurrentMode {TURRET, CLIMBER}; - public CurrentMode currentMode = CurrentMode.TURRET; - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { configureButtonBindings(); /* Default Commands */ + + // moves climber in xy space with two-axis input from the operator controller + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7), + m_robotClimber)); + + // IK command // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), @@ -187,28 +190,21 @@ public class RobotContainer { // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - // Storage Management - // m_robotStorage.setDefaultCommand( - // new RunCommand(() -> m_robotStorage.manageStorage(), - // m_robotStorage).withName("Storage manageStorage defaultCommand")); - + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) + // ); + // 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.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), + new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); - - // Turret Manual - + // Turret Manual m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret) - .until(() -> this.currentMode.equals(CurrentMode.CLIMBER))); - // .withName("Turret runTurretWithInput defaultCommand"); - - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getDriverController().getLeftY(), getDriverController().getRightY()), m_robotClimber) - .until(() -> this.currentMode.equals(CurrentMode.TURRET))); - - // EnableTurret() + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); // m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); @@ -245,6 +241,10 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + + + /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -256,6 +256,9 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + + + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); @@ -315,16 +318,15 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); - // .whenReleased(EnableClimber())); + // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + // .whileHeld(new TurretManual(m_robotTurret)); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); @@ -334,12 +336,6 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } - //EnableTurret() - //set turret default to joysticks, set climber default to null - - //EnableClimber() - //set climber default to joysticks set turret defaults to null - /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -460,35 +456,35 @@ public class RobotContainer { * @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."); + 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) { @@ -500,22 +496,22 @@ public class RobotContainer { } 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()); + // 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() { diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index 83b6e2b..620e6fe 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -129,8 +129,8 @@ public class ClimberRewrite extends SubsystemBase { } public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); + m_shoulder.set(shoulderOutput); + m_elbow.set(elbowOutput); } public double[] getJointAngles() { From 43e3a14c4df980832d5c034369d9624a4e43d400 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 15:37:01 -0600 Subject: [PATCH 131/180] button box thing hopefully done --- .../java/frc4388/robot/RobotContainer.java | 50 ++- .../ClimberCommands/RunClimberPath.java | 6 +- .../robot/commands/RunTurretOrClimber.java | 50 +++ .../frc4388/robot/subsystems/Climber.java | 340 ++++++++++++++++-- .../robot/subsystems/ClimberRewrite.java | 340 ------------------ 5 files changed, 402 insertions(+), 384 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunTurretOrClimber.java delete mode 100644 src/main/java/frc4388/robot/subsystems/ClimberRewrite.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 625b1de..6f4a969 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; -import frc4388.robot.subsystems.ClimberRewrite; +import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.ShooterConstants; @@ -110,7 +110,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); public 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 Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -149,6 +149,8 @@ public class RobotContainer { .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); public static boolean softLimits = true; + public enum Mode {SHOOTER, CLIMBER}; + public Mode currentMode = Mode.SHOOTER; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -202,13 +204,29 @@ public class RobotContainer { new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + new RunCommand(() -> { + if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + }, m_robotTurret)); m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); - // m_robotTurret.setDefaultCommand( + new RunCommand(() -> { + if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); } + }, m_robotHood)); + + m_robotClimber.setDefaultCommand( + new RunCommand(() -> { + if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + if (this.currentMode.equals(Mode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); } + }, m_robotClimber)); + + // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); // continually sends updates to the Blinkin LED controller to keep the lights on @@ -318,18 +336,18 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) - .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) - .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) + // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) + // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) + // .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) - .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) - .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) - .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( - new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); + // .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) + // .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) + // .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( + // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java index 23f37e6..592d72d 100644 --- a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -10,11 +10,11 @@ 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.ClimberRewrite; +import frc4388.robot.subsystems.Climber; import frc4388.utility.Vector2D; public class RunClimberPath extends CommandBase { - ClimberRewrite climber; + Climber climber; Claws claws; Point[] path; @@ -23,7 +23,7 @@ public class RunClimberPath extends CommandBase { boolean endPath; /** Creates a new RunClimberPath. */ - public RunClimberPath(Point[] _path, ClimberRewrite _climber, Claws _claws) { + public RunClimberPath(Point[] _path, Climber _climber, Claws _claws) { path = _path; endPath = false; diff --git a/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java b/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java new file mode 100644 index 0000000..c610eda --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java @@ -0,0 +1,50 @@ +// 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.RobotContainer; +import frc4388.robot.subsystems.Climber; +import frc4388.robot.subsystems.Turret; + +public class RunTurretOrClimber extends CommandBase { + + Turret turret; + Climber climber; + + /** Creates a new RunTurretOrClimber. */ + public RunTurretOrClimber(Turret turret, Climber climber) { + // Use addRequirements() here to declare subsystem dependencies. + + this.turret = turret; + this.climber = climber; + + addRequirements(this.turret, this.climber); + } + + // 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() { + // if (RobotContainer.currentMode.equals(RobotContainer.Mode.TURRET)) { + // this.turret.runTurretWithInput(getOperatorController().getLeftX()) + // } else if (RobotContainer.currentMode.equals(RobotContainer.Mode.CLIMBER)) { + + // } + } + + // 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/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 7425346..07870ff 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,47 +4,337 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.revrobotics.CANSparkMax; +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; +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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; +import frc4388.robot.Constants.ClimberConstants; public class Climber extends SubsystemBase { + private static double[][] shoulderFeedMap; + private static double[][] elbowFeedMap; - WPI_TalonFX m_climberShoulder; - WPI_TalonFX m_climberElbow; - - /** Creates a new Climber. */ - public Climber(WPI_TalonFX climberShoulder, WPI_TalonFX climberElbow) { - m_climberShoulder = climberShoulder; - m_climberElbow = climberElbow; - - m_climberShoulder.configFactoryDefault(); - m_climberElbow.configFactoryDefault(); - m_climberShoulder.setNeutralMode(NeutralMode.Brake); - m_climberElbow.setNeutralMode(NeutralMode.Brake); + public static void configureFeedMaps() { + } - public void runShoulderWithInput(double input) { - m_climberShoulder.set(input); + private WPI_TalonFX m_shoulder; + private WPI_TalonFX m_elbow; + private WPI_Pigeon2 m_gyro; + + private Point tPoint; + + private boolean groundRelative; + private double shoulderSpeedLimiter; + private double elbowSpeedLimiter; + + /** Creates a new ClimberRewrite. */ + public Climber(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_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_FORWARD_SOFT_LIMIT); + m_elbow.configForwardSoftLimitEnable(true); + // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); + // m_elbow.configReverseSoftLimitEnable(true); + + m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); + m_shoulder.configForwardSoftLimitEnable(true); + m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); + 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) + m_gyro = gyro; + + groundRelative = _groundRelative; + this.elbowSpeedLimiter = 1.0; } - public void runElbowWithInput(double input){ - m_climberElbow.set(input); + 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 runBothMotorsWithInputs(double shoulderInput, double elbowInput) { - m_climberShoulder.set(shoulderInput); - m_climberElbow.set(elbowInput); + 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() { + 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 * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); + } + + 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]); + } + + 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 * .02; + tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; } @Override public void periodic() { - // This method will be called once per scheduler run + SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); + SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); + // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); + // setJointAngles(jointAngles); + + // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). + double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); + double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); + double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); + + if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { + this.elbowSpeedLimiter = 1.0; + } + + // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). + double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); + double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); + double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); + + if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { + this.shoulderSpeedLimiter = 1.0; + } } - public double getCurrent() { - return m_climberElbow.getSupplyCurrent(); + /** + * 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 + * @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); + 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; } + + 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 getClimberPosition(double[] jointAngles) { + return getClimberPosition(jointAngles[0], jointAngles[1]); + } + + public void setClimberSoftLimits(boolean set){ + m_elbow.configForwardSoftLimitEnable(set); + m_shoulder.configForwardSoftLimitEnable(set); + } + + public void setEncoders(double value){ + m_elbow.setSelectedSensorPosition(value); + m_shoulder.setSelectedSensorPosition(value); + } + + public double getCurrent(){ + return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); + } + } diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java deleted file mode 100644 index 78cc123..0000000 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ /dev/null @@ -1,340 +0,0 @@ -// 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.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; -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.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; -import frc4388.robot.Constants.ClimberConstants; - -public class ClimberRewrite extends SubsystemBase { - private static double[][] shoulderFeedMap; - private static double[][] elbowFeedMap; - - public static void configureFeedMaps() { - - } - - private WPI_TalonFX m_shoulder; - private WPI_TalonFX m_elbow; - private WPI_Pigeon2 m_gyro; - - private Point tPoint; - - private boolean groundRelative; - private double shoulderSpeedLimiter; - private double elbowSpeedLimiter; - - /** 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_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_FORWARD_SOFT_LIMIT); - m_elbow.configForwardSoftLimitEnable(true); - // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); - // m_elbow.configReverseSoftLimitEnable(true); - - m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - m_shoulder.configForwardSoftLimitEnable(true); - m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - 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) - m_gyro = gyro; - - groundRelative = _groundRelative; - this.elbowSpeedLimiter = 1.0; - } - - 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() { - 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 * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); - } - - 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]); - } - - 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 * .02; - tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; - } - - @Override - public void periodic() { - SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); - SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); - // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - // setJointAngles(jointAngles); - - // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). - double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); - double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); - double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); - - if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { - this.elbowSpeedLimiter = 1.0; - } - - // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). - double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); - double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - - if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { - this.shoulderSpeedLimiter = 1.0; - } - } - - /** - * 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 - * @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); - 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; - } - - 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 getClimberPosition(double[] jointAngles) { - return getClimberPosition(jointAngles[0], jointAngles[1]); - } - - public void setClimberSoftLimits(boolean set){ - m_elbow.configForwardSoftLimitEnable(set); - m_shoulder.configForwardSoftLimitEnable(set); - } - - public void setEncoders(double value){ - m_elbow.setSelectedSensorPosition(value); - m_shoulder.setSelectedSensorPosition(value); - } - - public double getCurrent(){ - return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); - } - -} From cb0d5528e6c7f3e87b558d2a66aca939dab25f14 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 15:43:06 -0600 Subject: [PATCH 132/180] smol fix --- 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 6f4a969..603bf0e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -336,7 +336,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) @@ -349,8 +349,8 @@ public class RobotContainer { // .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); - // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) - // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + .whenPressed(new InstantCommand(() -> this.currentMode = Mode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentMode = Mode.SHOOTER)); // .whenReleased(EnableClimber())); // control turret manual mode From b308be888c7bc414ef35ac8d60e732e9718a5c12 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 17:18:39 -0600 Subject: [PATCH 133/180] climber auto vs manual mode switching done (i think) --- .../java/frc4388/robot/RobotContainer.java | 84 ++++++++----------- .../ClimberCommands/RunClimberPath.java | 2 +- .../robot/commands/RunTurretOrClimber.java | 50 ----------- 3 files changed, 36 insertions(+), 100 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/RunTurretOrClimber.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 603bf0e..10acaaf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -38,6 +38,7 @@ import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; import com.pathplanner.lib.commands.PPSwerveControllerCommand; import org.ejml.dense.row.decomposition.hessenberg.TridiagonalDecompositionHouseholderOrig_DDRM; +import org.opencv.core.Point; import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.PIDController; @@ -69,6 +70,7 @@ import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ClimberCommands.RunClaw; +import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; @@ -149,8 +151,13 @@ public class RobotContainer { .compile(".path")::matcher).andThen(m -> m.replaceFirst("")); public static boolean softLimits = true; - public enum Mode {SHOOTER, CLIMBER}; - public Mode currentMode = Mode.SHOOTER; + + // mode switching + private enum ControlMode { SHOOTER, CLIMBER }; + private ControlMode currentControlMode = ControlMode.SHOOTER; + + private enum ClimberMode { MANUAL, AUTONOMOUS }; + private ClimberMode currentClimberMode = ClimberMode.MANUAL; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -159,28 +166,22 @@ public class RobotContainer { configureButtonBindings(); /* Default Commands */ - // moves climber in xy space with two-axis input from the operator controller - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7), - // m_robotClimber)); - - // IK command // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); - // Swerve Drive with Input + // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), - //getDriverController().getRightX(), getDriverController().getRightX(), getDriverController().getRightY(), true), m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - // Intake with Triggers + + // Intake with Triggers m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( getOperatorController().getLeftTriggerAxis(), @@ -191,14 +192,12 @@ public class RobotContainer { // new ManageStorage(m_robotStorage, // m_robotBoomBoom, // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.runBothMotorsWithInputs(getDriverController().getLeftY() * 0.1, getDriverController().getRightY() * 0.1), m_robotClimber) - // ); - // Storage Management - /*m_robotStorage.setDefaultCommand( + + // Storage Management + m_robotStorage.setDefaultCommand( new RunCommand(() -> m_robotStorage.manageStorage(), - m_robotStorage).withName("Storage manageStorage defaultCommand"));*/ + m_robotStorage).withName("Storage manageStorage defaultCommand")); + // Serializer Management m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), @@ -210,20 +209,22 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); - + m_robotClimber.setDefaultCommand( new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { + if (this.currentClimberMode.equals(ClimberMode.MANUAL)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); } + } }, m_robotClimber)); // m_robotTurret.setDefaultCommand( @@ -259,10 +260,6 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - - - - /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -274,9 +271,6 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); - - - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); @@ -297,7 +291,8 @@ public class RobotContainer { // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - // // Left Bumper > Storage Out (note: neccessary?) + + // Left Bumper > Storage Out (note: necessary?) // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); @@ -316,7 +311,6 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); // .whileHeld% - /* Button Box Buttons */ new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) @@ -337,27 +331,19 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - - // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) - // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) - // .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) - - // .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) - // .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) - // .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( - // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); - - .whenPressed(new InstantCommand(() -> this.currentMode = Mode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentMode = Mode.SHOOTER)); - // .whenReleased(EnableClimber())); + .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentClimberMode = ClimberMode.AUTONOMOUS)) + // TODO: actual climber autonomous command goes here (next line) + .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) + .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL)); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) diff --git a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java index 592d72d..84f66d1 100644 --- a/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -23,7 +23,7 @@ public class RunClimberPath extends CommandBase { boolean endPath; /** Creates a new RunClimberPath. */ - public RunClimberPath(Point[] _path, Climber _climber, Claws _claws) { + public RunClimberPath(Climber _climber, Claws _claws, Point[] _path) { path = _path; endPath = false; diff --git a/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java b/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java deleted file mode 100644 index c610eda..0000000 --- a/src/main/java/frc4388/robot/commands/RunTurretOrClimber.java +++ /dev/null @@ -1,50 +0,0 @@ -// 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.RobotContainer; -import frc4388.robot.subsystems.Climber; -import frc4388.robot.subsystems.Turret; - -public class RunTurretOrClimber extends CommandBase { - - Turret turret; - Climber climber; - - /** Creates a new RunTurretOrClimber. */ - public RunTurretOrClimber(Turret turret, Climber climber) { - // Use addRequirements() here to declare subsystem dependencies. - - this.turret = turret; - this.climber = climber; - - addRequirements(this.turret, this.climber); - } - - // 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() { - // if (RobotContainer.currentMode.equals(RobotContainer.Mode.TURRET)) { - // this.turret.runTurretWithInput(getOperatorController().getLeftX()) - // } else if (RobotContainer.currentMode.equals(RobotContainer.Mode.CLIMBER)) { - - // } - } - - // 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; - } -} From eb34d03c3ceec001a93b0399997186211721e8d4 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 17:37:11 -0600 Subject: [PATCH 134/180] gains n stuff --- src/main/java/frc4388/robot/Constants.java | 6 +-- .../java/frc4388/robot/RobotContainer.java | 41 ++++++++--------- .../frc4388/robot/subsystems/Climber.java | 45 ++++++++++++------- 3 files changed, 53 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4a04665..d409cae 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -185,7 +185,7 @@ public final class Constants { public static final double MAX_ARM_LENGTH = 53; public static final double MIN_ARM_LENGTH = 1; - public static final double MOVE_SPEED = .1; // cm per second + public static final double MOVE_SPEED = 30000; // ticks per second public static final double SHOULDER_RESTING_ANGLE = 0; public static final double ELBOW_RESTING_ANGLE = 0; @@ -208,8 +208,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(.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 Gains SHOULDER_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.2); + public static final Gains ELBOW_GAINS = new Gains(0.5, 0.0, 30.0, 0.0, 0, 1.0); //Prev P 1 d 0.4 public static final int CLIMBER_TIMEOUT_MS = 50; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6f4a969..ce1e39f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -166,9 +166,9 @@ public class RobotContainer { // IK command - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), - // getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), + getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( @@ -208,23 +208,24 @@ public class RobotContainer { // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotTurret.setDefaultCommand( - new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - }, m_robotTurret)); + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + // if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + // }, m_robotTurret)); - m_robotHood.setDefaultCommand( - new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); } - }, m_robotHood)); + // m_robotHood.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + // if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); } + // }, m_robotHood)); m_robotClimber.setDefaultCommand( - new RunCommand(() -> { - if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - if (this.currentMode.equals(Mode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); } - }, m_robotClimber)); + // new RunCommand(() -> { + // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), + getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} + // }, m_robotClimber)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -336,7 +337,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) @@ -349,8 +350,8 @@ public class RobotContainer { // .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); - // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) - // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + .whenPressed(new InstantCommand(() -> this.currentMode = Mode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentMode = Mode.SHOOTER)); // .whenReleased(EnableClimber())); // control turret manual mode diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 07870ff..2300c8e 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,6 +4,7 @@ package frc4388.robot.subsystems; +import java.util.Arrays; import java.util.HashMap; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; @@ -35,6 +36,7 @@ public class Climber extends SubsystemBase { private WPI_Pigeon2 m_gyro; private Point tPoint; + private double[] tJointAngles; private boolean groundRelative; private double shoulderSpeedLimiter; @@ -50,7 +52,9 @@ public class Climber extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - // setClimberGains(); + tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; + + setClimberGains(); // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); // elbowStartPosition = m_elbow.getSelectedSensorPosition(); @@ -144,44 +148,53 @@ public class Climber extends SubsystemBase { } public void setJointAngles(double[] angles) { - System.out.println(angles); + System.out.println(Arrays.toString(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_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 + SmartDashboard.putString("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; - shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO; - elbowPosition *= ClimberConstants.ELBOW_GB_RATIO; + // 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); + m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); + m_elbow.set(TalonFXControlMode.Position, elbowAngle); } - public void controlWithInput(double xInput, double yInput) { + public void controlPointWithInput(double xInput, double yInput) { tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02; tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; } + public void controlJointsWithInput(double shoulderInput, double elbowInput) { + tJointAngles[0] += shoulderInput * ClimberConstants.MOVE_SPEED * .02; + tJointAngles[1] += elbowInput * ClimberConstants.MOVE_SPEED * .02; + } + + int pCount = 0; @Override public void periodic() { SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - // setJointAngles(jointAngles); + if(pCount % 1 == 0) + setJointAngles(tJointAngles); + + pCount ++; // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); From c434b616ca4fa513310bb2ffd2ae26a196a783ff Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 17:39:06 -0600 Subject: [PATCH 135/180] turret manual switching KINDA --- .../java/frc4388/robot/RobotContainer.java | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 10acaaf..decf65e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -152,10 +152,15 @@ public class RobotContainer { public static boolean softLimits = true; - // mode switching + // control mode switching private enum ControlMode { SHOOTER, CLIMBER }; private ControlMode currentControlMode = ControlMode.SHOOTER; + // turret mode switching + private enum TurretMode { MANUAL, AUTONOMOUS }; + private TurretMode currentTurretMode = TurretMode.MANUAL; + + // climber mode switching private enum ClimberMode { MANUAL, AUTONOMOUS }; private ClimberMode currentClimberMode = ClimberMode.MANUAL; @@ -209,7 +214,9 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + } if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); @@ -340,11 +347,17 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentClimberMode = ClimberMode.AUTONOMOUS)) - // TODO: actual climber autonomous command goes here (next line) + .whenPressed(new InstantCommand(() -> { + this.currentClimberMode = ClimberMode.AUTONOMOUS; + })) .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS)) + .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) + .whenReleased(new InstantCommand(() -> this.currentTurretMode = TurretMode.MANUAL)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); From bc95d7a0b8fcca5852a577a3317e37be8cbcf5e2 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 17:44:09 -0600 Subject: [PATCH 136/180] Update Constants.java --- src/main/java/frc4388/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d409cae..90a20db 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -209,7 +209,7 @@ public final class Constants { public static final int ELBOW_PID_LOOP_IDX = 1; public static final Gains SHOULDER_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.2); - public static final Gains ELBOW_GAINS = new Gains(0.5, 0.0, 30.0, 0.0, 0, 1.0); //Prev P 1 d 0.4 + public static final Gains ELBOW_GAINS = new Gains(0.5, 1.0, 20.0, 0.0, 0, 1.0); //Prev P 1 d 0.4 public static final int CLIMBER_TIMEOUT_MS = 50; From e1c17ca9db789046c9978c28721fd33ccf309a07 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 18:25:55 -0600 Subject: [PATCH 137/180] managaestor --- src/main/java/frc4388/robot/RobotContainer.java | 6 +++--- .../robot/commands/StorageCommands/ManageStorage.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Climber.java | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 486cfb5..1d3cc08 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -199,9 +199,9 @@ public class RobotContainer { // m_robotTurret).withName("Storage ManageStorage defaultCommand")); // Storage Management - m_robotStorage.setDefaultCommand( - new RunCommand(() -> m_robotStorage.manageStorage(), - m_robotStorage).withName("Storage manageStorage defaultCommand")); + // m_robotStorage.setDefaultCommand( + // new RunCommand(() -> m_robotStorage.manageStorage(), + // m_robotStorage).withName("Storage manageStorage defaultCommand")); // Serializer Management m_robotSerializer.setDefaultCommand( diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java index 44cc062..df54662 100644 --- a/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java +++ b/src/main/java/frc4388/robot/commands/StorageCommands/ManageStorage.java @@ -60,8 +60,8 @@ public class ManageStorage extends CommandBase { } private void checkColor() { - this.alliance = this.storage.getColor(); - rightColor = this.alliance.equals(Robot.alliance); + // this.alliance = this.storage.getColor(); + // rightColor = this.alliance.equals(Robot.alliance); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 2300c8e..c3941ed 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -148,7 +148,7 @@ public class Climber extends SubsystemBase { } public void setJointAngles(double[] angles) { - System.out.println(Arrays.toString(angles)); + // System.out.println(Arrays.toString(angles)); setJointAngles(angles[0], angles[1]); } From 810538ba3911af4276ffa448a5118521ca74263d Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Fri, 18 Mar 2022 18:26:10 -0600 Subject: [PATCH 138/180] merge fix --- src/main/java/frc4388/robot/Constants.java | 13 +- .../frc4388/robot/subsystems/Climber.java | 150 ++++++++++++------ 2 files changed, 111 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 90a20db..76fdf67 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -202,14 +202,19 @@ public final class Constants { public static final double ELBOW_REVERSE_SOFT_LIMIT = 0; // PID Constants - public static final int SHOULDER_SLOT_IDX = 0; + public static final int SHOULDER_POSITION_SLOT_IDX = 0; + public static final int SHOULDER_VELOCITY_SLOT_IDX = 1; public static final int SHOULDER_PID_LOOP_IDX = 1; - public static final int ELBOW_SLOT_IDX = 0; + public static final int ELBOW_POSITION_SLOT_IDX = 0; + public static final int ELBOW_VELOCITY_SLOT_IDX = 1; public static final int ELBOW_PID_LOOP_IDX = 1; - public static final Gains SHOULDER_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.2); - public static final Gains ELBOW_GAINS = new Gains(0.5, 1.0, 20.0, 0.0, 0, 1.0); //Prev P 1 d 0.4 + public static final Gains SHOULDER_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); + public static final Gains ELBOW_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); + + public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); + public static final Gains ELBOW_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); 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 2300c8e..70a2882 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -36,7 +36,9 @@ public class Climber extends SubsystemBase { private WPI_Pigeon2 m_gyro; private Point tPoint; + private double[] tJointAngles; + private double[] tJointSpeeds; private boolean groundRelative; private double shoulderSpeedLimiter; @@ -52,9 +54,10 @@ public class Climber extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; + setClimberPositionGains(); + setClimberVelocityGains(); - setClimberGains(); + tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; // shoulderStartPosition = m_shoulder.getSelectedSensorPosition(); // elbowStartPosition = m_elbow.getSelectedSensorPosition(); @@ -87,54 +90,78 @@ public class Climber extends SubsystemBase { this.elbowSpeedLimiter = 1.0; } - 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); + public void setClimberPositionGains() { + m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_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); + m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_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 usePositionGains() { + m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); } - public void compensateFeedForArmAngles() { - double shoulderPosition = m_shoulder.getSelectedSensorPosition(); - double elbowPosition = m_elbow.getSelectedSensorPosition(); + public void setClimberVelocityGains() { + m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + m_shoulder.config_kF(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kP(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kI(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_shoulder.config_kD(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - 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); + m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + m_elbow.config_kF(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kP(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kI(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + m_elbow.config_kD(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); } + public void useVelocityGains() { + m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + } + + // 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() { + // 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 * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); @@ -160,7 +187,7 @@ public class Climber extends SubsystemBase { // elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle; // // Convert radians to ticks - SmartDashboard.putString("angles", shoulderAngle + ", " + elbowAngle); + // 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; @@ -168,16 +195,43 @@ public class Climber extends SubsystemBase { // shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO; // elbowPosition *= ClimberConstants.ELBOW_GB_RATIO; - // shoulderPosition += m_shoulderOffset; - // elbowPosition += m_elbowOffset; + // // shoulderPosition += m_shoulderOffset; + // // elbowPosition += m_elbowOffset; m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); m_elbow.set(TalonFXControlMode.Position, elbowAngle); } - public void controlPointWithInput(double xInput, double yInput) { - tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02; - tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02; + public void setJointSpeeds(double[] speeds) { + setJointSpeeds(speeds[0], speeds[1]); + } + + public void setJointSpeeds(double shoulderSpeed, double elbowSpeed) { + m_shoulder.set(TalonFXControlMode.Velocity, shoulderSpeed); + m_elbow.set(TalonFXControlMode.Velocity, elbowSpeed); + } + + boolean movingPrev = false; + public void controlWithInput(double xInput, double yInput) { + boolean moving = xInput != 0 && yInput != 0; + + if(movingPrev != moving) { + if(moving) { + useVelocityGains(); + } else { + usePositionGains(); + tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; + } + } + + if(moving) { + double[] jointSpeeds = new double[] {xInput * ClimberConstants.MOVE_SPEED, yInput * ClimberConstants.MOVE_SPEED}; + setJointSpeeds(jointSpeeds); + } else { + setJointAngles(tJointAngles); + } + + movingPrev = moving; } public void controlJointsWithInput(double shoulderInput, double elbowInput) { From 30d9b49fbde45de4b162196b711bef1852ad8c69 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 18:29:53 -0600 Subject: [PATCH 139/180] broken button box stuff --- .../java/frc4388/robot/RobotContainer.java | 31 ++++++++++++++++--- 1 file changed, 26 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1d3cc08..800127c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,6 +69,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; +import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto; import frc4388.robot.commands.ClimberCommands.RunClaw; import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; @@ -153,8 +154,8 @@ public class RobotContainer { public static boolean softLimits = true; // control mode switching - private enum ControlMode { SHOOTER, CLIMBER }; - private ControlMode currentControlMode = ControlMode.SHOOTER; + public static enum ControlMode { SHOOTER, CLIMBER }; + public static ControlMode currentControlMode = ControlMode.SHOOTER; // turret mode switching private enum TurretMode { MANUAL, AUTONOMOUS }; @@ -359,11 +360,31 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + // .whenPressed(new InstantCommand(() -> { - // this.currentClimberMode = ClimberMode.AUTONOMOUS; + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.AUTONOMOUS; } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.AUTONOMOUS; } // })) - // .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) - // .whenReleased(new InstantCommand(() -> this.currentClimberMode = ClimberMode.MANUAL)); + + // // * custom Command inside InstantCommand + // .whenPressed(new InstantCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry); } + // }) + // .until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) + + // // * external RunTurretOrClimberAutos command, which runs either one (conditionally) based on currentControlMode + // .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws)) + + // // * CommandChooser with BooleanSuppliers + + // // .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) + // // .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) + + // .whenReleased(new InstantCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.MANUAL; } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.MANUAL; } + // })); // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) // .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS)) From e52ba2fe4c72f50836ebce162d3e976f39b70d03 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 18:31:56 -0600 Subject: [PATCH 140/180] fix --- src/main/java/frc4388/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 800127c..f584997 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -69,7 +69,6 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; -import frc4388.robot.commands.ButtonBoxCommands.RunTurretOrClimberAuto; import frc4388.robot.commands.ClimberCommands.RunClaw; import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; From e51f86e9c1d7e6c81e34965878e24e2366c42723 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 18:33:22 -0600 Subject: [PATCH 141/180] fixed merge conflict before it even happens lol --- .../java/frc4388/robot/RobotContainer.java | 50 ------------------- 1 file changed, 50 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f584997..68ea963 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -337,58 +337,8 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - - // .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) - // .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) - // .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) - - // .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) - // .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) - // .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( - // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); - .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); - // .whenReleased(EnableClimber())); - - // control turret manual mode - // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // .whileHeld(new RunCommand(() -> TurretManual.setManual(true))) - // .whenReleased(new RunCommand(() -> TurretManual.setManual(false))); - - // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - - // .whenPressed(new InstantCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.AUTONOMOUS; } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.AUTONOMOUS; } - // })) - - // // * custom Command inside InstantCommand - // .whenPressed(new InstantCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}); } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry); } - // }) - // .until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) - - // // * external RunTurretOrClimberAutos command, which runs either one (conditionally) based on currentControlMode - // .whenPressed(new RunTurretOrClimberAuto(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry, m_robotClimber, m_robotClaws)) - - // // * CommandChooser with BooleanSuppliers - - // // .whenPressed(new RunClimberPath(m_robotClimber, m_robotClaws, new Point[] {new Point()}).until(() -> this.currentClimberMode.equals(ClimberMode.MANUAL))) - // // .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) - - // .whenReleased(new InstantCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { this.currentTurretMode = TurretMode.MANUAL; } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { this.currentClimberMode = ClimberMode.MANUAL; } - // })); - - // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) - // .whenPressed(new InstantCommand(() -> this.currentTurretMode = TurretMode.AUTONOMOUS)) - // .whenPressed(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry).until(() -> this.currentTurretMode.equals(TurretMode.MANUAL))) - // .whenReleased(new InstantCommand(() -> this.currentTurretMode = TurretMode.MANUAL)); new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) From e19dc2d77c25436ef6999ee83519a636e2d699fd Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 20:02:23 -0600 Subject: [PATCH 142/180] private > public, return to swerve rotation --- .../robot/commands/ShooterCommands/Shoot.java | 60 ++++++++++--------- 1 file changed, 33 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index aabac8f..5f4ae4c 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -20,40 +20,42 @@ import frc4388.utility.Gains; public class Shoot extends CommandBase { // subsystems - public SwerveDrive m_swerve; - public BoomBoom m_boomBoom; - public Turret m_turret; - public Hood m_hood; + private SwerveDrive m_swerve; + private BoomBoom m_boomBoom; + private Turret m_turret; + private Hood m_hood; // given - public double m_gyroAngle; - public double m_odoX; - public double m_odoY; - public double m_distance; + private double m_gyroAngle; + private double m_odoX; + private double m_odoY; + private double m_distance; // targets - public double m_targetVel; - public double m_targetHood; - public double m_targetAngle; - public Pose2d m_targetPoint; + private double m_targetVel; + private double m_targetHood; + private double m_targetAngle; + private Pose2d m_targetPoint; // pid - public double error; - public double prevError; - public Gains gains = ShooterConstants.SHOOT_GAINS; - public double kP, kI, kD; - public double proportional, integral, derivative; - public double time; - public double output; - public double normOutput; - public double tolerance; - public boolean isAimedInTolerance; - public int inverted; + private double error; + private double prevError; + private Gains gains = ShooterConstants.SHOOT_GAINS; + private double kP, kI, kD; + private double proportional, integral, derivative; + private double time; + private double output; + private double normOutput; + private double tolerance; + + private boolean isAimedInTolerance; + private int inverted; + private double initialSwerveRotation; // testing - public boolean simMode = true; - public DummySensor driveDummy; - public DummySensor turretDummy; + private boolean simMode = true; + private DummySensor driveDummy; + private DummySensor turretDummy; /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball @@ -117,6 +119,7 @@ public class Shoot extends CommandBase { m_odoY = -1;//m_swerve.getOdometry().getY(); m_gyroAngle = m_swerve.getRegGyro().getDegrees(); + initialSwerveRotation = m_gyroAngle; // get targets (shooter tables) m_targetVel = m_boomBoom.getVelocity(m_distance); @@ -182,7 +185,10 @@ public class Shoot extends CommandBase { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + // return to initial swerve rotation + m_swerve.driveWithInput(0, 0, initialSwerveRotation, true); + } // Returns true when the command should end. @Override From 46396a704f42183ee09bfa83fcd3b0348cd35240 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 23:07:53 -0600 Subject: [PATCH 143/180] qawsedrftgyhujikolp --- src/main/java/frc4388/robot/Constants.java | 18 +++---- src/main/java/frc4388/robot/Robot.java | 18 +++---- .../java/frc4388/robot/RobotContainer.java | 14 ++--- .../frc4388/robot/subsystems/Climber.java | 54 ++++++++++--------- .../frc4388/robot/subsystems/SwerveDrive.java | 12 +++-- 5 files changed, 63 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 76fdf67..1b73301 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -36,7 +36,7 @@ public final class Constants { public static final double TICKS_PER_ROTATION_FX = 2048; public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 4.0; + public static final double ROTATION_SPEED = 3.0; public static final double WIDTH = 23.5; public static final double HEIGHT = 23.5; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -203,18 +203,18 @@ public final class Constants { // PID Constants public static final int SHOULDER_POSITION_SLOT_IDX = 0; - public static final int SHOULDER_VELOCITY_SLOT_IDX = 1; - public static final int SHOULDER_PID_LOOP_IDX = 1; + public static final int SHOULDER_VELOCITY_SLOT_IDX = 0; + public static final int SHOULDER_PID_LOOP_IDX = 0; public static final int ELBOW_POSITION_SLOT_IDX = 0; - public static final int ELBOW_VELOCITY_SLOT_IDX = 1; - public static final int ELBOW_PID_LOOP_IDX = 1; + public static final int ELBOW_VELOCITY_SLOT_IDX = 0; + public static final int ELBOW_PID_LOOP_IDX = 0; - public static final Gains SHOULDER_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); - public static final Gains ELBOW_POSITION_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); + public static final Gains SHOULDER_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2); + public static final Gains ELBOW_POSITION_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.2); - public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); - public static final Gains ELBOW_VELOCITY_GAINS = new Gains(1.0, 0.0, 0.1, 0.0, 0, 0.2); + public static final Gains SHOULDER_VELOCITY_GAINS = new Gains(0.2, 0.0, 5.0, 0.0, 0, 0.2); + public static final Gains ELBOW_VELOCITY_GAINS = new Gains(0.3, 0.005, 5.0, 0.0, 0, 1.0); public static final int CLIMBER_TIMEOUT_MS = 50; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index f01d45a..a0d5e5c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -143,15 +143,15 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); current = - m_robotContainer.m_robotBoomBoom.getCurrent() + - m_robotContainer.m_robotClimber.getCurrent() + - m_robotContainer.m_robotHood.getCurrent() + - m_robotContainer.m_robotIntake.getCurrent() + - m_robotContainer.m_robotExtender.getCurrent() + - m_robotContainer.m_robotSerializer.getCurrent() + - m_robotContainer.m_robotStorage.getCurrent() + - m_robotContainer.m_robotSwerveDrive.getCurrent(); - m_robotContainer.m_robotTurret.getCurrent(); + // m_robotContainer.m_robotBoomBoom.getCurrent() + + m_robotContainer.m_robotClimber.getCurrent(); //+ + // m_robotContainer.m_robotHood.getCurrent() + + // m_robotContainer.m_robotIntake.getCurrent() + + // m_robotContainer.m_robotExtender.getCurrent() + + // m_robotContainer.m_robotSerializer.getCurrent() + + // m_robotContainer.m_robotStorage.getCurrent() + + // m_robotContainer.m_robotSwerveDrive.getCurrent(); + // m_robotContainer.m_robotTurret.getCurrent(); SmartDashboard.putNumber("Total Robot Current Draw", current); SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1d3cc08..3273411 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -173,8 +173,8 @@ public class RobotContainer { // IK command m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), - getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( @@ -226,11 +226,11 @@ public class RobotContainer { // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } // }, m_robotHood)); - m_robotClimber.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(), - getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} + // m_robotClimber.setDefaultCommand( + // // new RunCommand(() -> { + // // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + // /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), + // getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} // }, m_robotClimber)); // m_robotTurret.setDefaultCommand( diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index d9b57f0..15d5a90 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -54,8 +54,9 @@ public class Climber extends SubsystemBase { m_elbow.configFactoryDefault(); m_elbow.setNeutralMode(NeutralMode.Brake); - setClimberPositionGains(); + // setClimberPositionGains(); setClimberVelocityGains(); + useVelocityGains(); tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; @@ -90,24 +91,24 @@ public class Climber extends SubsystemBase { this.elbowSpeedLimiter = 1.0; } - public void setClimberPositionGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); + // public void setClimberPositionGains() { + // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + // m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - } + // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + // m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); + // m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); + // } - public void usePositionGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - } + // public void usePositionGains() { + // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); + // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); + // } public void setClimberVelocityGains() { m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); @@ -198,8 +199,8 @@ public class Climber extends SubsystemBase { // // shoulderPosition += m_shoulderOffset; // // elbowPosition += m_elbowOffset; - m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); - m_elbow.set(TalonFXControlMode.Position, elbowAngle); + // m_shoulder.set(TalonFXControlMode.Position, shoulderAngle); + // m_elbow.set(TalonFXControlMode.Position, elbowAngle); } public void setJointSpeeds(double[] speeds) { @@ -212,15 +213,18 @@ public class Climber extends SubsystemBase { } boolean movingPrev = false; + boolean moving; public void controlWithInput(double xInput, double yInput) { - boolean moving = xInput != 0 && yInput != 0; + moving = xInput != 0 || yInput != 0; if(movingPrev != moving) { if(moving) { useVelocityGains(); + SmartDashboard.putString("Climber Gains", "Velocity"); } else { - usePositionGains(); + // usePositionGains(); tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; + SmartDashboard.putString("Climber Gains", "Position"); } } @@ -242,13 +246,15 @@ public class Climber extends SubsystemBase { int pCount = 0; @Override public void periodic() { + SmartDashboard.putBoolean("Moving", moving); + SmartDashboard.putBoolean("MovingPrev", movingPrev); SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - if(pCount % 1 == 0) - setJointAngles(tJointAngles); + // if(pCount % 1 == 0) + // setJointAngles(tJointAngles); - pCount ++; + // pCount ++; // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 555ef83..24727e7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -116,7 +117,7 @@ public class SwerveDrive extends SubsystemBase { Translation2d speed = new Translation2d(leftX, leftY); speed = speed.times(speed.getNorm() * speedAdjust); if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) - rotTarget = new Rotation2d(rightX, rightY); + rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); @@ -199,9 +200,12 @@ public class SwerveDrive extends SubsystemBase { * * @return Rotation2d object holding current gyro in radians */ - public Rotation2d getRegGyro() { - double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; - return new Rotation2d(regCur * Math.PI / 180); + public Rotation2d getRegGyro() { + // * test chassis regression + // double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; + // * new robot regression + double regCur = 0.2507023948 + m_gyro.getRotation2d().getDegrees() * 0.999034743; + return new Rotation2d(Math.toRadians(regCur)); } /** From 6570f81cf7899bf19b74d1cd9c2ac9ae0126f9a8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 23:11:30 -0600 Subject: [PATCH 144/180] swerve rotation test button --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e5f83c6..9f46fa2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -263,8 +263,12 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); // Right Bumper > Shift Up - new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) - .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + new JoystickButton(getDriverController(), XboxController.Button.kA.value) + .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true)) + .whenReleased(() -> m_robotSwerveDrive.stopModules()); /* Operator Buttons */ From cc6c455475aa37b3ad2b26d1e97feed30e7f1918 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 23:54:16 -0600 Subject: [PATCH 145/180] aarav good math --- .../robot/commands/ShooterCommands/AimToCenter.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 38a8888..083eec7 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -60,6 +60,18 @@ public class AimToCenter extends CommandBase { return angle; } + public static double aaravAngleToCenter(double x, double y, double gyro) { + double exp = Math.toDegrees(Math.atan(y/x)) - gyro; + if (x > 0) { return exp; } + if (x < 0) { return (180 + exp); } + + if (x == 0 && y > 0) { return (90 - gyro); } + if (x == 0 && y < 0) { return (-1 * gyro); } + + System.out.println("Invalid case."); + return 0; + } + /** * Checks if in deadzone. * @param angle Angle to check. From 152c79f1fa43fa2b78e0ebdaa17ef2cc828d3a67 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 23:54:59 -0600 Subject: [PATCH 146/180] change --- .../frc4388/robot/commands/ShooterCommands/AimToCenter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 083eec7..cfe8fd6 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -66,7 +66,7 @@ public class AimToCenter extends CommandBase { if (x < 0) { return (180 + exp); } if (x == 0 && y > 0) { return (90 - gyro); } - if (x == 0 && y < 0) { return (-1 * gyro); } + if (x == 0 && y < 0) { return (-90 - gyro); } System.out.println("Invalid case."); return 0; From 113a26d831359cfc146d12f836bc3f5c685b3c35 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 23:56:01 -0600 Subject: [PATCH 147/180] se5dr --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 42 +++++++++++-------- .../commands/ShooterCommands/AimToCenter.java | 15 +++---- 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a0d5e5c..da66be3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -256,7 +256,7 @@ public class Robot extends TimedRobot { Robot.alliance = DriverStation.getAlliance(); - 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()); selectedOdo = odoChooser.getSelected(); if (selectedOdo == null) { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9f46fa2..866fafb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -212,26 +212,26 @@ public class RobotContainer { // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { - // if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - // } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - // }, m_robotTurret)); + m_robotTurret.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + }, m_robotTurret)); - // m_robotHood.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } - // }, m_robotHood)); + m_robotHood.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } + }, m_robotHood)); - // m_robotClimber.setDefaultCommand( - // // new RunCommand(() -> { - // // if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - // /*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), - // getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //} - // }, m_robotClimber)); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + getOperatorController().getRightY() * 10000); }} + , m_robotClimber)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -269,6 +269,12 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kA.value) .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); + + new JoystickButton(getDriverController(), XboxController.Button.kB.value) + .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); + + /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 38a8888..df612be 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -35,29 +35,30 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - x = 0; - y = 0; + x = m_drive.getOdometry().getX(); + y = m_drive.getOdometry().getY(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); + System.out.println("Target Angle" + m_targetAngle); m_turret.runShooterRotatePID(m_targetAngle); // Check if limelight is within range (comment out to disable vision odo) if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){ - m_visionOdometry.updateOdometryWithVision(); - m_visionOdometry.setLEDs(true); + // m_visionOdometry.updateOdometryWithVision(); + // m_visionOdometry.setLEDs(true); } else{ - m_visionOdometry.setLEDs(false); + // m_visionOdometry.setLEDs(false); } } public static double angleToCenter(double x, double y, double gyro) { - double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) - return angle; + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + return (angle - 360); } /** From 87de6a2c2f3e3b55cf24b6b2a3421305c866d0c3 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 11:25:29 -0600 Subject: [PATCH 148/180] try change --- .../robot/commands/ShooterCommands/AimToCenter.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index fad71f7..c881512 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -42,7 +42,7 @@ public class AimToCenter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); + m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getRegGyro().getDegrees())) % 360; System.out.println("Target Angle" + m_targetAngle); m_turret.runShooterRotatePID(m_targetAngle); @@ -62,12 +62,13 @@ public class AimToCenter extends CommandBase { } public static double aaravAngleToCenter(double x, double y, double gyro) { - double exp = Math.toDegrees(Math.atan(y/x)) - gyro; + double yes = 360 - gyro; + double exp = Math.toDegrees(Math.atan(y/x)) - yes; if (x > 0) { return exp; } if (x < 0) { return (180 + exp); } - if (x == 0 && y > 0) { return (90 - gyro); } - if (x == 0 && y < 0) { return (-90 - gyro); } + if (x == 0 && y > 0) { return (90 - yes); } + if (x == 0 && y < 0) { return (-90 - yes); } System.out.println("Invalid case."); return 0; From dbc1e070e0ec115311d7f48333b469525fd6027f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 16:37:59 -0600 Subject: [PATCH 149/180] AimToCenter works without fixing odometry issues --- src/main/java/frc4388/robot/Constants.java | 28 +++++++++---------- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 4 ++- .../commands/ShooterCommands/AimToCenter.java | 28 +++++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 22 +++++++-------- .../robot/subsystems/SwerveModule.java | 5 ++-- .../java/frc4388/robot/subsystems/Turret.java | 10 +++++++ 7 files changed, 58 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1b73301..26b599c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -37,26 +37,26 @@ public final class Constants { public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 3.0; - public static final double WIDTH = 23.5; - public static final double HEIGHT = 23.5; + public static final double WIDTH = 23.75; + public static final double HEIGHT = 23.75; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2; public static final double MAX_SPEED_FEET_PER_SEC = 20; // TODO: redundant constant? public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant? // IDs - public static final int LEFT_FRONT_STEER_CAN_ID = 2; // - public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // - public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // - public static final int LEFT_BACK_STEER_CAN_ID = 6; // - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // - public static final int RIGHT_BACK_STEER_CAN_ID = 8; // - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;// - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // + public static final int LEFT_FRONT_STEER_CAN_ID = 2; // * + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // * + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // * + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // * + public static final int LEFT_BACK_STEER_CAN_ID = 6; // * + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // * + public static final int RIGHT_BACK_STEER_CAN_ID = 8; // * + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // * + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // * + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // * + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // * + public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // * public static final int GYRO_ID = 14; // offsets are in degrees diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index da66be3..a28a7e7 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -262,7 +262,7 @@ public class Robot extends TimedRobot { if (selectedOdo == null) { selectedOdo = m_robotContainer.getOdometry(); } - m_robotContainer.resetOdometry(selectedOdo); + // m_robotContainer.resetOdometry(selectedOdo); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 866fafb..15810a5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -273,8 +273,10 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kB.value) .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); - + new JoystickButton(getDriverController(), XboxController.Button.kY.value) + .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-45), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index c881512..882e010 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -4,6 +4,7 @@ package frc4388.robot.commands.ShooterCommands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; @@ -35,15 +36,18 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - x = m_drive.getOdometry().getX(); - y = m_drive.getOdometry().getY(); + x = -m_drive.getOdometry().getY(); + y = -m_drive.getOdometry().getX(); + + SmartDashboard.putNumber("trans x", x); + SmartDashboard.putNumber("trans y", y); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getRegGyro().getDegrees())) % 360; - System.out.println("Target Angle" + m_targetAngle); + m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getOdometry().getRotation().getDegrees())) % 360; + SmartDashboard.putNumber("Target Angle", m_targetAngle); m_turret.runShooterRotatePID(m_targetAngle); // Check if limelight is within range (comment out to disable vision odo) @@ -62,15 +66,17 @@ public class AimToCenter extends CommandBase { } public static double aaravAngleToCenter(double x, double y, double gyro) { - double yes = 360 - gyro; - double exp = Math.toDegrees(Math.atan(y/x)) - yes; - if (x > 0) { return exp; } - if (x < 0) { return (180 + exp); } + + double actualGyro = -gyro + 90; + + double exp = Math.toDegrees(Math.atan(y/-x)) - actualGyro; + if (-x > 0) { return (180 + exp); } + if (-x < 0) { return (360 + exp); } - if (x == 0 && y > 0) { return (90 - yes); } - if (x == 0 && y < 0) { return (-90 - yes); } + if (x == 0 && y > 0) { return (270 - actualGyro); } + if (x == 0 && y < 0) { return (90 - actualGyro); } - System.out.println("Invalid case."); + System.err.println("Invalid case."); return 0; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 24727e7..6399fbe 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -36,10 +36,10 @@ public class SwerveDrive extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; - Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth)); - Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth)); - Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth)); - Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth)); + Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(-halfHeight)); + Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfWidth), Units.inchesToMeters(halfHeight)); + Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(-halfHeight)); + Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfWidth), Units.inchesToMeters(halfHeight)); public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); @@ -48,7 +48,7 @@ public class SwerveDrive extends SubsystemBase { public WPI_Pigeon2 m_gyro; public SwerveDrivePoseEstimator m_poseEstimator; - public SwerveDriveOdometry m_odometry; + // public SwerveDriveOdometry m_odometry; public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; @@ -69,14 +69,14 @@ public class SwerveDrive extends SubsystemBase { modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack}; m_poseEstimator = new SwerveDrivePoseEstimator( - m_gyro.getRotation2d(), + getRegGyro(),//m_gyro.getRotation2d(), new Pose2d(), m_kinematics, VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune - m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); + // m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); m_gyro.reset(); SmartDashboard.putData("Field", m_field); @@ -160,7 +160,7 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); - m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); } @@ -172,9 +172,9 @@ public class SwerveDrive extends SubsystemBase { // chassis speeds // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds - // SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); - // SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); - // SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); + SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 11e861e..32bb5ca 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -79,6 +79,7 @@ public class SwerveModule extends SubsystemBase { // driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); + canCoderConfiguration.sensorCoefficient = 0.087890625; canCoderConfiguration.magnetOffsetDegrees = offset; canCoderConfiguration.sensorDirection = true; canCoder.configAllSettings(canCoderConfiguration); @@ -90,9 +91,7 @@ public class SwerveModule extends SubsystemBase { } private Rotation2d getAngle() { - // Note: This assumes the CANCoders are setup with the default feedback - // coefficient - // and the sensor value reports degrees. + // ! Note: This assumes the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 9d9e35c..95a41d0 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.commands.ShooterCommands.Shoot; +import frc4388.robot.subsystems.BoomBoom.ShooterTableEntry; import frc4388.utility.Gains; public class Turret extends SubsystemBase { @@ -195,6 +196,15 @@ public class Turret extends SubsystemBase { } public void runShooterRotatePID(double targetAngle) { + + double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2; + + targetAngle = (targetAngle % 360); + + if (targetAngle > softMid) { + targetAngle = targetAngle - 360; + } + targetAngle = targetAngle / ShooterConstants.TURRET_DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); } From be850585ec7c87c44e7e1ab12c5b7ff097fc3ddf Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 19:58:21 -0600 Subject: [PATCH 150/180] locked motors + shoot kinda works --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 8 +-- .../java/frc4388/robot/RobotContainer.java | 65 ++++++++++--------- src/main/java/frc4388/robot/RobotMap.java | 12 ++-- .../robot/commands/ShooterCommands/Shoot.java | 38 +++++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +++ .../robot/subsystems/SwerveModule.java | 8 +++ 7 files changed, 87 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 26b599c..e5351ea 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -277,7 +277,7 @@ public final class Constants { public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(5.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOT_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index a28a7e7..2232b4c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -142,9 +142,9 @@ public class Robot extends TimedRobot { m_robotTime.updateTimes(); // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); - current = + // current = // m_robotContainer.m_robotBoomBoom.getCurrent() + - m_robotContainer.m_robotClimber.getCurrent(); //+ + // m_robotContainer.m_robotClimber.getCurrent(); //+ // m_robotContainer.m_robotHood.getCurrent() + // m_robotContainer.m_robotIntake.getCurrent() + // m_robotContainer.m_robotExtender.getCurrent() + @@ -152,8 +152,8 @@ public class Robot extends TimedRobot { // m_robotContainer.m_robotStorage.getCurrent() + // m_robotContainer.m_robotSwerveDrive.getCurrent(); // m_robotContainer.m_robotTurret.getCurrent(); - SmartDashboard.putNumber("Total Robot Current Draw", current); - SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); + // SmartDashboard.putNumber("Total Robot Current Draw", current); + // SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 15810a5..1a2ed8c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,7 +112,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + // public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); public 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 Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -172,9 +172,9 @@ public class RobotContainer { /* Default Commands */ // IK command - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + // getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( @@ -208,17 +208,17 @@ public class RobotContainer { new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - m_robotTurret.setDefaultCommand( - new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { - if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - }, m_robotTurret)); + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + // if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + // } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + // }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { @@ -226,12 +226,12 @@ public class RobotContainer { if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); - m_robotClimber.setDefaultCommand( - new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - getOperatorController().getRightY() * 10000); }} - , m_robotClimber)); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> { + // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } + // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, + // getOperatorController().getRightY() * 10000); }} + // , m_robotClimber)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -269,15 +269,22 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kA.value) .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); - + new JoystickButton(getDriverController(), XboxController.Button.kB.value) .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); - - new JoystickButton(getDriverController(), XboxController.Button.kY.value) - .whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-45), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); + + new JoystickButton(getDriverController(), XboxController.Button.kY.value) + .whileHeld(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + .whenReleased(() -> m_robotSwerveDrive.stopModules()); + new JoystickButton(getDriverController(), XboxController.Button.kX.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); + /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) @@ -335,18 +342,18 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) + // .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) + // .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); + // .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 9638c45..3a92ee8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -140,13 +140,13 @@ public class RobotMap { SwerveDriveConstants.SWERVE_TIMEOUT_MS); NeutralMode mode = NeutralMode.Coast; - leftFrontSteerMotor.setNeutralMode(mode); + leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); leftFrontWheelMotor.setNeutralMode(mode);// Coast - rightFrontSteerMotor.setNeutralMode(mode); + rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); rightFrontWheelMotor.setNeutralMode(mode);// Coast - leftBackSteerMotor.setNeutralMode(mode); + leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); leftBackWheelMotor.setNeutralMode(mode);// Coast - rightBackSteerMotor.setNeutralMode(mode); + rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); rightBackWheelMotor.setNeutralMode(mode);// Coast // current limits @@ -196,8 +196,8 @@ 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_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO +// 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); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 5f4ae4c..339785d 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -53,7 +53,7 @@ public class Shoot extends CommandBase { private double initialSwerveRotation; // testing - private boolean simMode = true; + private boolean simMode = false; private DummySensor driveDummy; private DummySensor turretDummy; @@ -99,10 +99,10 @@ public class Shoot extends CommandBase { */ public void updateError() { m_targetPoint = SwerveDriveConstants.HUB_POSE; - m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); - // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); - error = (m_targetAngle - turretDummy.get() + 360) % 360; - // error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); + m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + // error = (m_targetAngle - turretDummy.get() + 360) % 360; + error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); if (simMode) { @@ -115,8 +115,11 @@ public class Shoot extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - m_odoX = 0;//m_swerve.getOdometry().getX(); - m_odoY = -1;//m_swerve.getOdometry().getY(); + + m_turret.gotoMidpoint(); + + m_odoX = 0;//-m_swerve.getOdometry().getY(); + m_odoY = -8;//-m_swerve.getOdometry().getX(); m_gyroAngle = m_swerve.getRegGyro().getDegrees(); initialSwerveRotation = m_gyroAngle; @@ -125,14 +128,15 @@ public class Shoot extends CommandBase { m_targetVel = m_boomBoom.getVelocity(m_distance); m_targetHood = m_boomBoom.getHood(m_distance); - m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; + m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + // m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; // deadzone processing if (AimToCenter.isDeadzone(m_targetAngle)) {} // initial error updateError(); - System.out.println("Error: " + error); + // System.out.println("Error: " + error); prevError = error; } /** @@ -159,22 +163,28 @@ public class Shoot extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + + if (simMode) { System.out.println("Normalized Output: " + normOutput); } - // custom pid - runPID(); - + // custom pid + if (simMode) { driveDummy.apply(normOutput); System.out.println("Drive Dummy: " + driveDummy.get()); } + + runPID(); + SmartDashboard.putNumber("Error", this.error); + SmartDashboard.putNumber("Shoot.java TargetAngle", this.m_targetAngle); + SmartDashboard.putNumber("Normalized Output", normOutput); m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the // entire swerve drive or its the line below // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); - m_hood.runAngleAdjustPID(m_targetHood); - m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + // m_hood.runAngleAdjustPID(m_targetHood); + // m_boomBoom.runDrumShooterVelocityPID(m_targetVel); if (simMode) { turretDummy.apply(normOutput); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6399fbe..2c5600b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -127,6 +127,14 @@ public class SwerveDrive extends SubsystemBase { : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); + if (ignoreAngles) { + SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; + for (int i = 0; i < states.length; i ++) { + SwerveModuleState state = states[i]; + lockedStates[i]= new SwerveModuleState(0, state.angle); + } + setModuleStates(lockedStates); + } setModuleStates(states); // SmartDashboard.putNumber("rot", rot); // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 32bb5ca..d4905af 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -38,6 +38,9 @@ public class SwerveModule extends SubsystemBase { public double m_currentPos; public double m_lastPos; + public SwerveModuleState lastState = new SwerveModuleState(); + public SwerveModuleState currentState; + /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { this.driveMotor = driveMotor; @@ -181,10 +184,15 @@ public class SwerveModule extends SubsystemBase { @Override public void periodic() { + + currentState = this.getState(); + Rotation2d currentRotation = getAngle(); SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360); + + lastState = currentState; } public void reset() { From 6b38e89660b2dcc82db0572481d5030870871a0a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 21:03:01 -0600 Subject: [PATCH 151/180] calibration speed + velocity ramp only if soft limits --- .../java/frc4388/robot/RobotContainer.java | 12 ++- .../commands/ShooterCommands/AimToCenter.java | 30 +++++-- .../robot/commands/ShooterCommands/Shoot.java | 84 ++++++++++--------- .../java/frc4388/robot/subsystems/Hood.java | 32 ++++--- .../java/frc4388/robot/subsystems/Turret.java | 35 +++++--- 5 files changed, 116 insertions(+), 77 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1a2ed8c..954a0fc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -271,7 +271,7 @@ public class RobotContainer { .whenReleased(() -> m_robotSwerveDrive.stopModules()); new JoystickButton(getDriverController(), XboxController.Button.kB.value) - .whileHeld(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)) + .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); new JoystickButton(getDriverController(), XboxController.Button.kY.value) @@ -340,12 +340,20 @@ public class RobotContainer { new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) // .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) - + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) // .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 882e010..de4b234 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -4,6 +4,9 @@ package frc4388.robot.commands.ShooterCommands; +import java.util.function.Supplier; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ShooterConstants; @@ -15,9 +18,11 @@ import frc4388.robot.subsystems.VisionOdometry; public class AimToCenter extends CommandBase { /** Creates a new AimWithOdometry. */ Turret m_turret; - SwerveDrive m_drive; VisionOdometry m_visionOdometry; + Supplier supplier; + Pose2d odo; + // use odometry to find x and y later double x; double y; @@ -25,28 +30,35 @@ public class AimToCenter extends CommandBase { // public static Gains m_aimGains; - public AimToCenter(Turret turret, SwerveDrive drive, VisionOdometry visionOdometry) { + public AimToCenter(Turret turret, VisionOdometry visionOdometry, Supplier supplier) { // Use addRequirements() here to declare subsystem dependencies. m_turret = turret; - m_drive = drive; m_visionOdometry = visionOdometry; - addRequirements(m_turret, m_drive, m_visionOdometry); + + this.supplier = supplier; + + addRequirements(m_turret, m_visionOdometry); } // Called when the command is initially scheduled. @Override public void initialize() { - x = -m_drive.getOdometry().getY(); - y = -m_drive.getOdometry().getX(); - + odo = this.supplier.get(); + // ! Yes I realize this stupid, yes it works I promise, coordinate system is funky + x = -odo.getY(); + y = -odo.getX(); + SmartDashboard.putNumber("trans x", x); SmartDashboard.putNumber("trans y", y); } - + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = (aaravAngleToCenter(x, y, m_drive.getOdometry().getRotation().getDegrees())) % 360; + + odo = this.supplier.get(); // * update odometry using really cool supplier -aarav + + m_targetAngle = (aaravAngleToCenter(x, y, odo.getRotation().getDegrees())) % 360; SmartDashboard.putNumber("Target Angle", m_targetAngle); m_turret.runShooterRotatePID(m_targetAngle); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 339785d..f12e518 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -20,22 +20,22 @@ import frc4388.utility.Gains; public class Shoot extends CommandBase { // subsystems - private SwerveDrive m_swerve; - private BoomBoom m_boomBoom; - private Turret m_turret; - private Hood m_hood; + private SwerveDrive swerve; + private BoomBoom drum; + private Turret turret; + private Hood hood; // given - private double m_gyroAngle; - private double m_odoX; - private double m_odoY; - private double m_distance; + private double gyroAngle; + private double odoX; + private double odoY; + private double distance; // targets - private double m_targetVel; - private double m_targetHood; - private double m_targetAngle; - private Pose2d m_targetPoint; + private double targetVel; + private double targetHood; + private double targetAngle; + private Pose2d targetPoint; // pid private double error; @@ -59,20 +59,22 @@ public class Shoot extends CommandBase { /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball - * TODO: Velocity Correction - * @param sDrive Drive Train - * @param sShooter Shooter Drum - * @param sTurret Shooter Turret - * @param sHood Shooter Hood + * + * @param swerve Drive Train + * @param drum Shooter Drum + * @param turret Shooter Turret + * @param hood Shooter Hood + * + * @author Aarav Shah */ - public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) { + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { // Use addRequirements() here to declare subsystem dependencies. - m_swerve = sDrive; - m_boomBoom = sShooter; - m_turret = sTurret; - m_hood = sHood; + this.swerve = swerve; + this.drum = drum; + this.turret = turret; + this.hood = hood; - addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); + addRequirements(this.swerve, this.drum, this.turret, this.hood); kP = gains.kP; kI = gains.kI; @@ -98,16 +100,16 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - m_targetPoint = SwerveDriveConstants.HUB_POSE; + targetPoint = SwerveDriveConstants.HUB_POSE; // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); - m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); // error = (m_targetAngle - turretDummy.get() + 360) % 360; - error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; + error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); if (simMode) { SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); - System.out.println("Target Angle: " + m_targetAngle); + System.out.println("Target Angle: " + targetAngle); System.out.println("Error: " + error); } } @@ -116,23 +118,23 @@ public class Shoot extends CommandBase { @Override public void initialize() { - m_turret.gotoMidpoint(); + turret.gotoMidpoint(); - m_odoX = 0;//-m_swerve.getOdometry().getY(); - m_odoY = -8;//-m_swerve.getOdometry().getX(); + odoX = 0;//-m_swerve.getOdometry().getY(); + odoY = -8;//-m_swerve.getOdometry().getX(); - m_gyroAngle = m_swerve.getRegGyro().getDegrees(); - initialSwerveRotation = m_gyroAngle; + gyroAngle = swerve.getRegGyro().getDegrees(); + initialSwerveRotation = gyroAngle; // get targets (shooter tables) - m_targetVel = m_boomBoom.getVelocity(m_distance); - m_targetHood = m_boomBoom.getHood(m_distance); + targetVel = drum.getVelocity(distance); + targetHood = drum.getHood(distance); - m_targetAngle = AimToCenter.aaravAngleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees()); + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); // m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; // deadzone processing - if (AimToCenter.isDeadzone(m_targetAngle)) {} + if (AimToCenter.isDeadzone(targetAngle)) {} // initial error updateError(); @@ -174,12 +176,12 @@ public class Shoot extends CommandBase { driveDummy.apply(normOutput); System.out.println("Drive Dummy: " + driveDummy.get()); } - + runPID(); SmartDashboard.putNumber("Error", this.error); - SmartDashboard.putNumber("Shoot.java TargetAngle", this.m_targetAngle); + SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); SmartDashboard.putNumber("Normalized Output", normOutput); - m_swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the + swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the // entire swerve drive or its the line below // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); @@ -190,14 +192,14 @@ public class Shoot extends CommandBase { turretDummy.apply(normOutput); System.out.println("Turret Dummy: " + turretDummy.get()); } - m_turret.m_boomBoomRotateMotor.set(normOutput); + turret.m_boomBoomRotateMotor.set(normOutput); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { // return to initial swerve rotation - m_swerve.driveWithInput(0, 0, initialSwerveRotation, true); + swerve.driveWithInput(0, 0, initialSwerveRotation, true); } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 5f720d4..6176c9a 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -35,6 +35,7 @@ public class Hood extends SubsystemBase { public double m_fireAngle; public double speedLimiter; + public double calibrationSpeed = 1.0; /** Creates a new Hood. */ public Hood(CANSparkMax angleAdjusterMotor) { @@ -63,23 +64,30 @@ public class Hood extends SubsystemBase { SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). - double currentPos = this.getEncoderPosition(); - double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); - double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); - if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); - } + if (areSoftLimitsEnabled()) { + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); - if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); - } + if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); + } - if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { - this.speedLimiter = 1.0; + if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } } } + public boolean areSoftLimitsEnabled() { + return this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_angleAdjusterMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse); + } + /** * Set status of hood motor soft limits. * @param set Boolean to set soft limits to. @@ -107,7 +115,7 @@ public class Hood extends SubsystemBase { * @param input value from -1.0 to 1.0, postive is upward (more horizontal shootijng angle) */ public void runHood(double input) { - m_angleAdjusterMotor.set(input * this.speedLimiter); + m_angleAdjusterMotor.set(input * this.speedLimiter * this.calibrationSpeed); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 95a41d0..d8c59d4 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -46,7 +46,8 @@ public class Turret extends SubsystemBase { long leftCurrentTime; long leftElapsedTime; - double speedLimiter; + public double speedLimiter; + public double calibrationSpeed = 1.0; public Turret(CANSparkMax boomBoomRotateMotor) { @@ -148,23 +149,31 @@ public class Turret extends SubsystemBase { // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). - double currentPos = this.getEncoderPosition(); - double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); - double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); - } + if (areSoftLimitsEnabled()) { - if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); - } + double currentPos = this.getEncoderPosition(); + double forwardDistance = Math.abs(currentPos - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT); + double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); - if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { - this.speedLimiter = 1.0; + if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) { + this.speedLimiter = 1.0; + } } } + public boolean areSoftLimitsEnabled() { + return this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kForward) && this.m_boomBoomRotateMotor.isSoftLimitEnabled(SoftLimitDirection.kReverse); + } + /** * Set status of turret motor soft limits. * @param set Boolean to set soft limits to. @@ -192,7 +201,7 @@ public class Turret extends SubsystemBase { * @param input from -1.0 to 1.0, positive is clockwise */ public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed); } public void runShooterRotatePID(double targetAngle) { From 5cdef72c879826bbb17052b462a79970533245bf Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 21:31:34 -0600 Subject: [PATCH 152/180] sought --- .../robot/commands/ShooterCommands/Seek.java | 29 ++++++++++++++ .../robot/commands/ShooterCommands/Shoot.java | 40 +++++++++++-------- 2 files changed, 53 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java new file mode 100644 index 0000000..a706512 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -0,0 +1,29 @@ +// 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.ShooterCommands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class Seek extends SequentialCommandGroup { + + /** Seeks. + * Seeking -> Sought + * @author Aarav Shah + * @blame Aarav Shah (thomas did this) + */ + public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new Shoot(swerve, drum, turret, hood), new TrackTarget(turret, drum, hood, visionOdometry)); + } +} diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index f12e518..d172256 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -25,6 +25,8 @@ public class Shoot extends CommandBase { private Turret turret; private Hood hood; + private boolean toShoot; + // given private double gyroAngle; private double odoX; @@ -67,13 +69,15 @@ public class Shoot extends CommandBase { * * @author Aarav Shah */ - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, boolean toShoot) { // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; this.drum = drum; this.turret = turret; this.hood = hood; - + + this.toShoot = toShoot; + addRequirements(this.swerve, this.drum, this.turret, this.hood); kP = gains.kP; @@ -85,7 +89,7 @@ public class Shoot extends CommandBase { derivative = 0; time = 0.02; - tolerance = 5.0; + tolerance = 10.0; isAimedInTolerance = false; if (simMode) { @@ -96,6 +100,10 @@ public class Shoot extends CommandBase { } } + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { + this(swerve, drum, turret, hood, false); + } + /** * Updates error for custom PID. */ @@ -131,21 +139,19 @@ public class Shoot extends CommandBase { targetHood = drum.getHood(distance); targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - // m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; // deadzone processing if (AimToCenter.isDeadzone(targetAngle)) {} // initial error updateError(); - // System.out.println("Error: " + error); prevError = error; } /** * Run custom PID. */ public void runPID() { - if (error > 180){ + if (error > 180) { error = 360 - error; inverted = -1; } @@ -156,10 +162,10 @@ public class Shoot extends CommandBase { updateError(); proportional = error; - integral = integral + error * time; + integral = integral + (error * time); derivative = (error - prevError) / time; output = kP * proportional + kI * integral + kD * derivative; - normOutput = output/360 * inverted; + normOutput = (output / 360) * inverted; } // Called every time the scheduler runs while the command is scheduled. @@ -178,28 +184,30 @@ public class Shoot extends CommandBase { } runPID(); + SmartDashboard.putNumber("Error", this.error); SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); SmartDashboard.putNumber("Normalized Output", normOutput); - swerve.driveWithInput(0, 0, normOutput, true); // i have no idea if this is how you rotate the - // entire swerve drive or its the line below - // m_swerve.driveWithInput(0, 0, Math.cos(output), Math.sin(output), true); - - // m_hood.runAngleAdjustPID(m_targetHood); - // m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + + swerve.driveWithInput(0, 0, normOutput, true); + turret.m_boomBoomRotateMotor.set(normOutput); + + if (this.toShoot) { + this.hood.runAngleAdjustPID(this.targetHood); + this.drum.runDrumShooterVelocityPID(this.targetVel); + } if (simMode) { turretDummy.apply(normOutput); System.out.println("Turret Dummy: " + turretDummy.get()); } - turret.m_boomBoomRotateMotor.set(normOutput); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { // return to initial swerve rotation - swerve.driveWithInput(0, 0, initialSwerveRotation, true); + // swerve.driveWithInput(0, 0, initialSwerveRotation, true); } // Returns true when the command should end. From c350dd4a9d20134cf056ed28bdebdf5adebed68e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 21:39:56 -0600 Subject: [PATCH 153/180] hard limit change --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 1 + src/test/java/frc4388/commands/AimToCenterTest.java | 6 +++++- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e5351ea..8f945b7 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -289,7 +289,7 @@ public final class Constants { public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7); public static final double TURRET_FORWARD_HARD_LIMIT = 18.429; - public static final double TURRET_REVERSE_HARD_LIMIT = -106.454; + public static final double TURRET_REVERSE_HARD_LIMIT = -117.8; public static final double TURRET_FORWARD_SOFT_LIMIT = TURRET_FORWARD_HARD_LIMIT - 5; public static final double TURRET_REVERSE_SOFT_LIMIT = TURRET_REVERSE_HARD_LIMIT + 2; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1a2ed8c..004b378 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -192,6 +192,7 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); + m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.35), m_robotBoomBoom)); // m_robotStorage.setDefaultCommand( // new ManageStorage(m_robotStorage, diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 0bc247d..0b68a94 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -10,7 +10,11 @@ public class AimToCenterTest { private static final double DELTA = 1e-15; - //@Test + /** + * Unit tests the isDeadzone function in AimToCenter.java + * @author Ryan Manley + * @link www.hoohle.com + */ public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { boolean output; From 23aee3c0a68315e7c25fed9afbd0b97338d95bc4 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 22:05:43 -0600 Subject: [PATCH 154/180] method for vel ramping --- .../java/frc4388/robot/commands/ShooterCommands/Shoot.java | 1 + src/main/java/frc4388/robot/subsystems/Hood.java | 3 +++ src/main/java/frc4388/robot/subsystems/Turret.java | 4 +++- 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index d172256..2d84ef5 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -214,6 +214,7 @@ public class Shoot extends CommandBase { @Override public boolean isFinished() { // if (simMode) { + SmartDashboard.putBoolean("isAimedInTolerance", isAimedInTolerance); return isAimedInTolerance; // } // return false; diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 6176c9a..29b09d2 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -64,7 +64,10 @@ public class Hood extends SubsystemBase { SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + runVelocityRamping(); + } + public void runVelocityRamping() { if (areSoftLimitsEnabled()) { double currentPos = this.getEncoderPosition(); double forwardDistance = Math.abs(currentPos - ShooterConstants.HOOD_FORWARD_SOFT_LIMIT); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index d8c59d4..1ba15e6 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -147,9 +147,11 @@ public class Turret extends SubsystemBase { leftPrevState = leftState; // * Update the state of the left limit switch. - // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). + runVelocityRamping(); + } + public void runVelocityRamping() { if (areSoftLimitsEnabled()) { double currentPos = this.getEncoderPosition(); From 474ce044a8fedd53c6fad7a2eda094e9d013c5a4 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 22:05:56 -0600 Subject: [PATCH 155/180] java docs --- .../java/frc4388/robot/subsystems/Climber.java | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 15d5a90..f0ababd 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -214,6 +214,12 @@ public class Climber extends SubsystemBase { boolean movingPrev = false; boolean moving; + /** + * + * @param xInput Rate of change of X position of target point + * @param yInput Rate of change of Y position of target point + * @deprecated use controlJointsWithInput() instead + */ public void controlWithInput(double xInput, double yInput) { moving = xInput != 0 || yInput != 0; @@ -301,7 +307,10 @@ public class Climber extends SubsystemBase { * * @param targetPoint Target xy point for the climber arm to go to * @param tiltAngle The tilt of the robot - * @return [shoulderAngle, elbowAngle] in radians */ + * @return [shoulderAngle, elbowAngle] in radians + * @deprecated + * */ + public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) { double [] angles = new double[2]; @@ -379,7 +388,12 @@ public class Climber extends SubsystemBase { angles[1] = elbowAngle; return angles; } - + /** + * Forward kinematics for climber + * @param shoulderAngle in radians + * @param elbowAngle in radians + * @return Point in 2d of end effector + */ public static Point getClimberPosition(double shoulderAngle, double elbowAngle) { Point climberPoint = new Point(0, 0); From 6f015a2f835ad805934c2c5fbfd0b5255255226e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 22:16:20 -0600 Subject: [PATCH 156/180] sum shoot change --- src/main/java/frc4388/robot/Constants.java | 1 + .../robot/commands/ShooterCommands/Shoot.java | 99 ++++++------------- 2 files changed, 29 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8f945b7..64e38b1 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -34,6 +34,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final double TICKS_PER_ROTATION_FX = 2048; + public static final double LOOP_TIME = 0.02; public static final class SwerveDriveConstants { public static final double ROTATION_SPEED = 3.0; diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 2d84ef5..7e560be 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.BoomBoom; @@ -21,44 +22,33 @@ public class Shoot extends CommandBase { // subsystems private SwerveDrive swerve; - private BoomBoom drum; private Turret turret; + private BoomBoom drum; private Hood hood; private boolean toShoot; // given - private double gyroAngle; - private double odoX; - private double odoY; + private double odoX, odoY; private double distance; + private double gyroAngle; // targets - private double targetVel; - private double targetHood; - private double targetAngle; - private Pose2d targetPoint; + private double targetAngle, targetVel, targetHood; // pid + private Gains gains = ShooterConstants.SHOOT_GAINS; private double error; private double prevError; - private Gains gains = ShooterConstants.SHOOT_GAINS; private double kP, kI, kD; private double proportional, integral, derivative; - private double time; - private double output; - private double normOutput; + private double output, normOutput; private double tolerance; private boolean isAimedInTolerance; private int inverted; private double initialSwerveRotation; - // testing - private boolean simMode = false; - private DummySensor driveDummy; - private DummySensor turretDummy; - /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball * @@ -87,17 +77,9 @@ public class Shoot extends CommandBase { proportional = 0; integral = 0; derivative = 0; - time = 0.02; tolerance = 10.0; isAimedInTolerance = false; - - if (simMode) { - driveDummy = new DummySensor(180); - turretDummy = new DummySensor(180); - - DummySensor.resetAll(); - } } public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { @@ -108,37 +90,30 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - targetPoint = SwerveDriveConstants.HUB_POSE; - // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get()); targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - // error = (m_targetAngle - turretDummy.get() + 360) % 360; error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360; isAimedInTolerance = (Math.abs(error) <= tolerance); - - if (simMode) { - SmartDashboard.putBoolean("isAimed?", isAimedInTolerance); - System.out.println("Target Angle: " + targetAngle); - System.out.println("Error: " + error); - } } // Called when the command is initially scheduled. @Override public void initialize() { - turret.gotoMidpoint(); + this.turret.gotoMidpoint(); - odoX = 0;//-m_swerve.getOdometry().getY(); - odoY = -8;//-m_swerve.getOdometry().getX(); + this.odoX = 0;//-m_swerve.getOdometry().getY(); + this.odoY = -8;//-m_swerve.getOdometry().getX(); - gyroAngle = swerve.getRegGyro().getDegrees(); - initialSwerveRotation = gyroAngle; + this.distance = Math.hypot(odoX, odoY); + + this.gyroAngle = this.swerve.getRegGyro().getDegrees(); + this.initialSwerveRotation = gyroAngle; // get targets (shooter tables) - targetVel = drum.getVelocity(distance); - targetHood = drum.getHood(distance); + this.targetVel = drum.getVelocity(distance); + this.targetHood = drum.getHood(distance); - targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); // deadzone processing if (AimToCenter.isDeadzone(targetAngle)) {} @@ -147,60 +122,45 @@ public class Shoot extends CommandBase { updateError(); prevError = error; } + /** * Run custom PID. */ public void runPID() { if (error > 180) { error = 360 - error; - inverted = -1; + this.inverted = -1; } else{ - inverted = 1; + this.inverted = 1; } prevError = error; updateError(); - proportional = error; - integral = integral + (error * time); - derivative = (error - prevError) / time; - output = kP * proportional + kI * integral + kD * derivative; - normOutput = (output / 360) * inverted; + this.proportional = error; + this.integral = integral + (error * Constants.LOOP_TIME); + this.derivative = (error - prevError) / Constants.LOOP_TIME; + this.output = kP * proportional + kI * integral + kD * derivative; + this.normOutput = (output / 360) * inverted; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - if (simMode) { - System.out.println("Normalized Output: " + normOutput); - } - // custom pid - - if (simMode) { - driveDummy.apply(normOutput); - System.out.println("Drive Dummy: " + driveDummy.get()); - } - runPID(); SmartDashboard.putNumber("Error", this.error); SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); - SmartDashboard.putNumber("Normalized Output", normOutput); + SmartDashboard.putNumber("Normalized Output", this.normOutput); - swerve.driveWithInput(0, 0, normOutput, true); - turret.m_boomBoomRotateMotor.set(normOutput); + this.swerve.driveWithInput(0, 0, normOutput, true); + this.turret.m_boomBoomRotateMotor.set(normOutput); if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); this.drum.runDrumShooterVelocityPID(this.targetVel); } - - if (simMode) { - turretDummy.apply(normOutput); - System.out.println("Turret Dummy: " + turretDummy.get()); - } } // Called once the command ends or is interrupted. @@ -213,10 +173,7 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - // if (simMode) { SmartDashboard.putBoolean("isAimedInTolerance", isAimedInTolerance); return isAimedInTolerance; - // } - // return false; } } From a2434a8fde3c70c30f490978a7ee34b7234a69ad Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Mar 2022 22:24:45 -0600 Subject: [PATCH 157/180] very smol change --- .../robot/commands/ShooterCommands/Shoot.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 7e560be..f14f449 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -114,9 +114,6 @@ public class Shoot extends CommandBase { this.targetHood = drum.getHood(distance); this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - - // deadzone processing - if (AimToCenter.isDeadzone(targetAngle)) {} // initial error updateError(); @@ -166,8 +163,17 @@ public class Shoot extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - // return to initial swerve rotation + + // ? return to initial swerve rotation // swerve.driveWithInput(0, 0, initialSwerveRotation, true); + + this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); + this.turret.m_boomBoomRotateMotor.set(0.0); + + if (this.toShoot) { + this.hood.runHood(0.0); + this.drum.runDrumShooter(0.0); + } } // Returns true when the command should end. From a467bd65d8deb1ecaea8d816ed5383bf4ad97b72 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 00:16:49 -0600 Subject: [PATCH 158/180] asdfg; --- .../java/frc4388/robot/RobotContainer.java | 47 +++++++++---------- src/main/java/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/subsystems/Climber.java | 13 +++-- 3 files changed, 33 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fbae619..a0a97f7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,7 +112,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - // public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); public 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 Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -128,7 +128,7 @@ public class RobotContainer { private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); - //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); + // public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -192,7 +192,7 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.35), m_robotBoomBoom)); + m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); // m_robotStorage.setDefaultCommand( // new ManageStorage(m_robotStorage, @@ -209,17 +209,17 @@ public class RobotContainer { new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual - m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { - // if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - // } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - // }, m_robotTurret)); + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + + m_robotTurret.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { @@ -227,15 +227,12 @@ public class RobotContainer { if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - // getOperatorController().getRightY() * 10000); }} - // , m_robotClimber)); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(0.0, getOperatorController().getRightY() * 0.5) + , m_robotClimber)); - // m_robotTurret.setDefaultCommand( - // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); + // m_robotTurret.setDefaultCommand( + // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); // 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")); @@ -347,7 +344,7 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - // .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) + .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) @@ -356,13 +353,13 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - // .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); - // .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3a92ee8..1db64ed 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -196,8 +196,8 @@ 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_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO + 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); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index f0ababd..666f9be 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -164,8 +164,8 @@ public class Climber extends SubsystemBase { // } public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER );//* this.shoulderSpeedLimiter); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER);// * this.elbowSpeedLimiter); } public double[] getJointAngles() { @@ -206,7 +206,11 @@ public class Climber extends SubsystemBase { public void setJointSpeeds(double[] speeds) { setJointSpeeds(speeds[0], speeds[1]); } - + /** + * Velocity PID set for joints + * @param shoulderSpeed + * @param elbowSpeed + */ public void setJointSpeeds(double shoulderSpeed, double elbowSpeed) { m_shoulder.set(TalonFXControlMode.Velocity, shoulderSpeed); m_elbow.set(TalonFXControlMode.Velocity, elbowSpeed); @@ -220,6 +224,7 @@ public class Climber extends SubsystemBase { * @param yInput Rate of change of Y position of target point * @deprecated use controlJointsWithInput() instead */ + @Deprecated public void controlWithInput(double xInput, double yInput) { moving = xInput != 0 || yInput != 0; @@ -310,7 +315,7 @@ public class Climber extends SubsystemBase { * @return [shoulderAngle, elbowAngle] in radians * @deprecated * */ - + @Deprecated public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) { double [] angles = new double[2]; From d6d849389a93d9a0df76a546b98eeb72591734ec Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 00:50:01 -0600 Subject: [PATCH 159/180] stop drive + turret during climb --- .../java/frc4388/robot/RobotContainer.java | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fbae619..5853bfb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -178,13 +178,21 @@ public class RobotContainer { // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput( - getDriverController().getLeftX(), - getDriverController().getLeftY(), - getDriverController().getRightX(), - getDriverController().getRightY(), - true), - m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), + getDriverController().getLeftY(), + getDriverController().getRightX(), + getDriverController().getRightY(), + true); } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { + m_robotSwerveDrive.driveWithInput( 0, + 0, + 0, + 0, + true); + }} + , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers m_robotIntake.setDefaultCommand( @@ -209,17 +217,17 @@ public class RobotContainer { new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); // Turret Manual - m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { - // if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - // } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } - // }, m_robotTurret)); + // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), + // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + + m_robotTurret.setDefaultCommand( + new RunCommand(() -> { + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + } + if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { From 94dd0797cf07f8067370b115635c01ada84e6972 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 00:54:33 -0600 Subject: [PATCH 160/180] fix turret --- src/main/java/frc4388/robot/RobotContainer.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f98c824..af39f4a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -223,9 +223,7 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { - if (this.currentTurretMode.equals(TurretMode.MANUAL)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - } + if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); From e34a89a46f4497d0cb043b0ea52836e6ab3c2ff3 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 01:42:34 -0600 Subject: [PATCH 161/180] shoot is janky --- src/main/java/frc4388/robot/Constants.java | 4 ++-- .../java/frc4388/robot/RobotContainer.java | 10 ++++---- .../robot/commands/ShooterCommands/Seek.java | 2 +- .../robot/commands/ShooterCommands/Shoot.java | 24 ++++++++----------- .../frc4388/robot/subsystems/SwerveDrive.java | 23 ++++++++++-------- 5 files changed, 31 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 64e38b1..3f0ae83 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -37,7 +37,7 @@ public final class Constants { public static final double LOOP_TIME = 0.02; public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 3.0; + public static final double ROTATION_SPEED = 2.0; public static final double WIDTH = 23.75; public static final double HEIGHT = 23.75; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; @@ -278,7 +278,7 @@ public final class Constants { public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a0a97f7..5743e3c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -265,7 +265,7 @@ public class RobotContainer { .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 1.0, 0.0, true)) + .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); new JoystickButton(getDriverController(), XboxController.Button.kB.value) @@ -273,7 +273,7 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); new JoystickButton(getDriverController(), XboxController.Button.kY.value) - .whileHeld(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)) + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) .whenReleased(() -> m_robotSwerveDrive.stopModules()); @@ -306,9 +306,9 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kX.value) .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) - .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) + // .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); // Right Bumper > Storage In // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index a706512..efcbaa3 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood), new TrackTarget(turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, false), new TrackTarget(turret, drum, hood, visionOdometry)); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index f14f449..64a79dc 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -82,16 +82,19 @@ public class Shoot extends CommandBase { isAimedInTolerance = false; } - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { - this(swerve, drum, turret, hood, false); - } + // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { + // this(swerve, drum, turret, hood, false); + // } /** * Updates error for custom PID. */ public void updateError() { targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - error = (targetAngle - turret.getBoomBoomAngleDegrees() + 360) % 360; + error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; + if (error > 180) { + error = 360 - error; + this.inverted = -1; } else { this.inverted = 1; } isAimedInTolerance = (Math.abs(error) <= tolerance); } @@ -99,7 +102,7 @@ public class Shoot extends CommandBase { @Override public void initialize() { - this.turret.gotoMidpoint(); + // this.turret.gotoMidpoint(); this.odoX = 0;//-m_swerve.getOdometry().getY(); this.odoY = -8;//-m_swerve.getOdometry().getX(); @@ -124,13 +127,6 @@ public class Shoot extends CommandBase { * Run custom PID. */ public void runPID() { - if (error > 180) { - error = 360 - error; - this.inverted = -1; - } - else{ - this.inverted = 1; - } prevError = error; updateError(); @@ -151,8 +147,8 @@ public class Shoot extends CommandBase { SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); SmartDashboard.putNumber("Normalized Output", this.normOutput); - this.swerve.driveWithInput(0, 0, normOutput, true); this.turret.m_boomBoomRotateMotor.set(normOutput); + this.swerve.driveWithInput(0, 0, normOutput, true); if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); @@ -167,7 +163,7 @@ public class Shoot extends CommandBase { // ? return to initial swerve rotation // swerve.driveWithInput(0, 0, initialSwerveRotation, true); - this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); + // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); this.turret.m_boomBoomRotateMotor.set(0.0); if (this.toShoot) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2c5600b..35cd854 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -105,13 +105,13 @@ public class SwerveDrive extends SubsystemBase { double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, - -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) + -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI /2))) : new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond, -rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } - + // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; Translation2d speed = new Translation2d(leftX, leftY); @@ -119,6 +119,9 @@ public class SwerveDrive extends SubsystemBase { if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); + if (ignoreAngles) { + rot = 0; + } double xSpeedMetersPerSecond = speed.getX(); double ySpeedMetersPerSecond = speed.getY(); chassisSpeeds = fieldRelative @@ -127,14 +130,14 @@ public class SwerveDrive extends SubsystemBase { : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * speedAdjust); SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( chassisSpeeds); - if (ignoreAngles) { - SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; - for (int i = 0; i < states.length; i ++) { - SwerveModuleState state = states[i]; - lockedStates[i]= new SwerveModuleState(0, state.angle); - } - setModuleStates(lockedStates); - } + // if (ignoreAngles) { + // SwerveModuleState[] lockedStates = new SwerveModuleState[states.length]; + // for (int i = 0; i < states.length; i ++) { + // SwerveModuleState state = states[i]; + // lockedStates[i]= new SwerveModuleState(0, state.angle); + // } + // setModuleStates(lockedStates); + // } setModuleStates(states); // SmartDashboard.putNumber("rot", rot); // SmartDashboard.putNumber("rotarget", rotTarget.getDegrees()); From 0eca600135eeb656f6ec7e5a056f5f7a9d9bcb3e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 12:09:41 -0600 Subject: [PATCH 162/180] Testy stuffs fot the Shoot --- .../java/frc4388/robot/RobotContainer.java | 18 +++++++++--------- .../robot/commands/ShooterCommands/Shoot.java | 8 ++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 2 ++ 3 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97dda40..2566688 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,13 +179,13 @@ public class RobotContainer { // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), getDriverController().getRightX(), getDriverController().getRightY(), true); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotSwerveDrive.driveWithInput( 0, 0, 0, @@ -223,14 +223,14 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); m_robotClimber.setDefaultCommand( @@ -279,9 +279,9 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); new JoystickButton(getDriverController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)) - .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - .whenReleased(() -> m_robotSwerveDrive.stopModules()); + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); + //!.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + //!.whenReleased(() -> m_robotSwerveDrive.stopModules()); new JoystickButton(getDriverController(), XboxController.Button.kX.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 64a79dc..7424701 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -148,7 +148,7 @@ public class Shoot extends CommandBase { SmartDashboard.putNumber("Normalized Output", this.normOutput); this.turret.m_boomBoomRotateMotor.set(normOutput); - this.swerve.driveWithInput(0, 0, normOutput, true); + this.swerve.driveWithInput(0, 0, normOutput, false); // ? should the output be field relative if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); @@ -164,7 +164,11 @@ public class Shoot extends CommandBase { // swerve.driveWithInput(0, 0, initialSwerveRotation, true); // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); - this.turret.m_boomBoomRotateMotor.set(0.0); + // this.turret.m_boomBoomRotateMotor.set(0.0); + + // ? should stop the turret and the swerve + ////this.swerve.stopModules(); + ////this.turret.runTurretWithInput(0); if (this.toShoot) { this.hood.runHood(0.0); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 35cd854..72a7194 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -97,6 +97,7 @@ public class SwerveDrive extends SubsystemBase { ignoreAngles = true; else ignoreAngles = false; + Translation2d speed = new Translation2d(speedX, speedY); double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); @@ -111,6 +112,7 @@ public class SwerveDrive extends SubsystemBase { SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } + // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; From e39c81d5564d6cba01c2bde126ffee7245b9a442 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 14:25:38 -0600 Subject: [PATCH 163/180] shoot working no optimization --- src/main/java/frc4388/robot/Constants.java | 3 +- .../java/frc4388/robot/RobotContainer.java | 45 ++--- .../commands/ShooterCommands/AimToCenter.java | 2 +- .../robot/commands/ShooterCommands/Shoot.java | 154 ++++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 8 +- .../java/frc4388/robot/subsystems/Turret.java | 4 + 6 files changed, 120 insertions(+), 96 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3f0ae83..39814d8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -278,7 +278,8 @@ public final class Constants { public static final double TURRET_CLIMBING_POS = -3.76; // Shoot Command Constants - public static final Gains SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains TURRET_SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_SHOOT_GAINS = new Gains(4.0, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2566688..8c8add1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,19 +179,19 @@ public class RobotContainer { // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { + // if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), getDriverController().getRightX(), getDriverController().getRightY(), - true); } - if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { - m_robotSwerveDrive.driveWithInput( 0, - 0, - 0, - 0, - true); - }} + true); //} + // if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { + // m_robotSwerveDrive.driveWithInput( 0, + // 0, + // 0, + // 0, + // true); + }//} , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); // Intake with Triggers @@ -278,24 +278,29 @@ public class RobotContainer { .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); - new JoystickButton(getDriverController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); - //!.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - //!.whenReleased(() -> m_robotSwerveDrive.stopModules()); - - new JoystickButton(getDriverController(), XboxController.Button.kX.value) + // new JoystickButton(getDriverController(), XboxController.Button.kY.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); + // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + // .whenReleased(() -> m_robotSwerveDrive.stopModules()); + + new JoystickButton(getDriverController(), XboxController.Button.kX.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - /* Operator Buttons */ - + /* Operator Buttons */ + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); + //!.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + //!.whenReleased(() -> m_robotSwerveDrive.stopModules()); - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + + // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index de4b234..806446d 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -85,7 +85,7 @@ public class AimToCenter extends CommandBase { if (-x > 0) { return (180 + exp); } if (-x < 0) { return (360 + exp); } - if (x == 0 && y > 0) { return (270 - actualGyro); } + if (x == 0 && y > 0) { return (270 - actualGyro); } // TODO: try putting these two lines before exp is calculated if (x == 0 && y < 0) { return (90 - actualGyro); } System.err.println("Invalid case."); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 7424701..ffc761a 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -37,13 +37,18 @@ public class Shoot extends CommandBase { private double targetAngle, targetVel, targetHood; // pid - private Gains gains = ShooterConstants.SHOOT_GAINS; + private Gains turretGains = ShooterConstants.TURRET_SHOOT_GAINS; + private Gains swerveGains = ShooterConstants.SWERVE_SHOOT_GAINS; private double error; private double prevError; private double kP, kI, kD; private double proportional, integral, derivative; private double output, normOutput; private double tolerance; + + boolean timerStarted; + long startTime; + private double timeTolerance; private boolean isAimedInTolerance; private int inverted; @@ -67,88 +72,95 @@ public class Shoot extends CommandBase { this.hood = hood; this.toShoot = toShoot; - - addRequirements(this.swerve, this.drum, this.turret, this.hood); - - kP = gains.kP; - kI = gains.kI; - kD = gains.kD; - + + kP = turretGains.kP; + kI = turretGains.kI; + kD = turretGains.kD; + proportional = 0; integral = 0; derivative = 0; - + tolerance = 10.0; + timeTolerance = 500; isAimedInTolerance = false; - } + this.inverted = 1; + + addRequirements(this.swerve, this.drum, this.turret, this.hood); + } + // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { - // this(swerve, drum, turret, hood, false); - // } - - /** - * Updates error for custom PID. - */ - public void updateError() { - targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; - if (error > 180) { - error = 360 - error; - this.inverted = -1; } else { this.inverted = 1; } - isAimedInTolerance = (Math.abs(error) <= tolerance); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - // this.turret.gotoMidpoint(); - - this.odoX = 0;//-m_swerve.getOdometry().getY(); - this.odoY = -8;//-m_swerve.getOdometry().getX(); - - this.distance = Math.hypot(odoX, odoY); - - this.gyroAngle = this.swerve.getRegGyro().getDegrees(); - this.initialSwerveRotation = gyroAngle; - - // get targets (shooter tables) - this.targetVel = drum.getVelocity(distance); - this.targetHood = drum.getHood(distance); - - this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + // this(swerve, drum, turret, hood, false); + // } - // initial error - updateError(); - prevError = error; - } - - /** - * Run custom PID. - */ - public void runPID() { - prevError = error; - updateError(); + /** + * Updates error for custom PID. + */ + public void updateError() { + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + + error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; + // if (error > 180) { + // error = 360 - error; // TODO: error - 360 + // this.inverted = -1; } else { this.inverted = 1; } + isAimedInTolerance = (Math.abs(error) <= tolerance); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timerStarted = false; + startTime = 0; + + // this.turret.gotoMidpoint(); + + this.odoX = 0;//-m_swerve.getOdometry().getY(); + this.odoY = -8;//-m_swerve.getOdometry().getX(); + + this.distance = Math.hypot(odoX, odoY); + + this.gyroAngle = this.swerve.getRegGyro().getDegrees(); + this.initialSwerveRotation = gyroAngle; + + // get targets (shooter tables) + this.targetVel = drum.getVelocity(distance); + this.targetHood = drum.getHood(distance); + + this.targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + + // initial error + updateError(); + prevError = error; + } + + /** + * Run custom PID. + */ + public void runPID() { + prevError = error; + updateError(); - this.proportional = error; - this.integral = integral + (error * Constants.LOOP_TIME); - this.derivative = (error - prevError) / Constants.LOOP_TIME; - this.output = kP * proportional + kI * integral + kD * derivative; - this.normOutput = (output / 360) * inverted; - } - + this.proportional = error; + this.integral = integral + (error * Constants.LOOP_TIME); + this.derivative = (error - prevError) / Constants.LOOP_TIME; + this.output = kP * proportional + kI * integral + kD * derivative; + this.normOutput = (output / 360) * inverted; + } + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - + runPID(); - + SmartDashboard.putNumber("Error", this.error); SmartDashboard.putNumber("Shoot.java TargetAngle", this.targetAngle); SmartDashboard.putNumber("Normalized Output", this.normOutput); - this.turret.m_boomBoomRotateMotor.set(normOutput); - this.swerve.driveWithInput(0, 0, normOutput, false); // ? should the output be field relative + this.turret.runTurretWithCustomPID(normOutput); + // this.turret.m_boomBoomRotateMotor.set(normOutput); + this.swerve.driveWithInput(0, 0, normOutput * (this.swerveGains.kP/this.turretGains.kP), false); // ? should the output be field relative if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); @@ -160,6 +172,7 @@ public class Shoot extends CommandBase { @Override public void end(boolean interrupted) { + System.err.println("SHOOT IS FINISHED: " + Boolean.toString(interrupted).toUpperCase()); // ? return to initial swerve rotation // swerve.driveWithInput(0, 0, initialSwerveRotation, true); @@ -179,7 +192,12 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - SmartDashboard.putBoolean("isAimedInTolerance", isAimedInTolerance); - return isAimedInTolerance; + + if (isAimedInTolerance && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 72a7194..ed9c193 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -82,10 +82,9 @@ public class SwerveDrive extends SubsystemBase { SmartDashboard.putData("Field", m_field); } - // https://github.com/ZachOrr/MK3-Swerve-Example /** * Method to drive the robot using joystick info. - * + * @link https://github.com/ZachOrr/MK3-Swerve-Example * @param speeds[0] Speed of the robot in the x direction (forward). * @param speeds[1] Speed of the robot in the y direction (sideways). * @param rot Angular rate of the robot. @@ -93,10 +92,7 @@ public class SwerveDrive extends SubsystemBase { * field. */ public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) { - if (speedX == 0 && speedY == 0 && rot == 0) - ignoreAngles = true; - else - ignoreAngles = false; + ignoreAngles = (speedX == 0) && (speedY == 0) && (rot == 0); Translation2d speed = new Translation2d(speedX, speedY); double mag = speed.getNorm(); diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1ba15e6..3ebfabc 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -206,6 +206,10 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER * this.speedLimiter * this.calibrationSpeed); } + public void runTurretWithCustomPID(double input) { + m_boomBoomRotateMotor.set(input * this.speedLimiter); + } + public void runShooterRotatePID(double targetAngle) { double softMid = (ShooterConstants.TURRET_FORWARD_SOFT_LIMIT - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) / 2; From 896bbc011cef371a987a2f473702d290beba7650 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 15:24:48 -0600 Subject: [PATCH 164/180] can now drive while Shoot.java was running --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 39 +++++++++---------- .../robot/commands/ShooterCommands/Shoot.java | 4 +- .../frc4388/robot/subsystems/BoomBoom.java | 3 +- 4 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 39814d8..e1dcc07 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -279,7 +279,7 @@ public final class Constants { // Shoot Command Constants public static final Gains TURRET_SHOOT_GAINS = new Gains(3.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains SWERVE_SHOOT_GAINS = new Gains(4.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_SHOOT_GAINS = new Gains(6.0, 0.0, 0.0, 0.0, 0, 1.0); /* Turret Constants */ // ID diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8c8add1..94da529 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -131,8 +131,8 @@ public class RobotContainer { // public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers - private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); + private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); // Autonomous @@ -270,31 +270,29 @@ public class RobotContainer { new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - new JoystickButton(getDriverController(), XboxController.Button.kA.value) - .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true)) - .whenReleased(() -> m_robotSwerveDrive.stopModules()); + // new JoystickButton(getDriverController(), XboxController.Button.kA.value) + // .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true)) + // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - new JoystickButton(getDriverController(), XboxController.Button.kB.value) - .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)) - .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); + // new JoystickButton(getDriverController(), XboxController.Button.kB.value) + // .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)) + // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); // new JoystickButton(getDriverController(), XboxController.Button.kY.value) // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - new JoystickButton(getDriverController(), XboxController.Button.kX.value) - .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); + // new JoystickButton(getDriverController(), XboxController.Button.kX.value) + // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + // .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) + // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); - //!.whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - //!.whenReleased(() -> m_robotSwerveDrive.stopModules()); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); @@ -340,9 +338,10 @@ public class RobotContainer { //B > Shoot with Lime new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0))); + // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + // .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) + .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); // .whileHeld% /* Button Box Buttons */ @@ -414,11 +413,11 @@ public class RobotContainer { return null; } - public XboxController getDriverController() { + public static XboxController getDriverController() { return m_driverXbox; } - public XboxController getOperatorController() { + public static XboxController getOperatorController() { return m_operatorXbox; } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index ffc761a..ecd88ab 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -9,6 +9,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants; +import frc4388.robot.Robot; +import frc4388.robot.RobotContainer; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.BoomBoom; @@ -160,7 +162,7 @@ public class Shoot extends CommandBase { this.turret.runTurretWithCustomPID(normOutput); // this.turret.m_boomBoomRotateMotor.set(normOutput); - this.swerve.driveWithInput(0, 0, normOutput * (this.swerveGains.kP/this.turretGains.kP), false); // ? should the output be field relative + this.swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index c81fd38..8e8c334 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -189,7 +189,8 @@ public class BoomBoom extends SubsystemBase { * @param speed percent output form -1.0 to 1.0 */ public void runDrumShooter(double speed) { - m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); + // m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2); + m_shooterFalconLeft.set(speed); SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2); SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent()); SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent()); From 21b8fa254b3b66bad5cd8fb2de661804954ad09a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 15:47:07 -0600 Subject: [PATCH 165/180] added limelight end mode for shoot --- .../java/frc4388/robot/RobotContainer.java | 2 +- .../robot/commands/ShooterCommands/Seek.java | 2 +- .../robot/commands/ShooterCommands/Shoot.java | 24 ++++++++++++------- .../robot/subsystems/VisionOdometry.java | 2 +- 4 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 94da529..910b44f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -292,7 +292,7 @@ public class RobotContainer { /* Operator Buttons */ new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index efcbaa3..310b77e 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, false), new TrackTarget(turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index ecd88ab..f3d1920 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -17,6 +17,7 @@ import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.VisionOdometry; import frc4388.utility.DummySensor; import frc4388.utility.Gains; @@ -27,6 +28,7 @@ public class Shoot extends CommandBase { private Turret turret; private BoomBoom drum; private Hood hood; + private VisionOdometry visionOdometry; private boolean toShoot; @@ -56,6 +58,8 @@ public class Shoot extends CommandBase { private int inverted; private double initialSwerveRotation; + private boolean endsWithLimelight; + /** * Creates a new shoot command, allowing the robot to aim and be ready to fire a ball * @@ -66,7 +70,7 @@ public class Shoot extends CommandBase { * * @author Aarav Shah */ - public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, boolean toShoot) { + public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry, boolean toShoot, boolean endsWithLimelight) { // Use addRequirements() here to declare subsystem dependencies. this.swerve = swerve; this.drum = drum; @@ -74,6 +78,7 @@ public class Shoot extends CommandBase { this.hood = hood; this.toShoot = toShoot; + this.endsWithLimelight = endsWithLimelight; kP = turretGains.kP; kI = turretGains.kI; @@ -89,7 +94,7 @@ public class Shoot extends CommandBase { this.inverted = 1; - addRequirements(this.swerve, this.drum, this.turret, this.hood); + addRequirements(this.swerve, this.drum, this.turret, this.hood, this.visionOdometry); } // public Shoot(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood) { @@ -194,12 +199,15 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - - if (isAimedInTolerance && !timerStarted) { - timerStarted = true; - startTime = System.currentTimeMillis(); + if (!endsWithLimelight) { + if (isAimedInTolerance && !timerStarted) { + timerStarted = true; + startTime = System.currentTimeMillis(); + } + SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); + } else { + return this.visionOdometry.m_camera.getLatestResult().hasTargets(); } - SmartDashboard.putBoolean("isDone", isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); - return (isAimedInTolerance && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance)); } } diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java index 2a0abd2..315623d 100644 --- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java +++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java @@ -31,7 +31,7 @@ import frc4388.utility.VisionObscuredException; */ public class VisionOdometry extends SubsystemBase { // roborio ip & port: 10.43.88.2:1735 - private PhotonCamera m_camera; + public PhotonCamera m_camera; private SwerveDrive m_drive; private Turret m_shooter; From 540f688502606af2054241b16676fa201deaf8a0 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 20 Mar 2022 15:59:54 -0600 Subject: [PATCH 166/180] added tt --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 94da529..8460768 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -283,8 +283,8 @@ public class RobotContainer { // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - // new JoystickButton(getDriverController(), XboxController.Button.kX.value) - // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) + new JoystickButton(getDriverController(), XboxController.Button.kX.value) + .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) // .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); From da8fac33778cab3f743dc799d2203339f60c9368 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 16:22:11 -0600 Subject: [PATCH 167/180] smol change --- .../java/frc4388/robot/RobotContainer.java | 25 +++++++++++-------- .../robot/commands/ShooterCommands/Shoot.java | 3 ++- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 612ccf0..de5830a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -74,6 +74,7 @@ import frc4388.robot.commands.ClimberCommands.RunClimberPath; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; +import frc4388.robot.commands.ShooterCommands.Seek; import frc4388.robot.commands.ShooterCommands.Shoot; import frc4388.robot.commands.ShooterCommands.TrackTarget; import frc4388.robot.commands.StorageCommands.ManageStorage; @@ -283,18 +284,22 @@ public class RobotContainer { // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - new JoystickButton(getDriverController(), XboxController.Button.kX.value) - .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); - // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - - /* Operator Buttons */ - + // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) + // .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) + // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); + + /* Operator Buttons */ + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + + // new JoystickButton(getDriverController(), XboxController.Button.kX.value) + // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index f3d1920..c4a448e 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -76,6 +76,7 @@ public class Shoot extends CommandBase { this.drum = drum; this.turret = turret; this.hood = hood; + this.visionOdometry = visionOdometry; this.toShoot = toShoot; this.endsWithLimelight = endsWithLimelight; @@ -188,7 +189,7 @@ public class Shoot extends CommandBase { // ? should stop the turret and the swerve ////this.swerve.stopModules(); - ////this.turret.runTurretWithInput(0); + this.turret.runTurretWithInput(0.0); if (this.toShoot) { this.hood.runHood(0.0); From 5d9d3ac1f1cd24a9b4e37477fce333b3439d34ab Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 20 Mar 2022 16:25:27 -0600 Subject: [PATCH 168/180] Heat seeking guided missiles --- .../commands/ShooterCommands/TrackTarget.java | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 426bcf2..f489de4 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -12,7 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; - +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; @@ -25,6 +25,7 @@ import frc4388.utility.desmos.DesmosServer; public class TrackTarget extends CommandBase { /** Creates a new TrackTarget. */ + SwerveDrive m_swerve; Turret m_turret; VisionOdometry m_visionOdometry; BoomBoom m_boomBoom; @@ -51,8 +52,9 @@ public class TrackTarget extends CommandBase { // public static Gains m_aimGains; - public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { + public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { // Use addRequirements() here to declare subsystem dependencies. + m_swerve = swerve; m_turret = turret; m_boomBoom = boomBoom; m_hood = hood; @@ -96,10 +98,16 @@ public class TrackTarget extends CommandBase { } output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; - output *= 4; - // output *= 0.5; - DesmosServer.putDouble("output", output); + output *= 2; + // //output *= 0.5; + // //DesmosServer.putDouble("output", output); m_turret.runTurretWithInput(output); + double position = m_turret.m_boomBoomRotateEncoder.getPosition(); + + if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || + Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) + m_swerve.driveWithInput(0, 0, output, false); + double y_rot = average.y / VisionConstants.LIME_VIXELS; y_rot *= Math.toRadians(VisionConstants.V_FOV); From 520d748afe2e3cbae7e0da913829b780d37ad24f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 16:48:06 -0600 Subject: [PATCH 169/180] fix --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++---- .../java/frc4388/robot/commands/ShooterCommands/Seek.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index de5830a..46ac339 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -293,8 +293,8 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); - // new JoystickButton(getDriverController(), XboxController.Button.kX.value) - // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); @@ -317,8 +317,8 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); //Toggles extender in and out - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) // .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java index 310b77e..73f82d0 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java @@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup { public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) { // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); - addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry)); + addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry)); } } From 7f1061ab9ab5b21edd30e1994f4145efb22c19de Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 17:28:10 -0600 Subject: [PATCH 170/180] odo chooser stuff --- src/main/java/frc4388/robot/Robot.java | 12 +++--------- .../robot/commands/ShooterCommands/Shoot.java | 6 ++---- src/main/java/frc4388/robot/subsystems/Claws.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Hood.java | 2 +- 4 files changed, 8 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2232b4c..da81db4 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -170,9 +170,7 @@ public class Robot extends TimedRobot { // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); // odo chooser stuff - addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), - new Pose2d(1, 2, new Rotation2d(Math.PI/3)), - new Pose2d(1, 3, new Rotation2d(Math.PI/4))); + addOdoChoices(new Pose2d(3.2766, -0.9398, new Rotation2d(0))); updateOdoChooser(); SmartDashboard.putData("Odometry Chooser", odoChooser); } @@ -185,7 +183,7 @@ public class Robot extends TimedRobot { public void addOdoChoices(Pose2d... points) { for (Pose2d point : points) { - String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + "°)"; + String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + ")"; odoChoices.put(key, point); } } @@ -253,17 +251,13 @@ public class Robot extends TimedRobot { public void teleopInit() { LOGGER.fine("teleopInit()"); - Robot.alliance = DriverStation.getAlliance(); - // m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); - selectedOdo = odoChooser.getSelected(); if (selectedOdo == null) { selectedOdo = m_robotContainer.getOdometry(); } - // m_robotContainer.resetOdometry(selectedOdo); - + m_robotContainer.resetOdometry(selectedOdo); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index c4a448e..60c69e7 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -121,10 +121,8 @@ public class Shoot extends CommandBase { timerStarted = false; startTime = 0; - // this.turret.gotoMidpoint(); - - this.odoX = 0;//-m_swerve.getOdometry().getY(); - this.odoY = -8;//-m_swerve.getOdometry().getX(); + this.odoX = 0.9398;//-this.swerve.getOdometry().getY(); // 3.2766 + this.odoY = -3.2766;//-this.swerve.getOdometry().getX(); // 0.9398 this.distance = Math.hypot(odoX, odoY); diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 2d8f726..2b811b0 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -153,8 +153,8 @@ public class Claws extends SubsystemBase { // @Override public void periodic() { - SmartDashboard.putNumber("Servo Left Pos", m_leftClaw.getRaw()); - SmartDashboard.putNumber("Servo Right Pos", m_rightClaw.getRaw()); + // 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); diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 29b09d2..7f81242 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -61,7 +61,7 @@ public class Hood extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); + // SmartDashboard.putNumber("Hood Angle", m_angleEncoder.getPosition()); // * speed limiting near soft limits. tolerance (distance when ramping starts) is 20 rotations. speed at hard limits is 0.2 (percent output). runVelocityRamping(); From 70e681c7d7572dd0eb312df533786b49b3cc3ada Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 18:09:16 -0600 Subject: [PATCH 171/180] climber subsystem 3.0 --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 11 +- src/main/java/frc4388/robot/RobotMap.java | 227 +++++----- .../robot/commands/ShooterCommands/Shoot.java | 4 +- .../frc4388/robot/subsystems/Climber.java | 427 +----------------- 5 files changed, 133 insertions(+), 538 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index da81db4..9b5f03c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -257,7 +257,7 @@ public class Robot extends TimedRobot { if (selectedOdo == null) { selectedOdo = m_robotContainer.getOdometry(); } - m_robotContainer.resetOdometry(selectedOdo); + // m_robotContainer.resetOdometry(selectedOdo); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 46ac339..57d4a4f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -113,7 +113,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public final Climber m_robotClimber = new Climber(m_robotMap.elbow); public 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 Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -235,7 +235,7 @@ public class RobotContainer { }, m_robotHood)); m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(0.0, getOperatorController().getRightY() * 0.5) + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getRightY() * 0.5) , m_robotClimber)); // m_robotTurret.setDefaultCommand( @@ -293,8 +293,11 @@ public class RobotContainer { new JoystickButton(getOperatorController(), XboxController.Button.kY.value) .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); @@ -359,7 +362,6 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) @@ -368,7 +370,6 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1db64ed..19a8264 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -49,6 +49,7 @@ public class RobotMap { configureExtenderMotors(); configureSerializerMotors(); configureStorageMotors(); + configureClimberMotors(); } /* LED Subsystem */ @@ -56,29 +57,30 @@ public class RobotMap { // void configureLEDMotorControllers() {} - /* 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); - public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); +/* 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); +public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); +public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); +public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); +public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); +public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); +public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); +public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); +public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); +public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - public SwerveModule leftFront; - public SwerveModule leftBack; - public SwerveModule rightFront; - public SwerveModule rightBack; +public final WPI_Pigeon2 gyro = new WPI_Pigeon2(SwerveDriveConstants.GYRO_ID); - void configureSwerveMotorControllers() { +public SwerveModule leftFront; +public SwerveModule leftBack; +public SwerveModule rightFront; +public SwerveModule rightBack; +void configureSwerveMotorControllers() { + leftFrontSteerMotor.configFactoryDefault(); leftFrontWheelMotor.configFactoryDefault(); rightFrontSteerMotor.configFactoryDefault(); @@ -87,58 +89,58 @@ public class RobotMap { leftBackWheelMotor.configFactoryDefault(); rightBackSteerMotor.configFactoryDefault(); rightBackWheelMotor.configFactoryDefault(); - + leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + + leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + NeutralMode mode = NeutralMode.Coast; leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); leftFrontWheelMotor.setNeutralMode(mode);// Coast @@ -148,7 +150,7 @@ public class RobotMap { leftBackWheelMotor.setNeutralMode(mode);// Coast rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); rightBackWheelMotor.setNeutralMode(mode);// Coast - + // current limits leftFrontSteerMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_STEER); @@ -160,7 +162,7 @@ public class RobotMap { rightFrontWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); leftBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); rightBackWheelMotor.configSupplyCurrentLimit(SwerveDriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG_WHEEL); - + leftFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); rightFrontSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); leftBackSteerMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_STEER); @@ -170,55 +172,56 @@ public class RobotMap { rightFrontWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); leftBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); rightBackWheelMotor.configStatorCurrentLimit(SwerveDriveConstants.STATOR_CURRENT_LIMIT_CONFIG_WHEEL); - + leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, - SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); + SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, - SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, - SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, - SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - + SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // config cancoder as remote encoder for swerve steer motors leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + 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 elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO +/* Climb Subsystem */ +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 Servo leftClaw = new Servo(1); // TODO: find actual channel - public final Servo rightClaw = new Servo(2); // TODO: find actual channel +private void configureClimberMotors() { + elbow.configFactoryDefault(); + elbow.setNeutralMode(NeutralMode.Brake); +} +/* Hooks Subsystem */ +public final Servo leftClaw = new Servo(1); +public final Servo rightClaw = new Servo(2); - // Shooter Config - /* Boom Boom Subsystem */ - public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); - public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); +// Shooter Config +/* Boom Boom Subsystem */ +public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); +public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // turret subsystem - public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); +// turret subsystem +public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); - // hood subsystem - public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - - // Create motor CANSparkMax - void configureShooterMotorControllers() { +// hood subsystem +public final CANSparkMax angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); +// Create motor CANSparkMax +void configureShooterMotorControllers() { + // LEFT FALCON shooterFalconLeft.configFactoryDefault(); shooterFalconLeft.setNeutralMode(NeutralMode.Coast); @@ -227,14 +230,14 @@ public class RobotMap { shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - + ShooterConstants.SHOOTER_TIMEOUT_MS); + // RIGHT FALCON shooterFalconRight.configFactoryDefault(); shooterFalconRight.setNeutralMode(NeutralMode.Coast); @@ -244,59 +247,59 @@ public class RobotMap { // m_shooterFalconRight.configPeakOutputForward(0, // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); + ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configStatorCurrentLimit(ShooterConstants.STATOR_CURRENT_LIMIT_CONFIG_SHOOTER, - ShooterConstants.SHOOTER_TIMEOUT_MS); - + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.follow(shooterFalconLeft); - + // turret shooterTurret.restoreFactoryDefaults(); shooterTurret.setIdleMode(IdleMode.kBrake); shooterTurret.setInverted(true); - + // hood subsystem angleAdjusterMotor.restoreFactoryDefaults(); angleAdjusterMotor.setIdleMode(IdleMode.kBrake); angleAdjusterMotor.setInverted(true); - } +} - /* Serializer Subsystem */ - public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); - public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); - - /* Intake Subsystem */ - public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); - public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); +/* Serializer Subsystem */ +public final CANSparkMax serializerBelt = new CANSparkMax(SerializerConstants.SERIALIZER_BELT, MotorType.kBrushless); +public final DigitalInput serializerBeam = new DigitalInput(SerializerConstants.SERIALIZER_BELT_BEAM); - void configureIntakeMotors() { +/* Intake Subsystem */ +public final WPI_TalonFX intakeMotor = new WPI_TalonFX(IntakeConstants.INTAKE_MOTOR); +public final CANSparkMax extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_MOTOR, MotorType.kBrushless); + +void configureIntakeMotors() { intakeMotor.configFactoryDefault(); intakeMotor.setInverted(false); intakeMotor.setNeutralMode(NeutralMode.Coast); - + intakeMotor.configSupplyCurrentLimit(IntakeConstants.SUPPLY_CURRENT_LIMIT_CONFIG_INTAKE); intakeMotor.configStatorCurrentLimit(IntakeConstants.STATOR_CURRENT_LIMIT_CONFIG_INTAKE); - } +} - void configureExtenderMotors() { - extenderMotor.restoreFactoryDefaults(); - extenderMotor.setInverted(true); - extenderMotor.setIdleMode(IdleMode.kBrake); +void configureExtenderMotors() { + extenderMotor.restoreFactoryDefaults(); + extenderMotor.setInverted(true); + extenderMotor.setIdleMode(IdleMode.kBrake); } - + void configureSerializerMotors() { serializerBelt.restoreFactoryDefaults(); - } + } /* Storage Subsystem */ public final CANSparkMax storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - + void configureStorageMotors() { storageMotor.restoreFactoryDefaults(); } - + } diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 60c69e7..d55f4e8 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -106,8 +106,8 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - + // targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + targetAngle = 0; error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; // if (error > 180) { // error = 360 - error; // TODO: error - 360 diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 666f9be..d16f830 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -4,429 +4,20 @@ package frc4388.robot.subsystems; -import java.util.Arrays; -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; 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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; - public class Climber extends SubsystemBase { - private static double[][] shoulderFeedMap; - private static double[][] elbowFeedMap; + private WPI_TalonFX elbow; - public static void configureFeedMaps() { - - } + /** Creates a new Climber */ + public Climber(WPI_TalonFX elbow) { this.elbow = elbow; } + + public void setEncoders(double value) { this.elbow.setSelectedSensorPosition(value); } + public double getCurrent() { return this.elbow.getSupplyCurrent(); } + public void setMotors(double elbowOutput) { this.elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); } - private WPI_TalonFX m_shoulder; - private WPI_TalonFX m_elbow; - private WPI_Pigeon2 m_gyro; - - private Point tPoint; - - private double[] tJointAngles; - private double[] tJointSpeeds; - - private boolean groundRelative; - private double shoulderSpeedLimiter; - private double elbowSpeedLimiter; - - /** Creates a new ClimberRewrite. */ - public Climber(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); - - // setClimberPositionGains(); - setClimberVelocityGains(); - useVelocityGains(); - - tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), 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_FORWARD_SOFT_LIMIT); - m_elbow.configForwardSoftLimitEnable(true); - // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); - // m_elbow.configReverseSoftLimitEnable(true); - - m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - m_shoulder.configForwardSoftLimitEnable(true); - m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - 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) - m_gyro = gyro; - - groundRelative = _groundRelative; - this.elbowSpeedLimiter = 1.0; - } - - // public void setClimberPositionGains() { - // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - // m_shoulder.config_kF(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_shoulder.config_kP(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_shoulder.config_kI(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_shoulder.config_kD(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - - // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - // m_elbow.config_kF(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_elbow.config_kP(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_elbow.config_kI(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - // m_elbow.config_kD(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_POSITION_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - // } - - // public void usePositionGains() { - // m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_POSITION_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - // m_elbow.selectProfileSlot(ClimberConstants.ELBOW_POSITION_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - // } - - public void setClimberVelocityGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_shoulder.config_kF(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kP(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kI(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_shoulder.config_kD(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - m_elbow.config_kF(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kF, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kP(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kP, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kI(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kI, ClimberConstants.CLIMBER_TIMEOUT_MS); - m_elbow.config_kD(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_VELOCITY_GAINS.kD, ClimberConstants.CLIMBER_TIMEOUT_MS); - } - - public void useVelocityGains() { - m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_VELOCITY_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX); - m_elbow.selectProfileSlot(ClimberConstants.ELBOW_VELOCITY_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX); - } - - // 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() { - // 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 * ClimberConstants.INPUT_MULTIPLIER );//* this.shoulderSpeedLimiter); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER);// * this.elbowSpeedLimiter); - } - - 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(Arrays.toString(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, shoulderAngle); - // m_elbow.set(TalonFXControlMode.Position, elbowAngle); - } - - public void setJointSpeeds(double[] speeds) { - setJointSpeeds(speeds[0], speeds[1]); - } - /** - * Velocity PID set for joints - * @param shoulderSpeed - * @param elbowSpeed - */ - public void setJointSpeeds(double shoulderSpeed, double elbowSpeed) { - m_shoulder.set(TalonFXControlMode.Velocity, shoulderSpeed); - m_elbow.set(TalonFXControlMode.Velocity, elbowSpeed); - } - - boolean movingPrev = false; - boolean moving; - /** - * - * @param xInput Rate of change of X position of target point - * @param yInput Rate of change of Y position of target point - * @deprecated use controlJointsWithInput() instead - */ - @Deprecated - public void controlWithInput(double xInput, double yInput) { - moving = xInput != 0 || yInput != 0; - - if(movingPrev != moving) { - if(moving) { - useVelocityGains(); - SmartDashboard.putString("Climber Gains", "Velocity"); - } else { - // usePositionGains(); - tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()}; - SmartDashboard.putString("Climber Gains", "Position"); - } - } - - if(moving) { - double[] jointSpeeds = new double[] {xInput * ClimberConstants.MOVE_SPEED, yInput * ClimberConstants.MOVE_SPEED}; - setJointSpeeds(jointSpeeds); - } else { - setJointAngles(tJointAngles); - } - - movingPrev = moving; - } - - public void controlJointsWithInput(double shoulderInput, double elbowInput) { - tJointAngles[0] += shoulderInput * ClimberConstants.MOVE_SPEED * .02; - tJointAngles[1] += elbowInput * ClimberConstants.MOVE_SPEED * .02; - } - - int pCount = 0; @Override - public void periodic() { - SmartDashboard.putBoolean("Moving", moving); - SmartDashboard.putBoolean("MovingPrev", movingPrev); - SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); - SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); - // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); - // if(pCount % 1 == 0) - // setJointAngles(tJointAngles); - - // pCount ++; - - // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). - double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); - double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); - double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); - - if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { - this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { - this.elbowSpeedLimiter = 1.0; - } - - // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). - double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); - double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); - double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); - - if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { - this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); - } - - if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { - this.shoulderSpeedLimiter = 1.0; - } - } - - /** - * 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 - * @return [shoulderAngle, elbowAngle] in radians - * @deprecated - * */ - @Deprecated - public static double[] getTargetJointAngles(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; - } - /** - * Forward kinematics for climber - * @param shoulderAngle in radians - * @param elbowAngle in radians - * @return Point in 2d of end effector - */ - 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 getClimberPosition(double[] jointAngles) { - return getClimberPosition(jointAngles[0], jointAngles[1]); - } - - public void setClimberSoftLimits(boolean set){ - m_elbow.configForwardSoftLimitEnable(set); - m_shoulder.configForwardSoftLimitEnable(set); - } - - public void setEncoders(double value){ - m_elbow.setSelectedSensorPosition(value); - m_shoulder.setSelectedSensorPosition(value); - } - - public double getCurrent(){ - return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); - } - -} + public void periodic() {} +} \ No newline at end of file From afc20154122aee2c238995fdb907a3465ef6ff8a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 19:31:42 -0600 Subject: [PATCH 172/180] build autos is back!!!!! with fancy new docs --- .../java/frc4388/robot/RobotContainer.java | 59 ++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 57d4a4f..71c17a8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -390,6 +390,53 @@ public class RobotContainer { .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } + /** + * Generate autonomous + * @param maxVel max velocity for the path (null to override default value of 5.0) + * @param maxAccel max acceleration for the path (null to override default value of 5.0) + * @param inputs strings (paths) or commands you want to run (in order) + * @return array of commands + */ + public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) { + maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY); + maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION); + + ArrayList commands = new ArrayList(); + commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); + + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + // parse input + for (int i=0; i m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation)))); + commands.add(new PPSwerveControllerCommand( + traj, + m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive.m_kinematics, + xController, + yController, + thetaController, + m_robotSwerveDrive::setModuleStates, + m_robotSwerveDrive)); + } + if (inputs[i] instanceof Command) { + commands.add((Command) inputs[i]); + } + } + + commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules())); + Command[] ret = new Command[commands.size()]; + ret = commands.toArray(ret); + return ret; + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * @@ -416,7 +463,17 @@ public class RobotContainer { // return new RunCommand(() -> { // }).withName("No Autonomous Path"); // } - return null; + + // ! this will run each of the specified PathPlanner paths in sequence. + // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")); + + // ! this will run each of the specified PathPlanner paths in sequence, while simultaneously running the intake throughout all the paths. + // * return new ParallelCommandGroup(buildAuto(null, + // * null, + // * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")), + // * new RunCommand(() -> m_robotIntake.runAtOutput(0.5)))); + + return null; } public static XboxController getDriverController() { From 446e671ac617200e217d9f70db42e3dafccd6c9e Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 20 Mar 2022 19:49:28 -0600 Subject: [PATCH 173/180] Better filtering --- .../commands/ShooterCommands/TrackTarget.java | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index f489de4..79f2a11 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -5,6 +5,9 @@ package frc4388.robot.commands.ShooterCommands; import java.util.ArrayList; +import java.util.HashMap; +import java.util.stream.Collector; +import java.util.stream.Collectors; import org.opencv.core.Point; @@ -80,17 +83,9 @@ public class TrackTarget extends CommandBase { m_visionOdometry.setLEDs(true); points = m_visionOdometry.getTargetPoints(); + points = filterPoints(points); + Point average = VisionOdometry.averagePoint(points); - - for(Point point : points) { - Vector2D difference = new Vector2D(point); - difference.subtract(new Vector2D(average)); - - if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD) - points.remove(point); - } - - average = VisionOdometry.averagePoint(points); DesmosServer.putPoint("average", average); for(int i = 0; i < points.size(); i++) { @@ -151,6 +146,26 @@ public class TrackTarget extends CommandBase { // m_turret.runshooterRotatePID(m_targetAngle); } + public ArrayList filterPoints(ArrayList points) { + Point average = VisionOdometry.averagePoint(points); + + HashMap pointDistances = new HashMap<>(); + double distanceSum = 0; + + for(Point point : points) { + Vector2D difference = new Vector2D(point); + difference.subtract(new Vector2D(average)); + + double mag = difference.magnitude(); + distanceSum += mag; + + pointDistances.put(point, mag); + } + + final double averageDist = distanceSum / points.size(); + return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); + } + public final double distanceRegression(double distance) { return (79.6078 * Math.pow(1.01343, distance) - 56.6671); } From e6aefcf91cf983a853c35e9fe72ddb809d6679b1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 19:52:42 -0600 Subject: [PATCH 174/180] smol path --- src/main/deploy/pathplanner/Move Forward.path | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path index ff9f904..653ed78 100644 --- a/src/main/deploy/pathplanner/Move Forward.path +++ b/src/main/deploy/pathplanner/Move Forward.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":4.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":{"x":3.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 71c17a8..a8a09a6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -402,7 +402,6 @@ public class RobotContainer { maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION); ArrayList commands = new ArrayList(); - commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); PIDController xController = SwerveDriveConstants.X_CONTROLLER; PIDController yController = SwerveDriveConstants.Y_CONTROLLER; @@ -473,7 +472,7 @@ public class RobotContainer { // * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")), // * new RunCommand(() -> m_robotIntake.runAtOutput(0.5)))); - return null; + return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Move Forward")); // test command } public static XboxController getDriverController() { From 00f6407a7f80f3a138b35ab1a10f083bfc7c8404 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 20:18:02 -0600 Subject: [PATCH 175/180] Climber fix and robotContainer cleanup --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 233 +++++++----------- .../commands/ShooterCommands/AimToCenter.java | 1 + .../java/frc4388/robot/subsystems/Claws.java | 129 +--------- 4 files changed, 107 insertions(+), 260 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e1dcc07..b46a442 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,7 +175,7 @@ public final class Constants { public static final int ELBOW_ID = 31; public static final int GYRO_ID = 14; - public static final double INPUT_MULTIPLIER = 0.7; + public static final double INPUT_MULTIPLIER = 0.4; public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0; public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0; @@ -269,7 +269,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.8; + public static final double TURRET_SPEED_MULTIPLIER = 0.4; public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5; public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 57d4a4f..cf3b893 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -127,10 +127,6 @@ public class RobotContainer { public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); - private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); - // public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); - // Controllers private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -171,79 +167,57 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); /* Default Commands */ - - // IK command - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - // getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); - - // Swerve Drive with Input + // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - // if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), getDriverController().getRightX(), getDriverController().getRightY(), - true); //} - // if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { - // m_robotSwerveDrive.driveWithInput( 0, - // 0, - // 0, - // 0, - // true); - }//} + true); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { + m_robotSwerveDrive.driveWithInput( 0, + 0, + 0, + 0, + true); + }} , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - // Intake with Triggers + // Intake with Triggers m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - - // m_robotStorage.setDefaultCommand( - // new ManageStorage(m_robotStorage, - // m_robotBoomBoom, - // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - // Storage Management - // m_robotStorage.setDefaultCommand( - // new RunCommand(() -> m_robotStorage.manageStorage(), - // m_robotStorage).withName("Storage manageStorage defaultCommand")); - - // Serializer Management + // Serializer Manual m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); - // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + // Turret Manual m_robotTurret.setDefaultCommand( new RunCommand(() -> { if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); + // Hood Manual m_robotHood.setDefaultCommand( new RunCommand(() -> { if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); + //Climber Manual m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getRightY() * 0.5) - , m_robotClimber)); - - // m_robotTurret.setDefaultCommand( - // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); - - // 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")); - + new RunCommand(() -> { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); } + }, m_robotClimber)); // autoInit(); // recordInit(); } @@ -256,138 +230,115 @@ public class RobotContainer { */ private void configureButtonBindings() { - /* Driver Buttons */ + //! Driver Buttons // Start > Calibrate Odometry new JoystickButton(getDriverController(), XboxController.Button.kBack.value) .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - // Start > Calibrate Odometry + + // Start > Calibrate Odometry new JoystickButton(getDriverController(), XboxController.Button.kStart.value) - .whenPressed(m_robotSwerveDrive::resetGyro); + .whenPressed(m_robotSwerveDrive::resetGyro); + // Left Bumper > Shift Down new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // Right Bumper > Shift Up new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - // new JoystickButton(getDriverController(), XboxController.Button.kA.value) - // .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true)) - // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - - // new JoystickButton(getDriverController(), XboxController.Button.kB.value) - // .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)) - // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); - - // new JoystickButton(getDriverController(), XboxController.Button.kY.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); - // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - - // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - - /* Operator Buttons */ - + + + //! Operator Buttons + + // Right Bumper > Storage Out + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + // Left Bumper > Storage In + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + // B > Toggle claws + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(new RunCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); + + // X > Toggles extender in and out + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + + // A > Spit Out Ball + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); + + // Y > Full aim command new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + // .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)); - - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - //Toggles extender in and out - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) - // .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); - - // Right Bumper > Storage In - // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) - // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - - // Left Bumper > Storage Out (note: necessary?) - // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) - // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - - // A > Shoot with Odo - /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ - - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); - - //B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - // .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) - .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // .whileHeld% - - /* Button Box Buttons */ + //! Button Box Buttons + // Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) - .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) - .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) - .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) - .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) - .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + // Middle Switch > Climber and Shooter mode switching new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + // Left Button > Extender In new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // Left Button > Extender Out new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } /** diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 806446d..5fe6a4e 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -74,6 +74,7 @@ public class AimToCenter extends CommandBase { public static double angleToCenter(double x, double y, double gyro) { double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + // double angle = Math.toDegrees(Math.atan2(y, -x) - gyro); return (angle - 360); } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 2b811b0..f5fcc1b 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -32,140 +32,35 @@ public class Claws extends SubsystemBase { private boolean m_open; public static enum ClawType {LEFT, RIGHT} - public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) { + public Claws(Servo leftClaw, 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.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; - - // m_leftClaw.set(ClawConstants.CALIBRATION_SPEED); - // m_rightClaw.set(ClawConstants.CALIBRATION_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) { - - // 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.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.setSpeed(direction * 0.1); - // } - - // m_open = open; - // } - - // 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); - // } - // } - - // 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.setPosition(.7); - // m_rightClaw.setPosition(.7); - m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);//ClawConstants.OPEN_POSITION); - m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);//ClawConstants.OPEN_POSITION); + m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900); + m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100); + SmartDashboard.putBoolean("Claws Closed", false); } 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 - 100); - m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 700); + m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 400); + m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 400); + SmartDashboard.putBoolean("Claws Closed", true); } } - // public double[] getOffsets() { - // return new double[] {m_leftOffset, m_rightOffset}; - // } + public void toggleClaws() { + m_open = !m_open; + setOpen(m_open); + } - // 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; - // } - - /** - * 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) { - // 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 + @Override 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); - // } - - // 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(); - // m_rightOffset = m_rightClaw.getPosition(); } } \ No newline at end of file From b61548d114d9fd4e18be9fdc37649bc8f38d31a0 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Sun, 20 Mar 2022 20:36:23 -0600 Subject: [PATCH 176/180] Fixed TrackTarget driving + cleaned code --- .../commands/ShooterCommands/TrackTarget.java | 109 +++++++----------- 1 file changed, 41 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 79f2a11..3c83331 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.RobotContainer; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; @@ -34,29 +35,18 @@ public class TrackTarget extends CommandBase { BoomBoom m_boomBoom; Hood m_hood; - // use odometry to find x and y later - double x; - double y; - double distance; - double vel; - double hood; - double average; - double output; - Pose2d pos = new Pose2d(); + static double velocity; + static double hoodPosition; + ArrayList points = new ArrayList<>(); private boolean targetLocked = false; private double velocityTolerance = 100.0; private double hoodTolerance = 5.0; - double m=0; - double b=0; boolean isExecuted = false; - // public static Gains m_aimGains; - public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) { - // Use addRequirements() here to declare subsystem dependencies. m_swerve = swerve; m_turret = turret; m_boomBoom = boomBoom; @@ -69,81 +59,57 @@ public class TrackTarget extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - x = 0; - y = 0; + velocity = 0; + hoodPosition = 0; } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); - SmartDashboard.putBoolean("Target Locked", this.targetLocked); - + public void execute() { try { m_visionOdometry.setLEDs(true); + points = m_visionOdometry.getTargetPoints(); - points = filterPoints(points); - Point average = VisionOdometry.averagePoint(points); - DesmosServer.putPoint("average", average); - for(int i = 0; i < points.size(); i++) { - DesmosServer.putPoint("Point" + i, points.get(i)); - } - - output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; + double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; output *= 2; - // //output *= 0.5; - // //DesmosServer.putDouble("output", output); + m_turret.runTurretWithInput(output); double position = m_turret.m_boomBoomRotateEncoder.getPosition(); if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 || Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5) - m_swerve.driveWithInput(0, 0, output, false); + m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true); + else + m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), + RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(), + true); - double y_rot = average.y / VisionConstants.LIME_VIXELS; - y_rot *= Math.toRadians(VisionConstants.V_FOV); - y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; - y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + double regressedDistance = getDistance(average.y); - double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); - DesmosServer.putDouble("distance", distance); + // ! add 30 to the distance to get in target. May need to be adjusted + velocity = m_boomBoom.getVelocity(regressedDistance + 30); + hoodPosition = m_boomBoom.getHood(regressedDistance + 30); - updateRegressionDesmos(); - double regressedDistance = distanceRegression(distance); - regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; - DesmosServer.putDouble("distanceReg", regressedDistance); - - //Vision odometry circle fit based pose estimate - // Point targetOffset = m_visionOdometry.getTargetOffset(); - // DesmosServer.putPoint("targetOff", targetOffset); - - vel = m_boomBoom.getVelocity(regressedDistance + 30); - hood = m_boomBoom.getHood(regressedDistance + 30); - // m_boomBoom.runDrumShooter(vel); - m_boomBoom.runDrumShooterVelocityPID(vel); - m_hood.runAngleAdjustPID(hood); + m_boomBoom.runDrumShooterVelocityPID(velocity); + m_hood.runAngleAdjustPID(hoodPosition); double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity(); double currentHood = this.m_hood.getEncoderPosition(); - this.targetLocked = (Math.abs(currentDrumVel - vel) < velocityTolerance) && (Math.abs(currentHood - hood) < hoodTolerance); + targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance); SmartDashboard.putNumber("Regressed Distance", regressedDistance); - SmartDashboard.putNumber("Distance", distance); - SmartDashboard.putNumber("Hood Target Angle Track", hood); - SmartDashboard.putNumber("Vel Target Track", vel); - - // isExecuted = true; - } - catch (Exception e){ + // SmartDashboard.putNumber("Distance", distance); + SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition); + SmartDashboard.putNumber("Vel Target Track", velocity); + SmartDashboard.putBoolean("Target Locked", targetLocked); + } catch (Exception e){ e.printStackTrace(); - // System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java"); } - // m_turret.runshooterRotatePID(m_targetAngle); } public ArrayList filterPoints(ArrayList points) { @@ -166,15 +132,22 @@ public class TrackTarget extends CommandBase { return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); } - public final double distanceRegression(double distance) { - return (79.6078 * Math.pow(1.01343, distance) - 56.6671); + public final double getDistance(double averageY) { + double y_rot = averageY / VisionConstants.LIME_VIXELS; + y_rot *= Math.toRadians(VisionConstants.V_FOV); + y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2; + y_rot += Math.toRadians(VisionConstants.LIME_ANGLE); + + double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot); + + double regressedDistance = distanceRegression(distance); + regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS; + + return regressedDistance; } - public void updateRegressionDesmos() { - m = DesmosServer.readDouble("m"); - b = DesmosServer.readDouble("b"); - - DesmosServer.putArray("MB", m, b); + public final double distanceRegression(double distance) { + return (79.6078 * Math.pow(1.01343, distance) - 56.6671); } // Called once the command ends or is interrupted. From ef037ed3d9e7ad26d8faa187b122aeaa587f8cf7 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 20:45:14 -0600 Subject: [PATCH 177/180] change --- .../robot/commands/ShooterCommands/Shoot.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index d55f4e8..e46b6cf 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -106,12 +106,18 @@ public class Shoot extends CommandBase { * Updates error for custom PID. */ public void updateError() { - // targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); - targetAngle = 0; + targetAngle = AimToCenter.aaravAngleToCenter(odoX, odoY, swerve.getRegGyro().getDegrees()); + error = (targetAngle - turret.getBoomBoomAngleDegrees()) % 360; + // if (error > 180) { - // error = 360 - error; // TODO: error - 360 - // this.inverted = -1; } else { this.inverted = 1; } + // error = error - 360; + // this.inverted = -1; + // } else { + // this.inverted = 1; + // } + if (error > 180) { error = error - 360; } + isAimedInTolerance = (Math.abs(error) <= tolerance); } @@ -122,7 +128,7 @@ public class Shoot extends CommandBase { startTime = 0; this.odoX = 0.9398;//-this.swerve.getOdometry().getY(); // 3.2766 - this.odoY = -3.2766;//-this.swerve.getOdometry().getX(); // 0.9398 + this.odoY = -3.2766;//-this.swerve.getOdometry().getX(); // -0.9398 this.distance = Math.hypot(odoX, odoY); @@ -151,7 +157,7 @@ public class Shoot extends CommandBase { this.integral = integral + (error * Constants.LOOP_TIME); this.derivative = (error - prevError) / Constants.LOOP_TIME; this.output = kP * proportional + kI * integral + kD * derivative; - this.normOutput = (output / 360) * inverted; + this.normOutput = (output / 360); // inverted; } // Called every time the scheduler runs while the command is scheduled. @@ -165,7 +171,6 @@ public class Shoot extends CommandBase { SmartDashboard.putNumber("Normalized Output", this.normOutput); this.turret.runTurretWithCustomPID(normOutput); - // this.turret.m_boomBoomRotateMotor.set(normOutput); this.swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), normOutput * (this.swerveGains.kP/this.turretGains.kP), true); // ? should the output be field relative if (this.toShoot) { From b1365185195c59823537f3ef200a21298ecc53ed Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 20:52:05 -0600 Subject: [PATCH 178/180] fixes --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b46a442..20c9f21 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -162,7 +162,7 @@ public final class Constants { public static final int STORAGE_CAN_ID = 18; public static final int BEAM_SENSOR_SHOOTER = 28; //TODO public static final int BEAM_SENSOR_INTAKE = 29; //TODO - public static final double STORAGE_SPEED = 0.3; + public static final double STORAGE_SPEED = 0.9; } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9ceded1..9e756d8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -264,7 +264,7 @@ public class RobotContainer { // B > Toggle claws new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new RunCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); + .whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); // X > Toggles extender in and out new JoystickButton(getOperatorController(), XboxController.Button.kX.value) @@ -272,7 +272,7 @@ public class RobotContainer { // A > Spit Out Ball new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)) + .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)) .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); // Y > Full aim command From 6012f9943973526833ddf21cb17f12914f7f5073 Mon Sep 17 00:00:00 2001 From: Ryan Date: Sun, 20 Mar 2022 21:21:45 -0600 Subject: [PATCH 179/180] fixes --- src/main/deploy/pathplanner/Move Forward.path | 2 +- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 20 ++++++++++--------- .../robot/commands/ShooterCommands/Shoot.java | 4 ++-- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path index 653ed78..b63994d 100644 --- a/src/main/deploy/pathplanner/Move Forward.path +++ b/src/main/deploy/pathplanner/Move Forward.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":4.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":{"x":3.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.8761638155965237,"y":2.097178183811552},"prevControl":null,"nextControl":{"x":3.891027717524718,"y":2.097178183811552},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.989837747689554,"y":2.097178183811552},"prevControl":{"x":4.984427304969435,"y":2.097178183811552},"nextControl":{"x":4.984427304969435,"y":2.097178183811552},"holonomicAngle":91.52752544221289,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.8761638155965237,"y":2.140568077269722},"prevControl":{"x":3.3359301436282043,"y":2.140568077269722},"nextControl":null,"holonomicAngle":178.2100893917539,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 20c9f21..650a28d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,7 +175,7 @@ public final class Constants { public static final int ELBOW_ID = 31; public static final int GYRO_ID = 14; - public static final double INPUT_MULTIPLIER = 0.4; + public static final double INPUT_MULTIPLIER = 1.0; public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0; public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9e756d8..c509da2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -267,8 +267,8 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); // X > Toggles extender in and out - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + // .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); // A > Spit Out Ball new JoystickButton(getOperatorController(), XboxController.Button.kA.value) @@ -276,14 +276,16 @@ public class RobotContainer { .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); // Y > Full aim command - new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + // .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); + + + //! Test Buttons + new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); @@ -423,7 +425,7 @@ public class RobotContainer { // * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")), // * new RunCommand(() -> m_robotIntake.runAtOutput(0.5)))); - return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Move Forward")); // test command + return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command } public static XboxController getDriverController() { diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index e46b6cf..7055573 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -127,8 +127,8 @@ public class Shoot extends CommandBase { timerStarted = false; startTime = 0; - this.odoX = 0.9398;//-this.swerve.getOdometry().getY(); // 3.2766 - this.odoY = -3.2766;//-this.swerve.getOdometry().getX(); // -0.9398 + this.odoX = -this.swerve.getOdometry().getY(); // 3.2766 + this.odoY = -this.swerve.getOdometry().getX(); // -0.9398 this.distance = Math.hypot(odoX, odoY); From f8092825f6e462d2ba79297cd09c8d89a837bcef Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sun, 20 Mar 2022 21:22:55 -0600 Subject: [PATCH 180/180] bye bye odo chooser :cry: --- src/main/java/frc4388/robot/Robot.java | 41 ++------------------------ 1 file changed, 2 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 9b5f03c..9929191 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -42,10 +42,7 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - - private SendableChooser odoChooser = new SendableChooser(); - private HashMap odoChoices = new HashMap<>(); - private Pose2d selectedOdo; + private double current; private static DesmosServer desmosServer = new DesmosServer(8000); @@ -140,8 +137,6 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - // SmartDashboard.putNumber("Turret", m_robotContainer.m_robotMap.shooterTurret.getEncoder().getPosition()); - // SmartDashboard.putNumber("Hood", m_robotContainer.m_robotMap.angleAdjusterMotor.getEncoder().getPosition()); // current = // m_robotContainer.m_robotBoomBoom.getCurrent() + // m_robotContainer.m_robotClimber.getCurrent(); //+ @@ -154,7 +149,7 @@ public class Robot extends TimedRobot { // m_robotContainer.m_robotTurret.getCurrent(); // SmartDashboard.putNumber("Total Robot Current Draw", current); // SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage()); - SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); + // SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent()); // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled // commands, running already-scheduled commands, removing finished or @@ -166,26 +161,6 @@ 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()); - // odo chooser stuff - addOdoChoices(new Pose2d(3.2766, -0.9398, new Rotation2d(0))); - updateOdoChooser(); - SmartDashboard.putData("Odometry Chooser", odoChooser); - } - - public void updateOdoChooser() { - for (Map.Entry entry : odoChoices.entrySet()) { - odoChooser.addOption(entry.getKey(), entry.getValue()); - } - } - - public void addOdoChoices(Pose2d... points) { - for (Pose2d point : points) { - String key = "(" + point.getX() + ", " + point.getY() + ", " + point.getRotation().getDegrees() + ")"; - odoChoices.put(key, point); - } } /** @@ -225,12 +200,6 @@ public class Robot extends TimedRobot { Robot.alliance = DriverStation.getAlliance(); - selectedOdo = odoChooser.getSelected(); - if (selectedOdo == null) { - selectedOdo = m_robotContainer.getOdometry(); - } - m_robotContainer.resetOdometry(selectedOdo); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) @@ -253,12 +222,6 @@ public class Robot extends TimedRobot { Robot.alliance = DriverStation.getAlliance(); - selectedOdo = odoChooser.getSelected(); - if (selectedOdo == null) { - selectedOdo = m_robotContainer.getOdometry(); - } - // m_robotContainer.resetOdometry(selectedOdo); - // 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