Merge remote-tracking branch 'origin/main' into vision

.github/workflows/gradle.yml

@@ -0,0 +1,35 @@
+# This workflow uses actions that are not certified by GitHub.
+# They are provided by a third-party and are governed by
+# separate terms of service, privacy policy, and support
+# documentation.
+# This workflow will build a Java project with Gradle and cache/restore any dependencies to improve the workflow execution time
+# For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-java-with-gradle
+
+name: Java CI with Gradle
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ '*' ]
+
+permissions:
+  contents: read
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v3
+    - name: Set up JDK 17
+      uses: actions/setup-java@v3
+      with:
+        java-version: '17'
+        distribution: 'temurin'
+    - name: Build with Gradle
+      uses: gradle/gradle-build-action@bd5760595778326ba7f1441bcf7e88b49de61a25 # v2.6.0
+      with:
+        arguments: build
+

README.md

@@ -0,0 +1,2 @@
+# Artemis - 2024
+The repository for Artemis, Team 2976 The Spartabots' robot for the 2024 FIRST Robotics Competition.

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

@@ -8,6 +8,7 @@
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.CANSparkBase.IdleMode;
 
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -16,6 +17,7 @@
 import frc.lib.util.COTSFalconSwerveConstants;
 import frc.lib.util.SwerveModuleConstants;
 
+
 /**
  * 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
@@ -49,6 +51,7 @@ public static final class HardwarePorts {
     public static final int climbLeaderMotor = 3;
     public static final int climbFollowerMotor = 4;
     public static final int pivotMotor = 30;
+    public static final int pivotCANcoderID = 31;
 
   }
 
@@ -77,6 +80,7 @@ public final class Vision {
   public static final IdleMode intakeNeutralMode = IdleMode.kCoast;
   public static final IdleMode shooterNeutralMode = IdleMode.kBrake;
 
+  public static final double noteIntakeDistance = 70.0;
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
   }
@@ -88,9 +92,11 @@ public static final class SwerveConstants {
     public static final COTSFalconSwerveConstants chosenModule = COTSFalconSwerveConstants
     .SDSMK4i(COTSFalconSwerveConstants.driveGearRatios.SDSMK4_L2);
 
+    //general constants
+
     /* Drivetrain Constants */
-    public static final double trackWidth = 0.5715;
-    public static final double wheelBase = 0.5715;
+    public static final double trackWidth = 0.47625;
+    public static final double wheelBase = 0.47625;
     public static final double wheelCircumference = chosenModule.wheelCircumference;
     /*
     * Swerve Kinematics
@@ -211,5 +217,4 @@ public static final class Mod3 {
                 canCoderID, angleOffset);
     }
   }
-
 }

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

@@ -18,12 +18,6 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 
-/**
- * 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 trigger mappings) should be declared here.
- */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final Swerve s_Swerve = Swerve.getInstance();
@@ -101,15 +95,7 @@ public RobotContainer() {
     configureBindings();
   }
 
-  /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
-   * predicate, or via the named factories in {@link
-   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
-   * joysticks}.
-   */
+  /* Map commands to buttons */
   private void configureBindings() {
     // driver controls
     driverBack.onTrue(new InstantCommand(() -> s_Swerve.resetOdometry(new Pose2d())));

src/main/java/frc/robot/SwerveModule.java

@@ -93,7 +93,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){
     }
     /**
      * Sets the rotation of the angle motor
-     * @param desiredState Desired set state  for PID Controller
+     * @param desiredState Desired set state for PID Controller
      */
     private void setAngle(SwerveModuleState desiredState){
         Rotation2d angle = (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.01)) ? lastAngle : desiredState.angle; //Prevent rotating module if speed is less then 1%. Prevents Jittering.

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

