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 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 29a2125..9d6381f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -105,13 +105,28 @@ 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 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 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 + + 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 { // 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 cf390e9..78b2aea 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,40 +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.Constants.SwerveDriveConstants.AutoConstants; -import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; +import frc4388.robot.subsystems.Arm; 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; @@ -56,6 +33,10 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* 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); + + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, @@ -69,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); @@ -101,7 +82,6 @@ public class RobotContainer { // chooser.addOption("Blue1Path", blue1Path); // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance); - // chooser.addOption("Red1Path", red1Path); // chooser.addOption("Red1PathWithBalance", red1PathWithBalance); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 373d0e8..6762ddd 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,11 +7,12 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; 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 frc4388.robot.Constants.ArmConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; @@ -30,10 +31,10 @@ public class RobotMap { public SwerveModule leftBack; public SwerveModule rightBack; - public RobotMap() { configureLEDMotorControllers(); configureDriveMotors(); + configArmMotors(); } /* LED Subsystem */ @@ -60,7 +61,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(); @@ -126,4 +126,38 @@ public class RobotMap { this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); } + + // 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 CANCoder pivotEncoder = new CANCoder(-1); + + 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); + + // 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 6367d4d..ca7b058 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -1,39 +1,118 @@ 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; public class Arm extends SubsystemBase { - private WPI_TalonFX m_armmotor; - private boolean m_debug; - // Moves arm to distence [dist] then returns new ang - public Arm(WPI_TalonFX armmotor, boolean debug) { - armmotor.configFactoryDefault(); - m_armmotor = armmotor; + private WPI_TalonFX m_tele; + private WPI_TalonFX m_pivot; + 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, 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 armmoter) { - this(armmoter, false); + public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) { + 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 + 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_armmotor.set(ControlMode.Position, len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + - ArmConstants.MIN_ARM_LEN); + 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() { - return (m_armmotor.getSelectedSensorPosition() - ArmConstants.MIN_ARM_LEN) / - (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN); + return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) / + (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; + + 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_armmotor.getSelectedSensorPosition()); + SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition()); } }