added state machine

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<String> 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

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;
+    }
+
+
+}

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;
+    }
+} 

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;
+    }
+}

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
+} 

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());
+  }
+}