@@ -0,0 +1,46 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pivot;
+import frc.robot.subsystems.Pivot.PivotState;;
+
+public class SetPivot extends Command {
+    Pivot s_Pivot;
+    PivotState state;
+    ProfiledPIDController pivotController = new ProfiledPIDController(0.06, 1e-2, 1e-3, new TrapezoidProfile.Constraints(500000, 3000*1e5));
+
+    public SetPivot(PivotState state) {
+        s_Pivot = Pivot.getInstance();
+        this.state = state;
+        addRequirements(s_Pivot);
+    }
+
+    @Override
+    public void initialize() {
+        s_Pivot.setState(state);
+        pivotController.reset(s_Pivot.getCANcoderPosition());
+    }
+
+    @Override
+    public void execute() {
+        double voltage = pivotController.calculate(s_Pivot.getCANcoderPosition(), s_Pivot.getSetPoint());
+        if (Math.abs(s_Pivot.getCANcoderPosition() - s_Pivot.getSetPoint()) < 15) {
+			voltage = 0.7;
+		}
+        s_Pivot.setVoltage(voltage);
+    }
+
+    @Override
+	public boolean isFinished() {
+		return Math.abs(s_Pivot.getSetPoint() - s_Pivot.getCANcoderPosition()) < 45;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		s_Pivot.setVoltage(0);
+		SmartDashboard.putString("eleEnd", "elevator end");
+	}
+}

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

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeShooter;
+
+public class SmartIntake extends Command{
+
+    private final IntakeShooter s_IntakeShooter;
+
+    public SmartIntake(){
+        s_IntakeShooter = IntakeShooter.getInstance();
+        addRequirements(s_IntakeShooter);
+    }
+
+    @Override
+    public void initialize() {
+        s_IntakeShooter.setIntake(0.7);
+    }
+
+    @Override
+    public void execute() {
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        s_IntakeShooter.setIntake(0);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return s_IntakeShooter.hasNote();
+    }
+
+
+
+}

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

@@ -0,0 +1,16 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeShooter;
+import frc.robot.subsystems.Swerve;
+
+public class TeleopCommandFactory extends Command{
+    private static Command selectedTeleopCommand;
+
+    private static IntakeShooter s_IntakeShooter = IntakeShooter.getInstance();
+    private static Swerve s_Swerve = Swerve.getInstance();
+
+    public TeleopCommandFactory(){
+
+    }
+}

src/main/java/frc/robot/subsystems/IntakeShooter.java

@@ -2,9 +2,14 @@
 
 import javax.sound.midi.MidiEvent;
 
+import com.ctre.phoenix6.signals.ControlModeValue;
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.ColorMatch;
+import com.revrobotics.ColorSensorV3;
 
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
@@ -16,6 +21,12 @@ public class IntakeShooter extends SubsystemBase {
     private CANSparkFlex mLeaderShooter;
     private CANSparkFlex mFollowerShooter;
 
+
+    private static final I2C.Port onboardI2C = I2C.Port.kOnboard;
+    private static ColorSensorV3 m_intakeSensor;
+
+    private static final ColorMatch m_colorMatcherIntake = new ColorMatch();
+
     public static IntakeShooter getInstance(){
         if(mIntakeShooter == null){
             mIntakeShooter = new IntakeShooter();
@@ -32,13 +43,41 @@ public IntakeShooter(){
 
         mFollowerShooter = new CANSparkFlex(Constants.HardwarePorts.shooterFollowerMotor, MotorType.kBrushless);
         configFollowerMotor();
+
+        m_intakeSensor = new ColorSensorV3(onboardI2C);
+        m_colorMatcherIntake.addColorMatch(new Color(255, 165, 0)); //rgb values for orange, could be inaccurate. YBVS
     }
 
+    /**
+     * Sets the percentage output of the intake and shooter motors
+     * @param val a value from [0, 1]
+     */
     public void setPercentage(double val){
         mLeaderShooter.set(val);
         mIntakeMotor.set(val);
     }
 
+    /**
+     * Sets the output of the intake motor
+     * @param val either a percentage from [0, 1] or desired voltage
+     * @param voltageControl is voltage control if true
+     */
+    public void setIntake(double val, boolean voltageControl){
+        if(voltageControl){
+            mIntakeMotor.setVoltage(val);
+        } else {
+            mIntakeMotor.set(val);
+        }
+    }
+
+    /**
+     * Equivalent to setIntake(val, false)
+     * @param val is the desired percentage output
+     */
+    public void setIntake(double val){
+        setIntake(val, false);
+    }
+
     public void configIntakeMotor(){
         mIntakeMotor.setSmartCurrentLimit(Constants.intakePeakCurrentLimit);
         mIntakeMotor.setInverted(false);
@@ -70,5 +109,13 @@ public void configFollowerMotor(){
         // mLeaderShooter.getPIDController().setI(Constants.SwerveConstants.driveKI);
         // mLeaderShooter.getPIDController().setD(Constants.SwerveConstants.driveKD);
     }
+
+    /**
+     * 
+     * @return whether the intake color sensor's proximity value is greater than a set threshold
+     */
+    public boolean hasNote(){
+        return m_intakeSensor.getProximity() >= Constants.noteIntakeDistance;
+    }
 
 }