sysID template from examples ported over

.DataLogTool/datalogtool.json

@@ -0,0 +1 @@
+{}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.3.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.2"
 }
 
 dependencies {

simgui.json

@@ -47,15 +47,6 @@
           "open": true
         },
         "open": true
-      },
-      "Shuffleboard": {
-        ".metadata": {
-          "open": true
-        },
-        ".recording": {
-          "open": true
-        },
-        "open": true
       }
     }
   },

src/main/java/frc/robot/Constants.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.math.util.Units;
 
 public final class Constants {
+
   public static final class DriveConstants {
 
     // PWM ports/CAN IDs for motor controllers
@@ -69,10 +70,9 @@ public static final class DriveConstants {
 
     public static final LinearSystem<N2, N2, N2> kDrivetrainPlant = LinearSystemId.identifyDrivetrainSystem(kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter,kvVoltSecondsPerRadian, kaVoltSecondsSquaredPerRadian);    
 
-    public static final int[] kLeftLeadEncoderPorts = new int[] {4, 5};
-    public static final int[] kRightLeadEncoderPorts = new int[] {6, 7};
-    public static final int[] kLeftFollowEncoderPorts = new int[] {8, 9};
-    public static final int[] kRightFollowEncoderPorts = new int[] {10, 11};
+    public static final int[] kLeftLeadEncoderPorts = new int[] {2,3};
+    public static final int[] kRightLeadEncoderPorts = new int [] {4,5};
+
 
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
@@ -85,7 +85,37 @@ public static final class DriveConstants {
 
   }
 
+
+  // public static final class ShooterConstants {
+
+  //   public static double kIntakeLauncherSpeed = -3.0;
+  //   public static double kIntakeFeederSpeed = -3.0;
+  //   public static int launchWheelPort = 6;
+  //   public static int intakeWheelPort = 7;
+  //   public static double AmpLaunchWheelSpeed = 3.0;
+  //   public static double AmpFeedWheelSpeed = 3.0;
+  //   public static double SpeakerLaunchWheelSpeed = 6.0;
+  //   public static double SpeakerFeedWheelSpeed = 6.0;
+
+
+  // }
+
+  // Intake Constants
+  public static final class IntakeConstants {
+
+    // Motor Speeds
+    public static double IntakeMotorReleaseSpeed = -0.01;
+    public static double IntakeMotorReceiveSpeed = 0.01;
+    public static double ArmMotorMoveForwardSpeed = 0.01;
+    public static double ArmMotorMoveBackwardSpeed = -0.01;
 
+    // Motor ID Ports
+    public static int IntakeMotorPort = 9;
+    public static int ArmMotorPort = 8;
+
+  }
+
+  // OI constants
   public static final class OIConstants {    
 
   }
@@ -141,6 +171,8 @@ public static double findTargetHeight(int targetID) {
     public static final double GOAL_RANGE_METERS = Units.feetToMeters(3);
     public static double CAMERA_PITCH_RADIANS = Units.degreesToRadians(3);
 
+    public static final int kAngleSetpoint = 0;
+
   }
 
   public static class FieldConstants {
@@ -162,8 +194,8 @@ public static final class ShooterConstants {
     // in reverse
     public static final double kSpeakerLauncherSpeed = -1;
     public static final double kSpeakerLaunchFeederSpeed = -1;
-    public static final double kSpeakerAmpLaunchSpeed = -0.5;
-    public static final double kSpeakerAmpLaunchFeederSpeed = -0.5;
+    public static final double kSpeakerAmpLaunchSpeed = -0.3;
+    public static final double kSpeakerAmpLaunchFeederSpeed = -0.3;
     public static final double kIntakeLauncherSpeed = 1;
     public static final double kIntakeFeederSpeed = .2;
 
@@ -172,5 +204,4 @@ public static final class ShooterConstants {
 
   }
 
-
 }

src/main/java/frc/robot/Robot.java

@@ -4,9 +4,12 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import org.littletonrobotics.urcl.URCL;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -28,6 +31,12 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+    //Log stuff hopefully
+
+    DataLogManager.start();
+    URCL.start();
+
   }
 
   /**
@@ -44,6 +53,8 @@ public void robotPeriodic() {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+    SmartDashboard.putData(CommandScheduler.getInstance());
+
 
   }
 
@@ -62,7 +73,7 @@ public void autonomousInit() {
   /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {
-    // m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();

src/main/java/frc/robot/RobotContainer.java

@@ -1,61 +1,144 @@
+// 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;
 
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.commands.LaunchAmp;
-import frc.robot.commands.LaunchSpeaker;
-import frc.robot.commands.PrepareLaunchAmp;
-import frc.robot.commands.PrepareLaunchSpeaker;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.VisionSubsystem;
-import frc.robot.Constants.DriveConstants;
+import frc.robot.Constants.OIConstants;
 import frc.robot.Constants.ShooterConstants;
-import frc.robot.commands.TurnToAngle;
-import frc.robot.commands.TurnToAngleProfiled;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.DriveSubsystem;
+  import frc.robot.subsystems.IntakeSubsystem;
+  import frc.robot.subsystems.ShooterSubsystem;
+  import frc.robot.Constants.DriveConstants;
+  import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-public class RobotContainer {
-  private final DriveSubsystem driveSubsystem = new DriveSubsystem();
-  private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
-  private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
-  private final VisionSubsystem visionSubsystem = new VisionSubsystem();
+/**
+* This class is where the bulk of the robot should be declared. Since Command-based is a
+* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+* subsystems, commands, and button mappings) should be declared here.
+**/
 
-  private final CommandXboxController driveController = new CommandXboxController(DriveConstants.driveControllerPort);
-  private final CommandXboxController operatorController = new CommandXboxController(DriveConstants.operatorControllerPort);
+/**
+  * Use this to define the command that runs during autonomous.
+  *
+  * <p>Scheduled during {@link Robot#autonomousInit()}.
+  */
+import frc.robot.commands.*;
 
-  public RobotContainer() {
-    configureButtonBindings();
-    driveSubsystem.setDefaultCommand(new RunCommand(() -> driveSubsystem.driveArcade(-driveController.getLeftY(), -driveController.getRightX()), driveSubsystem));
+  public class RobotContainer {
+    private final DriveSubsystem driveSubsystem = new DriveSubsystem();
+    private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+    private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
 
-  }
-
-  private void configureButtonBindings() {
-    /*Create an inline sequence to run when the operator presses and holds the A (green) button. Run the PrepareLaunch
-     * command for 1 seconds and then run the LaunchNote command */
-    operatorController.a().whileTrue(new PrepareLaunchSpeaker(shooterSubsystem).withTimeout(ShooterConstants.kLauncherDelay).andThen(new LaunchSpeaker(shooterSubsystem)).handleInterrupt(() -> shooterSubsystem.stop()));
-    operatorController.b().whileTrue(new PrepareLaunchAmp(shooterSubsystem).withTimeout(ShooterConstants.kLauncherDelay).andThen(new LaunchAmp(shooterSubsystem)).handleInterrupt(() -> shooterSubsystem.stop()));
-
-    // Set up a binding to run the intake command while the operator is pressing and holding the left Bumper
-    operatorController.leftBumper().whileTrue(shooterSubsystem.getIntakeCommand());
-
-    //New commands from this branch specifically, idk why they were removed
-    driveController.x().whileTrue(shooterSubsystem.getIntakeCommand());
-    driveController.y().whileTrue(shooterSubsystem.getLaunchCommand());
+    private final CommandXboxController driveController = new CommandXboxController(DriveConstants.driveControllerPort);
+    private final CommandXboxController operatorController = new CommandXboxController(DriveConstants.operatorControllerPort);
+
+    private final PowerDistribution pdh = new PowerDistribution(1, ModuleType.kRev);
 
-    driveController.rightBumper()
-        .whileTrue(new InstantCommand(() -> driveSubsystem.setMaxOutput(0.1)))
-        .whileFalse(new InstantCommand(() -> driveSubsystem.setMaxOutput(1.0)));
+      private final SendableChooser<Command> m_autoChooser = new SendableChooser<Command>();
 
 
-  }
-
-  // public Command getAutonomousCommand() {
-  //   return Autos.exampleAuto(driveSubsystem, driveController);
-  //   }
-  }
-
-
+    public RobotContainer() {
+      pdh.setSwitchableChannel(true);
+      configureButtonBindings();
+      driveSubsystem.setDefaultCommand(new RunCommand(() -> driveSubsystem.driveArcade(-driveController.getLeftY(), -driveController.getRightX()), driveSubsystem));
+
+    }
+
+    private void configureButtonBindings() {
+      /*Create an inline sequence to run when the operator presses and holds the A (green) button. Run the PrepareLaunch
+      * command for 1 seconds and then run the LaunchNote command */
+      driveController.a().whileTrue(new LaunchSpeaker(shooterSubsystem));
+      driveController.b().whileTrue(new LaunchAmp(shooterSubsystem));
+
+      // Set up a binding to run the intake command while the operator is pressing and holding the left Bumper
+      driveController.leftBumper().whileTrue(new IntakeSource(shooterSubsystem));
+
+      //New commands from this branch specifically, idk why they were removed
+
+      driveController.rightBumper()
+      .whileTrue(new InstantCommand(() -> driveSubsystem.setMaxOutput(0.1)))
+      .whileFalse(new InstantCommand(() -> driveSubsystem.setMaxOutput(1.0)));
+
+
+      // Arm Ground Intake Button Bindings
+      operatorController.leftBumper().whileTrue(new ArmForward(intakeSubsystem).handleInterrupt(() -> intakeSubsystem.stop()));
+      operatorController.rightBumper().whileTrue(new ArmBackward(intakeSubsystem).handleInterrupt(() -> intakeSubsystem.stop()));
+
+      // Intake Ground Intake Button Bindings
+      operatorController.x().whileTrue(new IntakeReceiver(intakeSubsystem).handleInterrupt(() -> intakeSubsystem.stop()));
+      operatorController.y().whileTrue(new IntakeReleaser(intakeSubsystem).withTimeout(1).handleInterrupt(() -> intakeSubsystem.stop()));
+
+      // FOR SYSID
+      //     m_driverController
+      //     .a().whileTrue(new PrepareLaunchSpeaker(m_shooter).withTimeout(ShooterConstants.kLauncherDelay).andThen(new LaunchSpeaker(m_shooter)).handleInterrupt(() -> m_shooter.stop()));
+
+      // m_driverController
+      //     .b().whileTrue(new PrepareLaunchAmp(m_shooter).withTimeout(ShooterConstants.kLauncherDelay).andThen(new LaunchAmp(m_shooter)).handleInterrupt(() -> m_shooter.stop()));
+
+      // m_driverController.leftBumper().whileTrue(m_shooter.getIntakeCommand());
+
+      // m_driverController.rightBumper()
+      //     .whileTrue(new InstantCommand(() -> m_drive.setMaxOutput(0.1)))
+      //     .whileFalse(new InstantCommand(() -> m_drive.setMaxOutput(1.0)));
+
+      // m_driverController
+      //     .a()
+      //     .whileTrue(new WaitCommand(0.1)
+      //     .andThen(new TurnToAngle(90, m_drive).withTimeout(1)));
+
+      // // Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout
+      // m_driverController
+      //     .b()
+      //     .whileTrue(new WaitCommand(0.1)
+      //     .andThen(new TurnToAngleProfiled(-90, m_drive).withTimeout(1)));
+
+
+    }
+
+    public void initializeAutoChooser() {
+
+      m_autoChooser.setDefaultOption("Drive forward: ", 
+      new WaitCommand(0.1)
+      .andThen(new DriveForwardCmd(driveSubsystem, 10, 0.5))
+      .withTimeout(3)
+      .andThen(new RunCommand(() -> driveSubsystem.driveArcade(0,0), driveSubsystem)));
+
+      m_autoChooser.addOption("SysID Quasistatic Foward(backward): ", 
+      driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+
+      m_autoChooser.addOption("SysID Quasistatic Backward(real forward): ", 
+      driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+
+      m_autoChooser.addOption("SysID Dynamic Foward(backward): ", 
+      driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+
+      m_autoChooser.addOption("SysID Dynamic Backward(real forward): ", 
+      driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
+
+
+    }
+
+
+
+    public Command getAutonomousCommand() {
+      return m_autoChooser.getSelected();
+    }
+    }
+

src/main/java/frc/robot/commands/ArmBackward.java

@@ -0,0 +1,37 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.subsystems.IntakeSubsystem;
+// IntakeReceiver is for the brush motors to receive the note from the ground.
+/*Basicly the same code from the shootersubsystem with different names.
+ *
+*/
+
+public class ArmBackward extends Command {
+    IntakeSubsystem intakeSubsystem;
+    public ArmBackward(IntakeSubsystem intakeSubsystem) {
+        this.intakeSubsystem = intakeSubsystem;
+        addRequirements(intakeSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        intakeSubsystem.setArmMotor(IntakeConstants.ArmMotorMoveBackwardSpeed);
+    }
+
+    @Override 
+    public void execute() {
+
+    }
+
+    @Override 
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override 
+    public void end(boolean interrupted) {
+        intakeSubsystem.stop();
+    }
+}