From bd43699921d8559e790eeb55120ed0eb15eaa6b8 Mon Sep 17 00:00:00 2001 From: shrubman8787 <138845668+shrubman8787@users.noreply.github.com> Date: Tue, 11 Feb 2025 17:20:23 +0800 Subject: [PATCH] added state machine --- src/main/java/frc/robot/Robot.java | 29 +- src/main/java/frc/robot/RobotContainer.java | 40 +++ .../frc/robot/commands/TeleopSuperStruct.java | 43 +++ .../java/frc/robot/states/StateMachine.java | 29 ++ .../frc/robot/states/SuperStructState.java | 15 + .../frc/robot/subsystems/AlgaeIntake.java | 174 ++++++++++++ .../{ElevatorSubsystem.java => Elevator.java} | 128 +++++++-- .../java/frc/robot/subsystems/Grabber.java | 267 +++++------------- .../java/frc/robot/subsystems/Intake.java | 162 +++++++++++ .../frc/robot/subsystems/SuperStruct.java | 229 +++++++++++++++ 10 files changed, 890 insertions(+), 226 deletions(-) create mode 100644 src/main/java/frc/robot/RobotContainer.java create mode 100644 src/main/java/frc/robot/commands/TeleopSuperStruct.java create mode 100644 src/main/java/frc/robot/states/StateMachine.java create mode 100644 src/main/java/frc/robot/states/SuperStructState.java create mode 100644 src/main/java/frc/robot/subsystems/AlgaeIntake.java rename src/main/java/frc/robot/subsystems/{ElevatorSubsystem.java => Elevator.java} (70%) create mode 100644 src/main/java/frc/robot/subsystems/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/SuperStruct.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 97207ec..bcc721c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,10 +15,15 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Grabber; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.SuperStruct; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.TeleopSuperStruct; import java.util.Map; @@ -34,6 +39,10 @@ public class Robot extends TimedRobot { private final SendableChooser m_chooser = new SendableChooser<>(); private final Grabber m_grabber = new Grabber(); + private final Intake m_intake = new Intake(); + private final Elevator m_elevator = new Elevator(); + private final SuperStruct m_superStruct = SuperStruct.getInstance(); + private Command m_teleopSuperStruct; /** * This function is run when the robot is first started up and should be used for any @@ -48,7 +57,10 @@ public Robot() { } - + @Override + public void robotInit() { + m_teleopSuperStruct = new TeleopSuperStruct(m_superStruct); + } /** * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics @@ -59,7 +71,10 @@ public Robot() { */ @Override public void robotPeriodic() { - m_grabber.periodic(); // Subsystem handles everything now! + m_grabber.periodic(); + m_intake.periodic(); + m_elevator.periodic(); + m_superStruct.periodic(); } /** @@ -95,7 +110,15 @@ public void autonomousPeriodic() { /** This function is called once when teleop is enabled. */ @Override - public void teleopInit() {} + public void teleopInit() { + m_teleopSuperStruct.schedule(); + } + + /** This function is called once when teleop is disabled. */ + @Override + public void teleopExit() { + m_teleopSuperStruct.cancel(); + } /** This function is called periodically during operator control. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..46fd5b5 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,40 @@ +package frc.robot; + + +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.button.CommandXboxController; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Grabber; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.SuperStruct; +public class RobotContainer { + + private CommandXboxController m_Controller; + private Joystick joystick; + + private final Elevator m_elevator; + private final Grabber m_grabber; + private final Intake m_intake; + private final SuperStruct m_SuperStruct; + + public RobotContainer() { + m_elevator = Elevator.getInstance(); + m_grabber = Grabber.getInstance(); + m_intake = Intake.getInstance(); + m_SuperStruct = SuperStruct.getInstance(); + + configureButtonBindings(); + } + + private void configureButtonBindings() { + } + + public Command getAutonomousCommand() { + return null; + } + + +} diff --git a/src/main/java/frc/robot/commands/TeleopSuperStruct.java b/src/main/java/frc/robot/commands/TeleopSuperStruct.java new file mode 100644 index 0000000..b9969e6 --- /dev/null +++ b/src/main/java/frc/robot/commands/TeleopSuperStruct.java @@ -0,0 +1,43 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.states.SuperStructState; +import frc.robot.subsystems.SuperStruct; + +public class TeleopSuperStruct extends Command { + private final SuperStruct m_superStruct; + + + public TeleopSuperStruct(SuperStruct superStruct) { + m_superStruct = superStruct; + addRequirements(superStruct); + } + + // Called when the command is initially scheduled + @Override + public void initialize() { + // Set to default state when starting teleop + m_superStruct.setState(SuperStructState.DEFAULT); + } + + // Called every time the scheduler runs while the command is scheduled + @Override + public void execute() { + + m_superStruct.handleControllerInput(); + } + + // Called once the command ends or is interrupted + @Override + public void end(boolean interrupted) { + // Return to safe default state when ending + m_superStruct.setState(SuperStructState.DEFAULT); + } + + // Returns true when the command should end + @Override + public boolean isFinished() { + + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/states/StateMachine.java b/src/main/java/frc/robot/states/StateMachine.java new file mode 100644 index 0000000..63eb1fd --- /dev/null +++ b/src/main/java/frc/robot/states/StateMachine.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 frc.robot.states; + +/** Add your docs here. */ +public class StateMachine { + private static StateMachine instance = null; + + private SuperStructState commandedState = SuperStructState.TRAVEL; + + public StateMachine() {} + + public static synchronized StateMachine getInstance() { + if (instance == null) { + instance = new StateMachine(); + } + return instance; + } + + public void setCommandedState(SuperStructState targetState) { + commandedState = targetState; + } + + public SuperStructState getCommandedState() { + return commandedState; + } +} diff --git a/src/main/java/frc/robot/states/SuperStructState.java b/src/main/java/frc/robot/states/SuperStructState.java new file mode 100644 index 0000000..dec5794 --- /dev/null +++ b/src/main/java/frc/robot/states/SuperStructState.java @@ -0,0 +1,15 @@ +package frc.robot.states; + +public enum SuperStructState { + L1, // + L2, // + L3, // + L4, // + TRAVEL, + CS, // CORAL STATION + PLACEMENT, + DEFAULT, + ALGAE_STOWAGE, + ALGAE_INTAKE, + ALGAE_PLACEMENT +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntake.java b/src/main/java/frc/robot/subsystems/AlgaeIntake.java new file mode 100644 index 0000000..58a4ec2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaeIntake.java @@ -0,0 +1,174 @@ +// 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 frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SoftLimitConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class AlgaeIntake extends SubsystemBase { + private final SparkMax leftAngle; + private final SparkMax rightAngle; + private final CANcoder angleEncoder; + private final SparkMax intakeMotor; + private final PIDController anglePIDController; + private static final double ANGLE_kP = 0.01; // Adjust these PID values! uwu + private static final double ANGLE_kI = 0.0; + private static final double ANGLE_kD = 0.0; + private static final double ANGLE_TOLERANCE = 2.0; // Degrees + + private static AlgaeIntake mInstance = null; + + public static AlgaeIntake getInstance() { + if (mInstance == null) { + mInstance = new AlgaeIntake(); + } + return mInstance; + } + + /** Creates a new AlgaeIntake. */ + public AlgaeIntake() { + leftAngle = new SparkMax(10, MotorType.kBrushless); + rightAngle = new SparkMax(11, MotorType.kBrushless); + angleEncoder = new CANcoder(12, "GTX7130"); + intakeMotor = new SparkMax(13, MotorType.kBrushless); + + configureNEO(leftAngle, false, true); + configureSlaveNEO(rightAngle, false, true); + configureNEO(intakeMotor, false, false); + configureCANcoder(); + + // Initialize angle PID controller + anglePIDController = new PIDController(ANGLE_kP, ANGLE_kI, ANGLE_kD); + anglePIDController.setTolerance(ANGLE_TOLERANCE); + } + + public void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { + SoftLimitConfig softLimitConfig = new SoftLimitConfig(); + softLimitConfig + .forwardSoftLimit(0.0) + .forwardSoftLimitEnabled(softlimit) + .reverseSoftLimit(0) + .reverseSoftLimitEnabled(softlimit); + + SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig + .smartCurrentLimit(40) + .idleMode(IdleMode.kBrake) + .voltageCompensation(12) + .inverted(inverted) + .apply(softLimitConfig); + + motor.setCANTimeout(250); + motor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void configureSlaveNEO(SparkMax motor, boolean inverted, boolean softlimit) { + SoftLimitConfig softLimitConfig = new SoftLimitConfig(); + softLimitConfig + .forwardSoftLimit(0.0) //45:1 gear reduction + .forwardSoftLimitEnabled(softlimit) + .reverseSoftLimit(0) + .reverseSoftLimitEnabled(softlimit); + + SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig + .smartCurrentLimit(40) + .idleMode(IdleMode.kBrake) + .voltageCompensation(12) + .inverted(inverted) + .apply(softLimitConfig) + .follow(10); + + motor.setCANTimeout(250); + motor.configure(sparkMaxConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + private void configureCANcoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + // Configure for absolute position mode + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + + // Apply configuration + angleEncoder.getConfigurator().apply(config); + + } + + /** + * Sets the angle of the intake using CANcoder feedback (✿◠‿◠) + * @param targetAngleDegrees The desired angle in degrees + */ + public void setAngle(double targetAngleDegrees) { + // Update PID setpoint + anglePIDController.setSetpoint(targetAngleDegrees); + + // Get current angle from CANcoder + double currentAngle = angleEncoder.getAbsolutePosition().getValueAsDouble(); + + // Calculate PID output + double pidOutput = anglePIDController.calculate(currentAngle); + + // Limit output for safety ʕ•ᴥ•ʔ + pidOutput = MathUtil.clamp(pidOutput, -0.4, 0.4); + + // Set motor outputs (both left and right angle motors) + leftAngle.set(pidOutput); + + } + + /** + * Checks if intake is at the target angle (◕ᴗ◕✿) + * @return true if at target angle, false otherwise + */ + public boolean isAtAngle() { + return anglePIDController.atSetpoint(); + } + + /** + * Gets the current angle from CANcoder uwu + * @return current angle in degrees + */ + public double getCurrentAngle() { + return angleEncoder.getAbsolutePosition().getValueAsDouble() * 360.0; + } + + public void intake() { + intakeMotor.set(0.2); + } + + public void outgae() { //get it? + intakeMotor.set(-0.2); + } + + public void setIntakeSpeed(double speed) { + intakeMotor.set(speed); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + // Add angle info to dashboard + SmartDashboard.putNumber("Algae Current Angle", getCurrentAngle()); + SmartDashboard.putNumber("Algae Target Angle", anglePIDController.getSetpoint()); + SmartDashboard.putBoolean("Algae At Target Angle", isAtAngle()); + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/Elevator.java similarity index 70% rename from src/main/java/frc/robot/subsystems/ElevatorSubsystem.java rename to src/main/java/frc/robot/subsystems/Elevator.java index dc3e583..661282c 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -15,7 +15,11 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.signals.SensorDirectionValue; + import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -23,15 +27,26 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.math.controller.PIDController; +import java.util.Map; +import frc.robot.states.SuperStructState; -public class ElevatorSubsystem extends SubsystemBase { +public class Elevator extends SubsystemBase { private final SparkMax leftMotor; private final SparkMax rightMotor; - // private final CANcoder encoder; + private final CANcoder Cancoder; private static final double kG = 0.05; // Gravity feed forward constant - adjust this! private static final double kDownSpeedMultiplier = 0.5; // Reduces down speed - adjust this! + private static Elevator mInstance = null; + + public static Elevator getInstance() { + if (mInstance == null) { + mInstance = new Elevator(); + } + return mInstance; + } + // Shuffleboard entries private final ShuffleboardTab elevatorTab = Shuffleboard.getTab("Elevator"); private final GenericEntry upButton = elevatorTab.add("Elevator Up", false) @@ -58,18 +73,21 @@ public class ElevatorSubsystem extends SubsystemBase { private final GenericEntry maxLeftRotationsEntry; private final GenericEntry maxRightRotationsEntry; - - private final PIDController pidController = new PIDController(0.07, 0.035, 0.007); // Adjust P, I, D values! private boolean positionLocked = false; private double targetPosition = 0.0; private final GenericEntry lockButton; + + private final CANcoder elevatorCoder; + private static final double CANCODER_OFFSET = 0.0; // Adjust this value! uwu + private final GenericEntry absolutePositionEntry; + /** Creates a new ElevatorSubsystem. */ - public ElevatorSubsystem() { + public Elevator() { leftMotor = new SparkMax(4, MotorType.kBrushless); // Update ID as needed rightMotor = new SparkMax(5, MotorType.kBrushless); // Update ID as needed - // encoder = new CANcoder(3); // Update CANcoder ID as needed! + Cancoder = new CANcoder(3); // Update CANcoder ID as needed! configureNEO(leftMotor, false); // Invert both motors to make default direction clockwise configureNEO(rightMotor, false); // Both motors still turn the same way @@ -109,8 +127,15 @@ public ElevatorSubsystem() { .withSize(1, 1) .getEntry(); - - // or + // Initialize CANcoder (ID 3 from your existing code) + elevatorCoder = new CANcoder(3); + configureCANcoder(); + + // Add absolute position display to dashboard + absolutePositionEntry = elevatorTab.add("Elevator Absolute Position", 0.0) + .withPosition(0, 7) + .withSize(2, 1) + .getEntry(); } private void configureNEO(SparkMax motor, boolean inverted) { @@ -142,6 +167,62 @@ private void configureNEO(SparkMax motor, boolean inverted) { motor.getEncoder().setPosition(0.0); // Reset encoder to zero } + /** + * Configure the CANcoder for absolute position reading (◕ᴗ◕✿) + */ + private void configureCANcoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + + // Configure for absolute position mode + config.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + config.MagnetSensor.MagnetOffset = 0.0; + + // Apply configuration + elevatorCoder.getConfigurator().apply(config); + + // Wait for config to apply + try { + Thread.sleep(100); + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + + /** + * Get the absolute position from CANcoder! (✿◠‿◠) + */ + public double getPosition() { + return Cancoder.getPosition().getValueAsDouble(); + } + + public void setPosition(double targetPosition) { + // Update PID setpoint + pidController.setSetpoint(targetPosition); + + // Calculate PID output + double currentPosition = leftMotor.getEncoder().getPosition(); + double pidOutput = pidController.calculate(currentPosition); + + // Add gravity compensation + double gravityCompensation = (pidOutput >= 0) ? kG : 0.0; + + // Set motor output + setElevatorSpeed(pidOutput + gravityCompensation); +} + +public boolean isAtPosition() { + double currentPosition = leftMotor.getEncoder().getPosition(); + return Math.abs(currentPosition - pidController.getSetpoint()) < 0.1; +} + /** + * Reset the NEO encoders based on CANcoder position ʕ•ᴥ•ʔ + */ + public void resetToAbsolute() { + double absolutePosition = getPosition(); + leftMotor.getEncoder().setPosition(absolutePosition); + rightMotor.getEncoder().setPosition(absolutePosition); + } + /** * Sets the elevator speed. Positive values move up, negative values move down. * Includes gravity compensation when moving up and speed reduction when moving down! ^w^ @@ -190,16 +271,11 @@ public void unlockPosition() { positionLocked = false; } + + @Override public void periodic() { - // Check for negative position and reset if needed - if (leftMotor.getEncoder().getPosition() < 0) { - leftMotor.getEncoder().setPosition(0.0); - } - if (rightMotor.getEncoder().getPosition() < 0) { - rightMotor.getEncoder().setPosition(0.0); - } - + // Handle position locking if (lockButton.getBoolean(false)) { if (!positionLocked) { @@ -223,13 +299,6 @@ public void periodic() { } } - // Update values instead of creating new widgets - speedEntry.setDouble(leftMotor.get()); - // positionEntry.setDouble(encoder.getPosition().getValueAsDouble()); //cancoder - positionEntry.setDouble(leftMotor.getEncoder().getPosition()); // encoder - leftRotationsEntry.setDouble(leftMotor.getEncoder().getPosition()); - rightRotationsEntry.setDouble(rightMotor.getEncoder().getPosition()); - // Track maximum rotations double leftRotations = Math.abs(leftMotor.getEncoder().getPosition()); double rightRotations = Math.abs(rightMotor.getEncoder().getPosition()); @@ -240,5 +309,18 @@ public void periodic() { // Update max rotation displays maxLeftRotationsEntry.setDouble(maxLeftRotations); maxRightRotationsEntry.setDouble(maxRightRotations); + + // State machine control (✿◠‿◠) + double currentHeight = leftMotor.getEncoder().getPosition(); + double pidOutput = pidController.calculate(currentHeight); + + // Add gravity compensation! ˎ₍•ʚ•₎ˏ + double gravityCompensation = kG; + + setElevatorSpeed(pidOutput + gravityCompensation); + + + // Update absolute position display + absolutePositionEntry.setDouble(getPosition()); } } diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index 1756ae2..802b5cd 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -15,230 +15,96 @@ import java.util.Map; import edu.wpi.first.math.controller.PIDController; + public class Grabber extends SubsystemBase { - private final SparkMax up; - private final SparkMax down; private final SparkMax angle; - // private final CANCoder angleEncoder; private static final double DEFAULT_KG = 0.003; - private int startupCounter = 0; - private int stallCounter = 0; - // Shuffleboard entries + private static Grabber mInstance = null; + + public static Grabber getInstance() { + if (mInstance == null) { + mInstance = new Grabber(); + } + return mInstance; + } + + // Shuffleboard entries for angle control only private final ShuffleboardTab grabberTab = Shuffleboard.getTab("Grabber"); - // private final GenericEntry upButton, downButton, upSpeed, downSpeed; - // private final GenericEntry forwardButton, backwardButton, flatButton; - // private final GenericEntry angleDisplay, kGTuner; - // private final GenericEntry bothInButton, bothOutButton; - // private final GenericEntry upRPMStatus, downRPMStatus; - - // private final ShuffleboardTab motorTab = Shuffleboard.getTab("Motor Controls"); - private final GenericEntry upButton = grabberTab.add("Up Motor", false) - .withWidget("Toggle Button") - .withPosition(0, 0) - .getEntry(); - private final GenericEntry downButton = grabberTab.add("Down Motor", false) - .withWidget("Toggle Button") - .withPosition(0, 1) - .getEntry(); - private final GenericEntry upSpeed = grabberTab.add("Up Motor Speed", 0.5) - .withWidget("Number Slider") - .withProperties(Map.of("min", -1.0, "max", 1.0)) - .withPosition(1, 0) - .getEntry(); - private final GenericEntry downSpeed = grabberTab.add("Down Motor Speed", 0.5) - .withWidget("Number Slider") - .withProperties(Map.of("min", -1.0, "max", 1.0)) - .withPosition(1, 1) - .getEntry(); - - // Add preset angle buttons to Shuffleboard - private final GenericEntry forwardButton = grabberTab.add("Turn 45 Forward", false) - .withWidget("Toggle Button") - .withPosition(0, 4) - .withSize(1, 1) - .getEntry(); - private final GenericEntry backwardButton = grabberTab.add("Turn 45 Back", false) - .withWidget("Toggle Button") - .withPosition(1, 4) - .withSize(1, 1) - .getEntry(); - - private final GenericEntry flatButton = grabberTab.add("Flat", false) - .withWidget("Toggle Button") - .withPosition(2, 4) - .withSize(1, 1) - .getEntry(); - - // Add angle display widget - private final GenericEntry angleDisplay = grabberTab.add("Current Angle", 0.0) - .withWidget("Text View") // Shows number clearly UwU - .withPosition(2, 4) - .withSize(1, 1) - .getEntry(); - - // Add kG tuning slider ✨ - private final GenericEntry kGTuner = grabberTab.add("Gravity Compensation", DEFAULT_KG) - .withWidget("Number Slider") - .withProperties(Map.of("min", 0.0, "max", 0.2)) // Reasonable range - .withPosition(3, 4) - .withSize(1, 1) - .getEntry(); - - // Add buttons for synchronized movement ✨ - private final GenericEntry bothInButton = grabberTab.add("Both Motors In", false) - .withWidget("Toggle Button") - .withPosition(0, 5) - .withSize(1, 1) - .getEntry(); - private final GenericEntry bothOutButton = grabberTab.add("Both Motors Out", false) - .withWidget("Toggle Button") - .withPosition(1, 5) - .withSize(1, 1) - .getEntry(); - - // Add RPM monitoring widgets ✨ - private final GenericEntry upRPMStatus = grabberTab.add("Up Motor RPM OK", true) - .withWidget("Boolean Box") // Shows green/red status - .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) - .withPosition(0, 6) - .withSize(1, 1) - .getEntry(); - - private final GenericEntry downRPMStatus = grabberTab.add("Down Motor RPM OK", true) - .withWidget("Boolean Box") - .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) - .withPosition(1, 6) - .withSize(1, 1) - .getEntry(); - - private final PIDController pidController = new PIDController( - 0.07, // kP - 0.035, // kI - helps eliminate steady-state error UwU - 0.007 // kD - reduces overshoot and oscillation ✨ - ); + private final GenericEntry angleDisplay; + private final GenericEntry kGTuner; + private final GenericEntry stateDisplay; + private final PIDController pidController; public Grabber() { - up = new SparkMax(2, MotorType.kBrushless); - down = new SparkMax(3, MotorType.kBrushless); angle = new SparkMax(1, MotorType.kBrushless); - // angleEncoder = angle.getEncoder(); - - configureNEO550(up); - configureNEO550(down); - configureNEO(angle); - + configureNEO(angle, false, true); angle.getEncoder().setPosition(0.0); - + // Initialize Shuffleboard entries + angleDisplay = grabberTab.add("Current Angle", 0.0) + .withWidget("Text View") + .withPosition(2, 4) + .getEntry(); + + kGTuner = grabberTab.add("Gravity Compensation", DEFAULT_KG) + .withWidget("Number Slider") + .withProperties(Map.of("min", 0.0, "max", 0.2)) + .withPosition(3, 4) + .getEntry(); + + stateDisplay = grabberTab.add("Current State", "DEFAULT") + .withWidget("Text View") + .withPosition(2, 0) + .getEntry(); + + pidController = new PIDController(0.07, 0.035, 0.007); } - @Override - public void periodic() { - double kG = kGTuner.getDouble(DEFAULT_KG); - - if (forwardButton.getBoolean(false)) { - angle.set(0.07 + kG); // Now positive is forward - if (angle.getEncoder().getPosition() >= 2.76) { // For 53° at 18.75:1 - angle.set(kG); + /** + * Sets the grabber to a specific angle (✿◠‿◠) + * @param targetAngleDegrees The desired angle in degrees + */ + public void setAngle(double targetAngleDegrees) { + // Convert degrees to motor rotations (18.75:1 gear ratio) + double targetRotations = targetAngleDegrees * (18.75 / 360.0); - } - - } else if (backwardButton.getBoolean(false)) { - angle.set(-0.025); // Now negative is backward - if (angle.getEncoder().getPosition() <= 0) { - angle.set(0); - } - } else if (flatButton.getBoolean(false)) { - angle.set(0.07 + kG); // Move forward - if (angle.getEncoder().getPosition() >= 1.71875) { // 33° position - angle.set(kG); - } - } else { - angle.set(kG * 0.5); // Flipped kG direction too! + // Update PID setpoint + pidController.setSetpoint(targetRotations); } - // Get button states and speeds fow motow contwol OwO - boolean upButtonState = upButton.getBoolean(false); - boolean downButtonState = downButton.getBoolean(false); - - double upSpeedValue = upSpeed.getDouble(0.5); - double downSpeedValue = downSpeed.getDouble(0.5); - - up.set(upButtonState ? upSpeedValue : 0); - down.set(downButtonState ? downSpeedValue : 0); - - // Update angle display with new ratio - double currentAngleDegrees = angle.getEncoder().getPosition() * (360.0 / 18.75); // Convert from rotations - angleDisplay.setDouble(currentAngleDegrees); - - // Handle synchronized motor control - if (bothInButton.getBoolean(false)) { - up.set(0.3); // Up motor forward - down.set(0.15); // Down motor reverse - } else if (bothOutButton.getBoolean(false)) { - double upRPM = Math.abs(up.getEncoder().getVelocity()); - double downRPM = Math.abs(down.getEncoder().getVelocity()); - - if (startupCounter < 10) { // Startup delay - up.set(-0.4); - down.set(-0.4); - startupCounter++; - } else if (upRPM < 50 || downRPM < 50) { - if (stallCounter < 50) { // Wait ~0.5 seconds (25 * 20ms) before stopping - stallCounter++; - up.set(-0.4); - down.set(-0.4); - } else { - up.set(0); - down.set(0); - bothOutButton.setBoolean(false); - startupCounter = 0; - stallCounter = 0; - } - } else { - stallCounter = 0; // Reset stall counter if RPM is good - up.set(-0.4); - down.set(-0.4); - } - } else { - startupCounter = 0; // Reset both counters when button released - stallCounter = 0; + /** + * Checks if the grabber is at the target angle (◕ᴗ◕✿) + * @return true if at target angle, false otherwise + */ + public boolean isAtTargetAngle() { + double currentAngle = angle.getEncoder().getPosition(); + return Math.abs(currentAngle - pidController.getSetpoint()) < 0.1; } - // Check RPM and update status - double upRPM = Math.abs(up.getEncoder().getVelocity()); - double downRPM = Math.abs(down.getEncoder().getVelocity()); - - SmartDashboard.putNumber("rpm", up.getEncoder().getVelocity()); - // ... (rest of your control logic) - } + @Override + public void periodic() { + double currentAngle = angle.getEncoder().getPosition(); + double kG = kGTuner.getDouble(DEFAULT_KG); - // Copy your configuration methods - private void configureNEO550(SparkMax motor) { - SparkMaxConfig neo550Config = new SparkMaxConfig(); - - neo550Config - .smartCurrentLimit(40) // Protect our smol motors! >w< - .idleMode(IdleMode.kCoast) // Better control! - .voltageCompensation(12.0) // Stable performance! UwU - .openLoopRampRate(0.1); // Smooth acceleration! - - // Apply our configuration with proper timeout - motor.setCANTimeout(250); - motor.configure(neo550Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + // Use PID to maintain target angle + angle.set(pidController.calculate(currentAngle) + kG); + + // Update displays + double currentAngleDegrees = angle.getEncoder().getPosition() * (360.0 / 18.75); + angleDisplay.setDouble(currentAngleDegrees); } - private void configureNEO(SparkMax motor) { + private void configureNEO(SparkMax motor, boolean inverted, boolean softlimit) { SparkMaxConfig neoConfig = new SparkMaxConfig(); - + // Create soft limit config SoftLimitConfig softLimitConfig = new SoftLimitConfig(); softLimitConfig .forwardSoftLimit(2.76) // +53 degrees with 18.75:1 - .forwardSoftLimitEnabled(true) + .forwardSoftLimitEnabled(softlimit) .reverseSoftLimit(0) // Keep starting point - .reverseSoftLimitEnabled(true); + .reverseSoftLimitEnabled(softlimit); neoConfig .smartCurrentLimit(40) @@ -246,8 +112,9 @@ private void configureNEO(SparkMax motor) { .voltageCompensation(12.0) .openLoopRampRate(0.1) .apply(softLimitConfig) - .inverted(false); // Flips motor direction - + .inverted(inverted); // Flips motor direction + + motor.setCANTimeout(250); motor.configure(neoConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..5658a32 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,162 @@ +// 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 frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.spark.*; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.wpilibj.shuffleboard.*; +import edu.wpi.first.networktables.GenericEntry; +import java.util.Map; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Intake extends SubsystemBase { + private final SparkMax up; + private final SparkMax down; + private int startupCounter = 0; + private int stallCounter = 0; + + private static Intake mInstance = null; + + public static Intake getInstance() { + if (mInstance == null) { + mInstance = new Intake(); + } + return mInstance; + } + + // Shuffleboard entries + private final ShuffleboardTab intakeTab = Shuffleboard.getTab("Intake"); + private final GenericEntry upButton = intakeTab.add("Up Motor", false) + .withWidget("Toggle Button") + .withPosition(0, 0) + .getEntry(); + private final GenericEntry downButton = intakeTab.add("Down Motor", false) + .withWidget("Toggle Button") + .withPosition(0, 1) + .getEntry(); + private final GenericEntry upSpeed = intakeTab.add("Up Motor Speed", 0.5) + .withWidget("Number Slider") + .withProperties(Map.of("min", -1.0, "max", 1.0)) + .withPosition(1, 0) + .getEntry(); + private final GenericEntry downSpeed = intakeTab.add("Down Motor Speed", 0.5) + .withWidget("Number Slider") + .withProperties(Map.of("min", -1.0, "max", 1.0)) + .withPosition(1, 1) + .getEntry(); + private final GenericEntry bothInButton = intakeTab.add("Both Motors In", false) + .withWidget("Toggle Button") + .withPosition(0, 5) + .getEntry(); + private final GenericEntry bothOutButton = intakeTab.add("Both Motors Out", false) + .withWidget("Toggle Button") + .withPosition(1, 5) + .getEntry(); + private final GenericEntry upRPMStatus = intakeTab.add("Up Motor RPM OK", true) + .withWidget("Boolean Box") + .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) + .withPosition(0, 6) + .getEntry(); + private final GenericEntry downRPMStatus = intakeTab.add("Down Motor RPM OK", true) + .withWidget("Boolean Box") + .withProperties(Map.of("colorWhenTrue", "Lime", "colorWhenFalse", "Red")) + .withPosition(1, 6) + .getEntry(); + + public Intake() { + up = new SparkMax(2, MotorType.kBrushless); + down = new SparkMax(3, MotorType.kBrushless); + + configureNEO550(up); + configureNEO550(down); + } + + + + private void configureNEO550(SparkMax motor) { + SparkMaxConfig neo550Config = new SparkMaxConfig(); + neo550Config + .smartCurrentLimit(40) + .idleMode(IdleMode.kCoast) + .voltageCompensation(12.0) + .openLoopRampRate(0.1); + + motor.setCANTimeout(250); + motor.configure(neo550Config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void periodic() { + + + // Manual control through Shuffleboard + boolean upButtonState = upButton.getBoolean(false); + boolean downButtonState = downButton.getBoolean(false); + double upSpeedValue = upSpeed.getDouble(0.5); + double downSpeedValue = downSpeed.getDouble(0.5); + + if (upButtonState) up.set(upSpeedValue); + if (downButtonState) down.set(downSpeedValue); + + // Handle synchronized movement + if (bothOutButton.getBoolean(false)) { + up.set(0.3); + down.set(0.15); + } else if (bothInButton.getBoolean(false)) { + intake(); + } + + // Update RPM status + updateRPMStatus(); + } + + public void setSpeed(double speed) { + up.set(speed); + down.set(speed); + } + + public void intake() { + double upRPM = Math.abs(up.getEncoder().getVelocity()); + double downRPM = Math.abs(down.getEncoder().getVelocity()); + + if (startupCounter < 10) { + up.set(-0.4); + down.set(-0.4); + startupCounter++; + } else if (upRPM < 50 || downRPM < 50) { + if (stallCounter < 50) { + stallCounter++; + up.set(-0.4); + down.set(-0.4); + } else { + up.set(0); + down.set(0); + bothOutButton.setBoolean(false); + startupCounter = 0; + stallCounter = 0; + } + } else { + stallCounter = 0; + up.set(-0.4); + down.set(-0.4); + } + } + + private void updateRPMStatus() { + double upRPM = Math.abs(up.getEncoder().getVelocity()); + double downRPM = Math.abs(down.getEncoder().getVelocity()); + + upRPMStatus.setBoolean(upRPM >= 50); + downRPMStatus.setBoolean(downRPM >= 50); + + SmartDashboard.putNumber("Up Motor RPM", upRPM); + SmartDashboard.putNumber("Down Motor RPM", downRPM); + } +} diff --git a/src/main/java/frc/robot/subsystems/SuperStruct.java b/src/main/java/frc/robot/subsystems/SuperStruct.java new file mode 100644 index 0000000..f6d1ed9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SuperStruct.java @@ -0,0 +1,229 @@ +// 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 frc.robot.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.states.StateMachine; +import frc.robot.states.SuperStructState; +import edu.wpi.first.wpilibj.XboxController; + +public class SuperStruct extends SubsystemBase { + Elevator mElevator; + Grabber mGrabber; + Intake mIntake; + AlgaeIntake mAlgaeIntake; + StateMachine mStateMachine; + SuperStructState mCommandedState; + private final XboxController controller; + private static final int CONTROLLER_PORT = 0; // Adjust port as needed! uwu + + private static SuperStruct mInstance = null; + + public static SuperStruct getInstance() { + if (mInstance == null) { + mInstance = new SuperStruct(); + } + return mInstance; + } + /** Creates a new StateMachine. */ + public SuperStruct() { + mElevator = Elevator.getInstance(); + mGrabber = Grabber.getInstance(); + mIntake = Intake.getInstance(); + mAlgaeIntake = AlgaeIntake.getInstance(); + mStateMachine = StateMachine.getInstance(); + mCommandedState = SuperStructState.DEFAULT; + controller = new XboxController(CONTROLLER_PORT); + } + + public void L1() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void L2() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void L3() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void L4() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + } + + public void TRAVEL() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.setSpeed(0.01); + } + + public void CS() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.intake(); + } + + public void PLACEMENT() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.setSpeed(0.3); + } + + public void DEFAULT() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mIntake.setSpeed(0); + mAlgaeIntake.setIntakeSpeed(0); + mAlgaeIntake.setAngle(0); + } + + public void ALGAE_STOWAGE() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mAlgaeIntake.setAngle(0); + } + + public void ALGAE_INTAKE() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mAlgaeIntake.intake(); + } + + public void ALGAE_PLACEMENT() { + mElevator.setPosition(0); + mGrabber.setAngle(0); + mAlgaeIntake.setAngle(0); + } + + public void updateState() { + switch (mCommandedState) { + case L1: + L1(); + break; + case L2: + L2(); + break; + case L3: + L3(); + break; + case L4: + L4(); + break; + case TRAVEL: + TRAVEL(); + break; + case CS: + CS(); + break; + case PLACEMENT: + PLACEMENT(); + break; + case DEFAULT: + DEFAULT(); + break; + case ALGAE_STOWAGE: + ALGAE_STOWAGE(); + break; + case ALGAE_INTAKE: + ALGAE_INTAKE(); + break; + case ALGAE_PLACEMENT: + ALGAE_PLACEMENT(); + break; + } + } + + public void setState(SuperStructState state) { + mStateMachine.setCommandedState(state); + } + + /** + * Handle Xbox controller inputs for state control (✿◠‿◠) + */ + public void handleControllerInput() { + // A button - L1 + if (controller.getAButton()) { + setState(SuperStructState.L1); + } + // B button - L2 + else if (controller.getBButton()) { + setState(SuperStructState.L2); + } + // Y button - L3 + else if (controller.getYButton()) { + setState(SuperStructState.L3); + } + // X button - L4 + else if (controller.getXButton()) { + setState(SuperStructState.L4); + } + // Right bumper - TRAVEL + else if (controller.getRightBumperButton()) { + setState(SuperStructState.TRAVEL); + } + // Left bumper - CS (Charging Station) + else if (controller.getLeftBumperButton()) { + setState(SuperStructState.CS); + } + // Start button - PLACEMENT + else if (controller.getStartButton()) { + setState(SuperStructState.PLACEMENT); + } + // Back button - DEFAULT + else if (controller.getBackButton()) { + setState(SuperStructState.DEFAULT); + } + // POV Up - ALGAE_STOWAGE + else if (controller.getPOV() == 0) { + setState(SuperStructState.ALGAE_STOWAGE); + } + // POV Right - ALGAE_INTAKE + else if (controller.getPOV() == 90) { + setState(SuperStructState.ALGAE_INTAKE); + } + // POV Down - ALGAE_PLACEMENT + else if (controller.getPOV() == 180) { + setState(SuperStructState.ALGAE_PLACEMENT); + } + } + + @Override + public void periodic() { + // Handle controller input first + handleControllerInput(); + + // This method will be called once per scheduler run + mCommandedState = mStateMachine.getCommandedState(); + updateState(); + + SmartDashboard.putString("Commanded State", mCommandedState.toString()); + // Add controller info to dashboard + SmartDashboard.putString("Last Button Pressed", getLastButtonPressed()); + } + + /** + * Helper to get last button pressed for dashboard uwu + */ + private String getLastButtonPressed() { + if (controller.getAButton()) return "A - L1"; + if (controller.getBButton()) return "B - L2"; + if (controller.getYButton()) return "Y - L3"; + if (controller.getXButton()) return "X - L4"; + if (controller.getRightBumperButton()) return "RB - TRAVEL"; + if (controller.getLeftBumperButton()) return "LB - CS"; + if (controller.getStartButton()) return "Start - PLACEMENT"; + if (controller.getBackButton()) return "Back - DEFAULT"; + if (controller.getPOV() == 0) return "POV Up - ALGAE_STOWAGE"; + if (controller.getPOV() == 90) return "POV Right - ALGAE_INTAKE"; + if (controller.getPOV() == 180) return "POV Down - ALGAE_PLACEMENT"; + return "None"; + } +}