From 2b71be8c7cb317e39168e6cbe1ea5ac667bb5bd4 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Fri, 20 Jan 2023 18:06:53 -0700 Subject: [PATCH 01/10] started arm subsystem --- src/main/java/frc4388/robot/RobotMap.java | 20 ++++++++++- .../java/frc4388/robot/subsystems/Arm.java | 33 +++++++++++++++++++ 2 files changed, 52 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Arm.java diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 08fcc1a..041cc7e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -31,10 +31,10 @@ public class RobotMap { public SwerveModule leftBack; public SwerveModule rightBack; - public RobotMap() { configureLEDMotorControllers(); configureDriveMotors(); + configArmMotors(); } /* LED Subsystem */ @@ -128,4 +128,22 @@ public class RobotMap { this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET); } + + // arm stuff + public WPI_TalonFX pivot = new WPI_TalonFX(-1); // TODO: Add real id + public WPI_TalonFX tele = new WPI_TalonFX(-1); // TODO: Add real id + + public void configArmMotors() { + // config factory default + pivot.configFactoryDefault(); + tele.configFactoryDefault(); + + // config open loop ramp + pivot.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + tele.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config closed loop ramp + pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + } } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java new file mode 100644 index 0000000..85e8860 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -0,0 +1,33 @@ +// 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 edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + /** Creates a new Arm. */ + private WPI_TalonFX pivot; + private WPI_TalonFX tele; + + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { + this.pivot = pivot; + this.tele = tele; + } + + public void runPivot(double output) { + pivot.set(output); + } + + public void runTelescope(double output) { + tele.set(output); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 904c3dcc2d6570ba1c5580d388a0b3f6ae8ebd5d Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Sat, 21 Jan 2023 13:22:37 -0700 Subject: [PATCH 02/10] defualt command and soft limits --- src/main/java/frc4388/robot/Constants.java | 10 ++++++++++ src/main/java/frc4388/robot/RobotContainer.java | 8 ++++++++ src/main/java/frc4388/robot/RobotMap.java | 17 +++++++++++++++++ src/main/java/frc4388/robot/subsystems/Arm.java | 15 ++++++++++++++- 4 files changed, 49 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9b39fd4..148dea2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -104,6 +104,16 @@ public final class Constants { public static final class GyroConstants { public static final int ID = -1; // TODO: find the actual ID } + + public static final class ArmConstants { + public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value + public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value + + public static final double TELE_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value + public static final double TELE_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value + + + } 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 a42a463..edbac4f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,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.Arm; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; @@ -33,7 +34,9 @@ public class RobotContainer { /* Subsystems */ private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); + private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele); private final LED m_robotLED = new LED(m_robotMap.LEDController); + /* Controllers */ @@ -58,6 +61,11 @@ public class RobotContainer { getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive) ); + + m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.runPivotAndTele( + getOperatorController().getLeftYAxis(), + getOperatorController().getLeftXAxis()), m_robotArm) + ); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 041cc7e..41a2f83 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,11 +7,13 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.wpilibj.motorcontrol.Spark; +import frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; @@ -145,5 +147,20 @@ public class RobotMap { // config closed loop ramp pivot.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); tele.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config neutral mode to brake + pivot.setNeutralMode(NeutralMode.Brake); + tele.setNeutralMode(NeutralMode.Brake); + + // soft limits + pivot.configForwardSoftLimitThreshold(ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + pivot.configReverseSoftLimitThreshold(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT); + pivot.configForwardSoftLimitEnable(false); + pivot.configReverseSoftLimitEnable(false); + + tele.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT); + tele.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT); + tele.configForwardSoftLimitEnable(false); + tele.configReverseSoftLimitEnable(false); } } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 85e8860..af214ba 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -22,10 +22,23 @@ public class Arm extends SubsystemBase { pivot.set(output); } - public void runTelescope(double output) { + public void runTele(double output) { tele.set(output); } + public void runPivotAndTele(double pOutput, double tOutput) { + pivot.set(pOutput); + tele.set(tOutput); + } + + public double getPivotPos() { + return pivot.getSelectedSensorPosition(); + } + + public double getTelePos() { + return tele.getSelectedSensorPosition(); + } + @Override public void periodic() { // This method will be called once per scheduler run From f5b7e5ffefb5857322450296ed3c1d5577459cde Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 4 Feb 2023 14:27:40 -0700 Subject: [PATCH 03/10] limit switches and soft limit created --- src/main/java/frc4388/robot/subsystems/Arm.java | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index af214ba..87feb56 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -4,18 +4,30 @@ package frc4388.robot.subsystems; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants; public class Arm extends SubsystemBase { /** Creates a new Arm. */ private WPI_TalonFX pivot; private WPI_TalonFX tele; + + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { this.pivot = pivot; this.tele = tele; + + //Limit switches + pivot.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + pivot.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + + tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + tele.configForwardSoftLimitThreshold(Constants.ArmConstants.TELE_FORWARD_SOFT_LIMIT); } public void runPivot(double output) { @@ -41,6 +53,8 @@ public class Arm extends SubsystemBase { @Override public void periodic() { - // This method will be called once per scheduler run + if (tele.isRevLimitSwitchClosed() == 1) { + tele.setSelectedSensorPosition(0); + } } } From dd4223e13ee0161f0eab48d7bbdeb0f6bd706681 Mon Sep 17 00:00:00 2001 From: Connorppeach Date: Thu, 9 Feb 2023 19:13:55 -0700 Subject: [PATCH 04/10] arm limits --- src/main/java/frc4388/robot/Constants.java | 11 +- .../java/frc4388/robot/RobotContainer.java | 5 +- src/main/java/frc4388/robot/RobotMap.java | 1 - .../java/frc4388/robot/subsystems/Arm.java | 120 ++++++++++-------- 4 files changed, 77 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 148dea2..cc3493d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -106,13 +106,16 @@ public final class Constants { } public static final class ArmConstants { + public static final double MIN_ARM_LEN = 1; + public static final double MAX_ARM_LEN = 2; + public static final double ARM_HEIGHT = 1; + public static final double CURVE_POWER = 2; + public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value - public static final double TELE_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value - public static final double TELE_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value - - + public static final double TELE_FORWARD_SOFT_LIMIT = 0; + public static final double TELE_REVERSE_SOFT_LIMIT = 0; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index edbac4f..2f994a9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,9 +62,8 @@ public class RobotContainer { -getDriverController().getRightXAxis(), false), m_robotSwerveDrive) ); - m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.runPivotAndTele( - getOperatorController().getLeftYAxis(), - getOperatorController().getLeftXAxis()), m_robotArm) + m_robotArm.setDefaultCommand(new RunCommand(() -> m_robotArm.armSetLength( + getOperatorController().getLeftYAxis()), m_robotArm) ); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 41a2f83..faf7ce2 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -63,7 +63,6 @@ public class RobotMap { public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); - void configureDriveMotors() { // config factory default leftFrontWheel.configFactoryDefault(); diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 87feb56..7b71a03 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -1,60 +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.subsystems; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - +import frc4388.robot.Constants.ArmConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; public class Arm extends SubsystemBase { - /** Creates a new Arm. */ - private WPI_TalonFX pivot; - private WPI_TalonFX tele; - - - - public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { - this.pivot = pivot; - this.tele = tele; - - //Limit switches - pivot.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - pivot.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - - tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - tele.configForwardSoftLimitThreshold(Constants.ArmConstants.TELE_FORWARD_SOFT_LIMIT); - } - - public void runPivot(double output) { - pivot.set(output); - } - - public void runTele(double output) { - tele.set(output); - } - - public void runPivotAndTele(double pOutput, double tOutput) { - pivot.set(pOutput); - tele.set(tOutput); - } - - public double getPivotPos() { - return pivot.getSelectedSensorPosition(); - } - - public double getTelePos() { - return tele.getSelectedSensorPosition(); - } - - @Override - public void periodic() { - if (tele.isRevLimitSwitchClosed() == 1) { - tele.setSelectedSensorPosition(0); + private WPI_TalonFX m_tele; + private WPI_TalonFX m_pivot; + private boolean m_debug; + // Moves arm to distence [dist] then returns new ang + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, boolean debug) { + tele.configFactoryDefault(); + m_tele = tele; + pivot.configFactoryDefault(); + m_pivot = pivot; + } + + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { + this(pivot, tele, false); + } + + public void armSetRotation(double rot) { + if (rot > 1 || rot < 0) return; + // Move arm code + m_pivot.set(ControlMode.Position, rot * (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) + + ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + } + + public void armSetLength(double len) { + if (len > 1 || len < 0) return; + // Move arm code + m_tele.set(ControlMode.Position, len * (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + + ArmConstants.TELE_FORWARD_SOFT_LIMIT); + } + + public double getArmLength() { + return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) / + (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT); + } + + public void runPivotTele(double pivot, double tele) { + var rot = 0; + + if (checkLimits(tele, rot)) { + armSetRotation(pivot); + armSetLength(tele); + } + } + + /** + * Checks that an input is within bounds + * @param _len length from 0 to 1 + * @param _theta radians from the zero (straight up) + * @return + */ + public static boolean checkLimits(double _len, double _theta) { + var len = _len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + ArmConstants.MIN_ARM_LEN; + var x = len * Math.cos(_theta); + var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta); + + var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x)); + if (y < minHeight) + return false; + + return true; + } + + @Override + public void periodic() { + if (m_debug) + SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); } - } } From 0e060243927021cd33b7c6ad2b6a152f2023ff70 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 13 Feb 2023 18:12:32 -0700 Subject: [PATCH 05/10] change --- src/main/java/frc4388/robot/subsystems/Arm.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 7b71a03..ae7959b 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -10,12 +10,14 @@ public class Arm extends SubsystemBase { private WPI_TalonFX m_tele; private WPI_TalonFX m_pivot; private boolean m_debug; - // Moves arm to distence [dist] then returns new ang + + // Moves arm to distance [dist] then returns new ang public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, boolean debug) { - tele.configFactoryDefault(); m_tele = tele; - pivot.configFactoryDefault(); m_pivot = pivot; + + tele.configFactoryDefault(); + pivot.configFactoryDefault(); } public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { From cb42b636eab987e3fbb39258e5fa9f2f28eecfa7 Mon Sep 17 00:00:00 2001 From: Abhi Date: Mon, 13 Feb 2023 19:33:26 -0700 Subject: [PATCH 06/10] Update build.gradle --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index b31ed86..d46f7ae 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.1.1" + id "edu.wpi.first.GradleRIO" version "2023.3.2" } sourceCompatibility = JavaVersion.VERSION_11 From 2e7177a002ddf2b97e779517a3ff7ec8cfae7368 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 13 Feb 2023 19:16:23 -0700 Subject: [PATCH 07/10] MATH.ABS --- src/main/java/frc4388/robot/subsystems/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index ae7959b..23aebdc 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -27,14 +27,14 @@ public class Arm extends SubsystemBase { public void armSetRotation(double rot) { if (rot > 1 || rot < 0) return; // Move arm code - m_pivot.set(ControlMode.Position, rot * (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) + + m_pivot.set(ControlMode.Position, rot * Math.abs(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) + ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); } public void armSetLength(double len) { if (len > 1 || len < 0) return; // Move arm code - m_tele.set(ControlMode.Position, len * (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + + m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + ArmConstants.TELE_FORWARD_SOFT_LIMIT); } From 1fa95bbacb78bf2451122e6b907b37174b8ea476 Mon Sep 17 00:00:00 2001 From: 66945 <54561572+66945@users.noreply.github.com> Date: Tue, 28 Feb 2023 17:47:35 -0700 Subject: [PATCH 08/10] extra stuff --- src/main/java/frc4388/robot/Constants.java | 6 +++ .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 5 ++- .../java/frc4388/robot/subsystems/Arm.java | 40 ++++++++++++++++--- 4 files changed, 44 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cc3493d..e06d69c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -116,6 +116,12 @@ public final class Constants { public static final double TELE_FORWARD_SOFT_LIMIT = 0; public static final double TELE_REVERSE_SOFT_LIMIT = 0; + + public static final double kP = 0; + public static final double kI = 0; + public static final double kD = 0; + + public static final double OFFSET = 0; } public static final class LEDConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2f994a9..a7f47c7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,7 +34,7 @@ public class RobotContainer { /* Subsystems */ private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack, m_robotMap.gyro); - private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele); + private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); private final LED m_robotLED = new LED(m_robotMap.LEDController); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index faf7ce2..0d9ee69 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -131,8 +131,9 @@ public class RobotMap { } // arm stuff - public WPI_TalonFX pivot = new WPI_TalonFX(-1); // TODO: Add real id - public WPI_TalonFX tele = new WPI_TalonFX(-1); // TODO: Add real id + public WPI_TalonFX pivot = new WPI_TalonFX(-1); // TODO: Add real id + public WPI_TalonFX tele = new WPI_TalonFX(-1); // TODO: Add real id + public CANCoder pivotEncoder = new CANCoder(-1); public void configArmMotors() { // config factory default diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 23aebdc..3b5165e 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -1,7 +1,13 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.CANCoderConfiguration; + import frc4388.robot.Constants.ArmConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -9,19 +15,35 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { private WPI_TalonFX m_tele; private WPI_TalonFX m_pivot; - private boolean m_debug; + private CANCoder m_pivotEncoder; + private boolean m_debug; // Moves arm to distance [dist] then returns new ang - public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, boolean debug) { - m_tele = tele; - m_pivot = pivot; + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) { + m_tele = tele; + m_pivot = pivot; + m_pivotEncoder = encoder; + + TalonFXConfiguration pivotConfig = new TalonFXConfiguration(); + pivotConfig.slot0.kP = ArmConstants.kP; + pivotConfig.slot0.kI = ArmConstants.kI; + pivotConfig.slot0.kD = ArmConstants.kD; + + pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + pivot.configAllSettings(pivotConfig); + + CANCoderConfiguration config = new CANCoderConfiguration(); + config.magnetOffsetDegrees = ArmConstants.OFFSET; + m_pivotEncoder.configAllSettings(config); tele.configFactoryDefault(); pivot.configFactoryDefault(); } - public Arm(WPI_TalonFX pivot, WPI_TalonFX tele) { - this(pivot, tele, false); + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { + this(pivot, tele, encoder, false); } public void armSetRotation(double rot) { @@ -36,6 +58,12 @@ public class Arm extends SubsystemBase { // Move arm code m_tele.set(ControlMode.Position, len * Math.abs(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT) + ArmConstants.TELE_FORWARD_SOFT_LIMIT); + + if (m_tele.isRevLimitSwitchClosed() == 1) { + m_tele.setSelectedSensorPosition(ArmConstants.TELE_REVERSE_SOFT_LIMIT); + } else if (m_tele.isFwdLimitSwitchClosed() == 1) { + m_tele.setSelectedSensorPosition(ArmConstants.TELE_FORWARD_SOFT_LIMIT); + } } public double getArmLength() { From 3cdf5e65fe3d89a6610d1440198449c0e81bc440 Mon Sep 17 00:00:00 2001 From: Aarav Date: Wed, 1 Mar 2023 16:45:57 -0700 Subject: [PATCH 09/10] added velocity control --- src/main/java/frc4388/robot/Constants.java | 2 ++ src/main/java/frc4388/robot/subsystems/Arm.java | 13 +++++++++++++ 2 files changed, 15 insertions(+) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e06d69c..3508a8d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -110,6 +110,8 @@ public final class Constants { public static final double MAX_ARM_LEN = 2; public static final double ARM_HEIGHT = 1; public static final double CURVE_POWER = 2; + + public static final double TELE_TICKS_PER_SECOND = (-5); public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 3b5165e..513be03 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -46,6 +46,14 @@ public class Arm extends SubsystemBase { this(pivot, tele, encoder, false); } + public void setRotVel(double vel) { + m_pivot.set(ControlMode.Velocity, vel); + } + + public void setTeleVel(double vel) { + m_tele.set(ControlMode.Velocity, vel); + } + public void armSetRotation(double rot) { if (rot > 1 || rot < 0) return; // Move arm code @@ -71,6 +79,11 @@ public class Arm extends SubsystemBase { (ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT); } + public double getArmRotation() { + return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) / + (ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT); + } + public void runPivotTele(double pivot, double tele) { var rot = 0; From 23a6337a9680d1ea417e2a6acd92b74eda77643c Mon Sep 17 00:00:00 2001 From: Aarav Date: Wed, 1 Mar 2023 16:58:20 -0700 Subject: [PATCH 10/10] fixed build failing --- src/main/java/frc4388/robot/Constants.java | 6 ---- .../java/frc4388/robot/RobotContainer.java | 29 ++----------------- src/main/java/frc4388/robot/RobotMap.java | 2 -- 3 files changed, 2 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ce15b92..9d6381f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -105,12 +105,6 @@ public final class Constants { public static final class GyroConstants { public static final int ID = 14; // TODO: find the actual ID } - - public static final class ArmConstants { - public static final int MIN_ARM_LEN = 0; - public static final int MAX_ARM_LEN = 1; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } public static final class ArmConstants { public static final double MIN_ARM_LEN = 1; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7c897c3..78b2aea 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,42 +7,17 @@ package frc4388.robot; -import java.util.ArrayList; -import java.util.List; - -import org.opencv.objdetect.HOGDescriptor; - -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -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.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.Trajectory.State; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -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.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Arm; -import frc4388.robot.subsystems.LED; -import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; -import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; import frc4388.robot.subsystems.SwerveDrive; -import frc4388.utility.controller.IHandController; -import frc4388.robot.commands.JoystickPlayback; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.PlaybackChooser; -import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -59,7 +34,7 @@ public class RobotContainer { /* Subsystems */ private final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder); - private final LED m_robotLED = new LED(m_robotMap.LEDController); + // private final LED m_robotLED = new LED(m_robotMap.LEDController); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, @@ -75,7 +50,7 @@ public class RobotContainer { /* Autos */ public SendableChooser chooser = new SendableChooser<>(); - private Command noAuto = new InstantCommand(); + // private Command noAuto = new InstantCommand(); // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index e454e52..6762ddd 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,12 +9,10 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_Pigeon2; import frc4388.robot.Constants.ArmConstants; -import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro;