Shooter

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 52c3f123c681d9be58a74929494d95135fbcde21..659840443eda629aa71516c6507c9eda3d7c8e99 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -24,6 +24,7 @@ import frc.robot.controls.Operator;
 import frc.robot.controls.PS5ControllerDriverConfig;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
+import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.util.PathGroupLoader;
 import frc.robot.util.Vision.DetectedObject;
@@ -43,6 +44,7 @@ public class RobotContainer {
   private Drivetrain drive = null;
   private Vision vision = null;
   private Turret turret = null;
+  private Shooter shooter = null;
   private Command auto = new DoNothing();
 
   // Controllers are defined here
@@ -56,6 +58,7 @@ public class RobotContainer {
    */
   public RobotContainer(RobotId robotId) {
     turret = new Turret();
+    shooter = new Shooter();
     // dispatch on the robot
     switch (robotId) {
       case TestBed1:

diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
index 6b99d880d9215b5551999926eca7a56082188b69..0a519e01327227625e41732fb9da68dbdc4ba904 100644 (file)
--- a/src/main/java/frc/robot/constants/Constants.java
+++ b/src/main/java/frc/robot/constants/Constants.java
@@ -12,6 +12,7 @@ public class Constants {
 
     // CAN bus names
     public static final String CANIVORE_CAN = "CANivore";
+    public static final String SUBSYSTEM_CANIVORE_CAN = "CANivoreSub";
     public static final String RIO_CAN = "rio";
 
     // Logging 

diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java
index 9ae589e3091a4ecc80958db774d08ccdb83b4cec..7971bdd6ffd5a109b20acdf6370fb19f94841ba3 100644 (file)
--- a/src/main/java/frc/robot/constants/IdConstants.java
+++ b/src/main/java/frc/robot/constants/IdConstants.java
@@ -21,4 +21,10 @@ public class IdConstants {
 
     // Motor ID
     public static final int TURRET_MOTOR_ID = 20;
+
+    // Shooter
+    public static final int SHOOTER_LEFT_ID = 6;
+    public static final int SHOOTER_RIGHT_ID = 4;
+    public static final int FEEDER_ID = 3;
+    public static final int SHOOTER_SENSOR_ID = 1;
 }

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
new file mode 100644 (file)
index 0000000..e69563e
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -0,0 +1,102 @@
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface.Measurement;
+import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
+import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest;
+import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class Shooter  {
+    
+    private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+    private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+
+    private TalonFX feederMotor = new TalonFX(IdConstants.FEEDER_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
+
+    //rotations/sec
+
+    // Goal Velocity / Double theCircumfrence
+    private double shooterTargetSpeed = 0;
+    private double feederPower = 0;
+    //Velocity in rotations per second
+    VelocityVoltage voltageRequest = new VelocityVoltage(0);
+
+    public Shooter(){
+        TalonFXConfiguration config = new TalonFXConfiguration();
+        config.Slot0.kP = 0.1; //tune p value
+        config.Slot0.kI = 0;
+        config.Slot0.kD = 0;
+        config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps
+        shooterMotorLeft.getConfigurator().apply(config);
+        shooterMotorRight.getConfigurator().apply(config);
+        
+        shooterMotorLeft.getConfigurator().apply(
+            new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+            .withNeutralMode(NeutralModeValue.Coast)
+        );
+
+        shooterMotorRight.getConfigurator().apply(
+            new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
+        );
+
+        feederMotor.getConfigurator().apply(
+            new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+            .withNeutralMode(NeutralModeValue.Coast)
+        );
+
+        SmartDashboard.putData("Turn on shooter", new InstantCommand(() -> setShooter(ShooterConstants.SHOOTER_VELOCITY)));
+        SmartDashboard.putData("Turn on feeder", new InstantCommand(() -> setFeeder(ShooterConstants.FEEDER_RUN_POWER)));
+        SmartDashboard.putData("Turn ALL off", new InstantCommand(() -> deactivateShooterAndFeeder()));
+        SmartDashboard.putData("Turn off Shooter", new InstantCommand(() -> setShooter(0)));
+        SmartDashboard.putData("Turn off feeder", new InstantCommand(() -> setFeeder(0)));
+    }
+
+    public void periodic(){
+        shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
+        shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
+        feederMotor.set(feederPower);
+    }
+
+    public void deactivateShooterAndFeeder() {
+        setFeeder(0);
+        setShooter(0);
+        System.out.println("Shooter deactivated");
+    }
+    public void setFeeder(double power){
+        System.out.println("VELOCITY: " + getShooterVelcoity()); 
+        feederPower = power;
+    }
+
+    public void setShooter(double linearVelocityMps) {
+        double wheelCircumference = Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER;
+        shooterTargetSpeed = linearVelocityMps * ShooterConstants.SHOOTER_GEAR_RATIO / wheelCircumference; // rps
+    }
+
+    public double getFeederVelocity() {
+        return feederMotor.getVelocity().getValueAsDouble();
+    }
+    public double getShooterVelcoity() {
+        return shooterMotorLeft.getVelocity().getValueAsDouble();
+    }
+}

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
new file mode 100644 (file)
index 0000000..c5fef88
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java
@@ -0,0 +1,18 @@
+package frc.robot.subsystems.shooter;
+
+public class ShooterConstants {
+    //TODO: find these values
+    public static final double FEEDER_RUN_POWER = 0.3; // meters per second??
+    public static final double SHOOTER_VELOCITY = 10.0; // meters per second
+    public static final double SHOOTER_GEAR_RATIO = 36.0 / 24.0; // gear ratio from motors to shooter wheel
+    // public static final double SHOOTER_LAUNCH_DIAMETER = 0.0762; // meters (3 inches)
+    public static final double SHOOTER_LAUNCH_DIAMETER = 0.1016; // meters (4 inches) I think this is right.
+    public static final double SHOOTER_RUN_POWER = 0.3;
+    public static final double SENSOR_DISTANNCE_THRESHOLD = 0.150; // meters
+    public static final double SENSOR_AMBIENCE_THRESHOLD = 0.100; // meters
+
+    // in m/s
+    public static final double EXIT_VELOCITY_TOLERANCE = 1.0;
+}
+// 8 velcocity is too little
+// 16 is too much
\ No newline at end of file