Yao shooter2

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

@@ -4,16 +4,93 @@
 
 package frc.robot;
 
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
  * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
- */
+ **/
 public final class Constants {
-  public static class OperatorConstants {
+
+    //Controllers
     public static final int kDriverControllerPort = 0;
-  }
+    public static final int kFunctionsControllerPort = 1;
+
+
+    //Shooter Motors 
+    public static final int KFlywheelMotor_1 = 15;
+    public static final int KFlywheelMotor_2 = 16;
+    public static final int KBackRollerMotor = 10;
+
+
+    //Intake
+    public static final int KIntakeMotor = 11;
+    public static final int kPivotMotor = 12;
+
+
+    //Transfer 
+    public static final int kTransferMotor_1 = 13;
+    public static final int kTransferMotor_2 = 14;
+
+
+    //Drive Motors
+    public static final int kFrontLeftDrive = 5;
+    public static final int kFrontRightDrive = 3;
+    public static final int kBackLeftDrive  = 9;
+    public static final int kBackRightDrive = 7;
+
+    //Steering Motors
+    public static final int kFrontLeftSteering = 4;
+    public static final int kFrontRightSteering = 2;
+    public static final int kBackLeftSteering  = 8;
+    public static final int kBackRightSteering = 6;
+
+    //Drivetrain Encoders
+    public static final int kFrontLeftEncoder = 1;
+    public static final int kFrontRightEncoder = 0;
+    public static final int kBackLeftEncoder = 2;
+    public static final int kBackRightEncoder = 3;
+
+    //Encoder Offset 
+    public static final double kFrontLeftEncoderOffset = 0.15;
+    public static final double kFrontRightEncoderOffset = -0.21;
+    public static final double kBackLeftEncoderOffset = 0.25;
+    public static final double kBackRightEncoderOffset = 0.07;
+
+    //Dampeners 
+    public static final double kSwerveDampner = 0.05;
+    public static final double kElevatorDampner = 0.5;
+    public static final double kClimbDampner = 0.5;
+    public static final double kAlgaeDampner = 0.2;
+
+
+    //Chasis Length
+    public static final double chasisWidth = Units.inchesToMeters(20);
+    public static final double chasisLength = Units.inchesToMeters(24);
+
+
+    //Camera
+    public static final AprilTagFields kField = AprilTagFields.kDefaultField;
+    public static final String kCameraName = "NAME";
+    public static final Transform3d kRobotToCam = new Transform3d(
+        Units.inchesToMeters(10), 0, Units.inchesToMeters(10), 
+        new Rotation3d(0   , 0, 0)
+    ); 
+
+
+    //Standard Deviations
+    public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+    public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
+
 }

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

@@ -54,12 +54,15 @@ public void disabledPeriodic() {}
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      CommandScheduler.getInstance().schedule(m_autonomousCommand);
-    }
+
+    // m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // // schedule the autonomous command (example)
+    // if (m_autonomousCommand != null) {
+    //   CommandScheduler.getInstance().schedule(m_autonomousCommand);
+    // }
+
   }
 
   /** This function is called periodically during autonomous. */

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

@@ -4,11 +4,21 @@
 
 package frc.robot;
 
-import frc.robot.Constants.OperatorConstants;
+import frc.robot.Constants;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ExampleCommand;
 import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.SwerveDrivetrainSubsystem;
+import frc.robot.subsystems.TransferSubsystem;
+import frc.robot.subsystems.VisionSubsystem;
 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.button.CommandJoystick;
+import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
+import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
@@ -19,17 +29,40 @@
  * subsystems, commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
+
+
   // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+  public final SwerveDrivetrainSubsystem swerveDrivetrainSubsystem = new SwerveDrivetrainSubsystem();
+  private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+  private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+  private final TransferSubsystem transferSubsystem = new TransferSubsystem();
+  private final VisionSubsystem visionSubsystem = new VisionSubsystem(Constants.kCameraName, Constants.kField, Constants.kRobotToCam, swerveDrivetrainSubsystem::addVisionMeasurements);
+
+  private final RunCommand shoot = new RunCommand(()-> this.shooterSubsystem.rev(), shooterSubsystem);
+  private final RunCommand intake = new RunCommand(()-> this.intakeSubsystem.intake(), intakeSubsystem);
+  private final RunCommand outake = new RunCommand(()-> this.intakeSubsystem.outake(), intakeSubsystem);
+  private final RunCommand transfer = new RunCommand(()-> this.transferSubsystem.transfer(), transferSubsystem);
+  private final InstantCommand stop = new InstantCommand(()-> this.shooterSubsystem.stop(), shooterSubsystem);
+  private final InstantCommand stopIntake = new InstantCommand(()-> this.intakeSubsystem.stopIntake(), intakeSubsystem);
+
+
+
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController m_driverController =
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+  public static final CommandJoystick m_driverController =
+      new CommandJoystick(Constants.kDriverControllerPort);
+
+
+
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
+
+    visionSubsystem.periodic();
+
+
   }
 
   /**
@@ -42,22 +75,31 @@ public RobotContainer() {
    * joysticks}.
    */
   private void configureBindings() {
+    m_driverController.button(6).onTrue(shoot);
+    m_driverController.button(2).onTrue(stop);
+    m_driverController.button(4).onTrue(outake);
+    m_driverController.button(5).onTrue(intake);
+    m_driverController.button(1).onTrue(stopIntake);
+
     // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
+    // new Trigger(m_exampleSubsystem::exampleCondition)
+    //     .onTrue(new ExampleCommand(m_exampleSubsystem));
+
+    // // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
+    // // cancelling on release.
+    // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
 
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
   }
 
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
    */
