Shooter

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

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import java.util.function.DoubleFunction;
+import java.util.function.DoubleSupplier;
 import java.util.stream.Collector.Characteristics;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -32,6 +34,14 @@ public final class Constants {
   public static final double CLIMBER_ENCODER_DISTANCE = 12; 
   public static final double SHOOTER_LIMELIGHT_ANGLE = 0.5; 
   public static final int FALCON_MAX_RPM = 6380;
+  public static final int BASE_SHOOTER_RPM = 3000;
+  public static final DoubleFunction<Double> BALL_VELOCITY_CONVERSION_TO_MOTOR_SPEED = d -> d * 0.5;
+  // Degrees
+  public static final double LIMELIGHT_ANGLE = 77;
+  // Meters
+  public static final double LIMELIGHT_HEIGHT = 0.6858;
+  public static final double HUB_HEIGHT = 2.64;
+
   public static final int SHOOTER_RPM = 3000;
 
   public static final class PID {
@@ -48,7 +58,7 @@ public static final class PID {
     public static final double P_SWERVE_POWER = 0.00015;
     public static final double I_SWERVE_POWER = 0.00;
     public static final double D_SWERVE_POWER = 0.01;
-
+    
     // Linear drive feed forward
     public static final SimpleMotorFeedforward DRIVE_FF = IS_REAL ?
         new SimpleMotorFeedforward( // real
@@ -84,7 +94,14 @@ public static final class PID {
     public static final double SHOOTER_P = 0.1; 
     public static final double SHOOTER_I = 1e-4; 
     public static final double SHOOTER_D = 1; 
+  }
 
+	public static final class RobotDimensions {
+    // Distance between wheels
+    public static final double WIDTH = Units.inchesToMeters(28); //inches
+    public static final double LENGTH = Units.inchesToMeters(28); //inches
+
+    public static final double WHEEL_CIRCUMFRENCE = Units.inchesToMeters(3.5) * Math.PI; 
   }
 
 	public static final class RobotDimensions {
@@ -106,6 +123,10 @@ public static final class Input {
     public static final ConstantButton CLIMB_BUTTON = new ConstantButton(1, 1); 
     public static final ConstantButton INTAKE_BUTTON = new ConstantButton(1, 5); 
     public static final ConstantButton STORAGE_TOGGLE = new ConstantButton(1, 0);
+    public static final ConstantButton SHOOTER_SPOOL = new ConstantButton(1, 9);
+    public static final ConstantButton SHOOTER_JUICED_SPOOL = new ConstantButton(1, 10);
+    public static final ConstantButton SHOOTER_SHOOT = new ConstantButton(1, 11);
+    public static final ConstantButton SHOOTER_JUICED_SHOOT = new ConstantButton(1, 12);
   }
 
   public static final class MotorSettings {
@@ -114,7 +135,6 @@ public static final class MotorSettings {
   }
 
   public static final class Motor {
-
     //Swerve
     public static final int SWERVE_FRONT_LEFT_POWER = 4;
     public static final int SWERVE_FRONT_LEFT_STEER = 15;
@@ -142,14 +162,29 @@ public static final class Motor {
     public static final double SWERVE_ROTATION_MAX_ACCELERATION = Math.PI / 4; // rad/s^2
     public static final double SWERVE_NOMINAL_OUTPUT_PERCENT = 0.05;
     public static final double SWERVE_NOMINAL_OUTPUT_STEER = 0.000;
+    public static final int SWERVE_BACK_RIGHT_POWER = 6;
+    public static final int SWERVE_BACK_RIGHT_STEER = 7;
+
+    public static final SwerveDriveKinematics SWERVE_DRIVE_KINEMATICS = new SwerveDriveKinematics(
+      new Translation2d(RobotDimensions.LENGTH / 2, -RobotDimensions.WIDTH / 2),
+      new Translation2d(RobotDimensions.LENGTH / 2, RobotDimensions.WIDTH / 2),
+      new Translation2d(-RobotDimensions.LENGTH / 2, -RobotDimensions.WIDTH / 2),
+      new Translation2d(-RobotDimensions.LENGTH, RobotDimensions.WIDTH / 2));
 
+    public static final double SWERVE_POWER_GEAR_RATIO = 6.55;
+
+    public static final double SWERVE_MAX_SPEED = 5.18; // m/s
+    public static final double SWERVE_MAX_ACCELERATION = 3; // m/s^2
+    public static final double SWERVE_ROTATION_MAX_SPEED = 2 * 2 * Math.PI; // rad/s
+    public static final double SWERVE_ROTATION_MAX_ACCELERATION = Math.PI / 4; // rad/s^2
+    public static final double SWERVE_NOMINAL_OUTPUT_PERCENT = 0.05;
     public static final double SWERVE_DEADBAND = 0.05;
 
     public static final TrapezoidProfile.Constraints THETA_CONTROL_CONSTRAINTS = new TrapezoidProfile.Constraints(SWERVE_ROTATION_MAX_SPEED, SWERVE_ROTATION_MAX_ACCELERATION);
 
     //Storage
     public static final int STORAGE_MOVEMENT_BELT = 8;
-      
+
     public static final int SHOOTER_PORT = 2;
     public static final int EXAMPLE_INTAKE_CHANNEL = 3;
 

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.ClimbCommand;
@@ -17,8 +18,11 @@
 import frc.robot.commands.auto.AutoPaths;
 import frc.robot.subsystems.Climb;
 import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Limelight;
+import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Storage;
 import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.Storage.ToggleState;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -28,20 +32,55 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-
-  // Subsystems
-  // private Climb climb = new Climb();
-  // private Intake intake = new Intake();
-  // private Storage storage = new Storage();
+  
+  //Subsystems
+  private Climb climb = new Climb(); 
+  private Intake intake = new Intake();
+  private Storage storage = new Storage();
   private Swerve swerve = new Swerve();
-  private GenericHID joy = new GenericHID(0);
+  private Shooter shooter = new Shooter();
+  private Limelight limelight = new Limelight();
+
+  //Commands
+  private IndexBall indexBall = new IndexBall(intake, storage);
+  private ClimbCommand climbCommand = new ClimbCommand(climb);
+  private SwerveDrive swerveDrive;
+
+  private Command shootCommand = new CommandBase() {
+    {
+      addRequirements(shooter, limelight);
+    }
 
-  // Commands
-  // private IndexBall indexBall = new IndexBall(intake, storage);
-  // private ClimbCommand climbCommand = new ClimbCommand(climb);
+    public void execute() {
+      double requiredVelocity = Constants.BALL_VELOCITY_CONVERSION_TO_MOTOR_SPEED.apply(limelight.getVelocity());
+      if (shooter.shooterMotorRPM() >= requiredVelocity) {
+        storage.toggleBelt(ToggleState.ON);
+      }
+      shooter.spool(requiredVelocity);
+    };
+  };
+
+  private Command spoolCommand = new CommandBase() {
+    {
+      addRequirements(shooter);
+    }
+
+    public void execute() {
+      if (!shootCommand.isScheduled()) {
+        shooter.spool(Constants.BASE_SHOOTER_RPM);
+      }
+    }
+
+    public void end(boolean interrupted) {
+      shooter.spool(0);
+    }
+  };
+
+  private GenericHID joy = new GenericHID(0);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    swerveDrive = new SwerveDrive(swerve, Constants.Input.X_AXIS.get(), Constants.Input.Y_AXIS.get(), Constants.Input.ROTATION.get());
     // Configure the button bindings
     configureButtonBindings();
   }
@@ -53,6 +92,10 @@ public RobotContainer() {
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    Constants.Input.CLIMB_BUTTON.get().whenPressed(climbCommand); 
+    Constants.Input.INTAKE_BUTTON.get().whileHeld(indexBall);
+    Constants.Input.SHOOTER_SPOOL.get().whileHeld(spoolCommand);
+    Constants.Input.SHOOTER_SHOOT.get().whileHeld(shootCommand);
     swerve.setDefaultCommand(new SwerveDrive(swerve, () -> joy.getRawAxis(0),
         () -> joy.getRawAxis(1), () -> joy.getRawAxis(4)));
     JoystickButton aButton = new JoystickButton(joy, 1);

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

@@ -2,6 +2,9 @@
 
 import java.util.function.Supplier;
 import org.opencv.core.Mat;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -15,36 +18,36 @@ public class AimCommand extends CommandBase {
 	private Swerve swerve;
 
 	public AimCommand(Limelight limelight, Swerve swerve) {
-		this.limelight = limelight; 
+		this.limelight = limelight;
 		this.swerve = swerve;
 		addRequirements(limelight, swerve);
 	}
 
-	 // Called when the command is first scheduled.
-	 @Override
-	 public void initialize() {
-		 
-	 }
- 
-	 // Called at 50hz while the command is scheduled.
-	 @Override
+	// Called when the command is first scheduled.
+	@Override
+	public void initialize() {
+
+	}
+
+	// Called at 50hz while the command is scheduled.
+	@Override
 	 public void execute() {
 		 double tx = limelight.getHorizontalAngle();
 		 double ty = limelight.getVerticalAngle();
 
-		 /*while (Math.abs(tx) > 0.5 || Math.abs(ty) > 0.5) {
-			 swerve.drive(tx * Constants.AIM_SCALING_FACTOR_X, Constants.AIM_SCALING_FACTOR_Y, 0);
-		 }*/
-	 }
-
-	 // Called once when the command ends or is interrupted.
-	 @Override
-	 public void end(boolean interrupted) {
-		 //swerve.drive(0, 0, 0);
-	 }
-
-	 @Override
-	 public boolean isFinished() {
-		 return false;
+		 if (Math.abs(tx) > 0.5 || Math.abs(ty) > 0.5) {
+       swerve.setModuleStates(Constants.Motor.SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, Math.PI / 2D, swerve.getRotation2d())));
+		 }
 	 }
+
+	// Called once when the command ends or is interrupted.
+	@Override
+	public void end(boolean interrupted) {
+		// swerve.drive(0, 0, 0);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
 }

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

@@ -30,7 +30,7 @@ public void execute() {
 		final double verticalAngleAbs = Math.abs(limelight.getHorizontalAngle());
 		// Checks if aimed
 		if (horizontalAngleAbs < Constants.SHOOTER_LIMELIGHT_ANGLE && verticalAngleAbs < Constants.SHOOTER_LIMELIGHT_ANGLE) {
-			shooter.spool(Constants.SHOOTER_RPM);
+			shooter.spool(Constants.BASE_SHOOTER_RPM);
 		} else {
 			shooter.stopMotor();
 		}

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

@@ -52,6 +52,7 @@ public void execute() {
     //construct chassis
     ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
       xSpeed, ySpeed, rotationSpeed, swerve.getRotation2d());
+
     SmartDashboard.putString("CHASSIS", chassisSpeeds.toString());
     //convert to states from the chassis  
     SwerveModuleState[] moduleState = Constants.Motor.SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
@@ -60,7 +61,7 @@ public void execute() {
     for (SwerveModuleState s : moduleState) {
       SmartDashboard.putString("SWERVESTATEPRE-" + i++, s.toString());
     }
-
+    
     swerve.setModuleStates(moduleState);
   }
 

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

@@ -3,11 +3,16 @@
 import com.ctre.phoenix.Util;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.TalonFXSimCollection;
+import com.ctre.phoenix.motorcontrol.TalonSRXSimCollection;
 import com.ctre.phoenix.motorcontrol.can.TalonFX;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
 import frc.robot.util.Utils;
@@ -42,7 +47,6 @@ public SwerveModule(TalonFX powerController, TalonSRX steerController, int offSe
         powerController.config_kP(0, Constants.PID.P_SWERVE_POWER);
         powerController.config_kI(0, Constants.PID.I_SWERVE_POWER);
         powerController.config_kD(0, Constants.PID.D_SWERVE_POWER);
-
         steerController.config_kF(0, 0);
         steerController.config_kP(0, Constants.PID.P_SWERVE_STEER);
         steerController.config_kI(0, Constants.PID.I_SWERVE_STEER);
@@ -97,5 +101,4 @@ public void periodic() {
         // Called at 50hz.
         SmartDashboard.putNumber(steerController.getDeviceID() + "-Steer Postition", Utils.ticksToDegrees(steerController.getSelectedSensorPosition(), 4096));
     }
-
 }

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

@@ -3,9 +3,10 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import static java.lang.Math.*;
+import static frc.robot.util.Utils.*;
 
 public class Limelight extends SubsystemBase {
 
@@ -15,15 +16,11 @@ public class Limelight extends SubsystemBase {
     private NetworkTableEntry verticalAngleOffSet;
     private NetworkTableEntry ta;
 
-
-
     public Limelight() {
 
         networkTable = NetworkTableInstance.getDefault().getTable("Limelight");
-
         // Horizontal Offset From Crosshair To Target (-27 degrees to 27 degrees)
         horizontalAngleOffSet = networkTable.getEntry("tx");
-
         // Vertical Offset From Crosshair To Target (-20.5 degrees to 20.5 degrees)
         verticalAngleOffSet = networkTable.getEntry("ty"); 
         // Target Area (0% of image to 100% of image
@@ -52,4 +49,32 @@ public boolean connected() {
     public double getTargetArea() {
         return ta.getNumber(0).doubleValue();
     }
+
+    // returns distance in meters
+    public double getDistance() {
+        final double a2 = getVerticalAngle();
+        return (Constants.HUB_HEIGHT - Constants.LIMELIGHT_HEIGHT) / Math.tan(Constants.LIMELIGHT_ANGLE + a2)        ;
+    }
+
+    public double getVelocity() {
+        final double xpos = getDistance();
+        final double ypos = Constants.LIMELIGHT_HEIGHT;
+        final double launchAngle = Math.toRadians(Constants.LIMELIGHT_ANGLE);
+        final double hub = Constants.HUB_HEIGHT;
+        /*
+            The stuff below is very interesting, it was calculated by using a System of Equations.
+            We know the target xpos, the target ypos, the launch angle, and the hub.
+            We need to find the launch speed and the time.
+            We are given 2 equations to do this, the x value finder and the y value finder.
+            We can plug in filler variables into the projectile motion equations and solve for launch speed.
+            Then, we can substitute the launch speed in one equation with the other equation launch speed.
+            We can use this new equation to solve for time.
+            Once we have solved for time, we can plug it into either the x value finder or the y value finder.
+            This gives us the target launch speed in meters/second.
+        */
+        final double time = sqrt((xpos * tan(launchAngle) - 2.64 + ypos) / hub);
+        final double launchSpeed = (xpos * sec(launchAngle)) / time;
+
+        return launchSpeed;
+    }
 }