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] 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() {