-  public Command getAutonomousCommand() {
-    // An example command will be run in autonomous
-    return Autos.exampleAuto(m_exampleSubsystem);
-  }
+
+
+  // public Command getAutonomousCommand() {
+  //   // An example command will be run in autonomous
+  //   // return Autos.exampleAuto(m_exampleSubsystem);
+  // }
 }

2026_Rebuilt/src/main/java/frc/robot/Swerve/SwerveModule.java

@@ -0,0 +1,151 @@
+package frc.robot.Swerve;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import frc.robot.Constants;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkFlex;
+
+
+public class SwerveModule {
+
+
+    //Motors
+    public SparkMax steeringMotor;
+    public SparkFlex driveMotor;
+
+    //Modules States
+    public SwerveModuleState moduleState;
+    public SwerveModulePosition modulePosition;
+
+    //Encoder
+    public AnalogEncoder encoder;
+    public RelativeEncoder driveEncoder;
+    public double encoderValue;
+    public double encoderOffset;
+
+    //PID
+    public PIDController pidController; 
+    public double distanceMoved = 0;
+    public double endpoint;
+    public double pidSpeed;
+    public double errorValue;
+    public Rotation2d Angle = Rotation2d.fromDegrees(0);
+
+    //Modifiers
+    private int steeringModifier = 1;
+    private int driveModifier = 1;
+
+
+
+
+
+    public SwerveModule(int driveMotorPort,int steeringMotorPort, int encoderPort, double offset, boolean steeringModified, boolean driveModified)
+    {
+
+        //Motors
+        steeringMotor = new SparkMax(steeringMotorPort, MotorType.kBrushless);
+        driveMotor = new SparkFlex(driveMotorPort, MotorType.kBrushless);
+
+        //Module State
+        moduleState = new SwerveModuleState();
+        modulePosition = new SwerveModulePosition(0,Rotation2d.fromDegrees(0));
+
+
+        //Encoder
+        encoder = new AnalogEncoder(encoderPort);
+        driveEncoder = driveMotor.getEncoder();
+
+
+        //PID 
+        //original 3
+        pidController = new PIDController(3, 0, 0);
+        pidController.enableContinuousInput(0, 1);
+        pidController.setTolerance(0.001);
+
+        //Offset
+        encoderOffset = offset;
+
+        //Invert
+        if (steeringModified == true){
+
+            steeringModifier = -1;
+        }
+
+        if (driveModified == true){
+
+            driveModifier = -1;
+        }
+
+
+
+
+
+    };
+
+
+
+    /** 
+    public SwerveModuleState getModuleState(){
+        return moduleState;
+    }
+    public SwerveModulePosition getModulePosition(){
+       return modulePosition = new SwerveModulePosition(distanceMoved, Angle);
+    }
+    */
+
+    public void setModuleState(SwerveModuleState state){
+
+
+        moduleState = state;
+
+
+        //Offset Calculations
+        encoderValue = (this.encoder.get()+encoderOffset);
+        if(encoderValue > 1){
+            encoderValue -= 1;
+        }
+        if (encoderValue < 0 ){
+            encoderValue += 1;
+        }
+
+
+        moduleState.optimize(Rotation2d.fromRadians((encoderValue/1)*(Math.PI*2)));
+
+
+        endpoint = (moduleState.angle.getRadians()/(Math.PI*2));
+        pidSpeed = pidController.calculate(encoderValue, endpoint);    
+        errorValue = 1 - Math.abs(pidController.getError());
+
+        if (errorValue <= pidController.getErrorTolerance()){
+            pidSpeed = 0;
+        }
+
+        //Set Steering Speed and Driving Speed
+        steeringMotor.set(steeringModifier*pidSpeed);
+        driveMotor.set(driveModifier*moduleState.speedMetersPerSecond);
+
+
+
+    }
+
+    public void setModulePosition(){
+
+        distanceMoved = driveEncoder.getPosition()*(Units.inchesToMeters(4)*Math.PI)/(6.75);
+
+        Angle = Rotation2d.fromRadians((encoder.get()/1)*(Math.PI*2));
+    }
+
+    public SwerveModuleState getState() {
+        return this.getState();
+    }
+
+}
+