Shooter

simgui-ds.json

@@ -4,14 +4,9 @@
       "visible": false
     }
   },
-  "Joysticks": {
+  "Keyboard 1 Settings": {
     "window": {
-      "visible": false
-    }
-  },
-  "System Joysticks": {
-    "window": {
-      "visible": false
+      "visible": true
     }
   },
   "keyboardJoysticks": [
@@ -107,7 +102,9 @@
     }
   ],
   "robotJoysticks": [
-    {},
+    {
+      "guid": "Keyboard1"
+    },
     {
       "guid": "Keyboard0"
     }

simgui.json

@@ -30,6 +30,9 @@
     },
     "windows": {
       "/Robot/drive/field2d": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
         "module- FL": {
           "style": "Line"
         },
@@ -42,6 +45,9 @@
         "module-RL": {
           "style": "Line"
         },
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
         "window": {
           "visible": true
         }
@@ -80,12 +86,13 @@
                 "open": true
               },
               "open": true
-            },
-            "open": true
-          },
-          "open": true
+            }
+          }
         },
-        "open": true
+        "open": true,
+        "shooter": {
+          "open": true
+        }
       },
       "Shuffleboard": {
         "open": true
@@ -94,5 +101,40 @@
   },
   "NetworkTables Info": {
     "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 347,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/Robot/shooter/bottomVelocity"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/Robot/shooter/bottomTarget"
+            }
+          ]
+        }
+      ]
+    }
   }
 }

src/main/java/org/sciborgs1155/robot/Ports.java

@@ -19,7 +19,7 @@ public static final class Drive {
   }
 
   public static final class Wheels {
-    public static final int TOP_MOTOR = -1;
-    public static final int BOTTOM_MOTOR = -1;
+    public static final int TOP_WHEEL = 18;
+    public static final int BOTTOM_WHEEL = 19;
   }
 }

src/main/java/org/sciborgs1155/robot/Robot.java

@@ -22,6 +22,7 @@
 import org.sciborgs1155.robot.commands.Autos;
 import org.sciborgs1155.robot.drive.Drive;
 import org.sciborgs1155.robot.drive.DriveConstants;
+import org.sciborgs1155.robot.shooter.Shooter;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -37,6 +38,7 @@ public class Robot extends CommandRobot implements Logged {
 
   // SUBSYSTEMS
   private final Drive drive = Drive.create();
+  private final Shooter shooter = Shooter.create();
 
   // COMMANDS
   @Log.NT private final Autos autos = new Autos();
@@ -103,6 +105,8 @@ private void configureBindings() {
     autonomous().whileTrue(new ProxyCommand(autos::get));
     FaultLogger.onFailing(f -> Commands.print(f.toString()));
 
+    operator.a().toggleOnTrue(shooter.shoot());
+
     driver
         .leftBumper()
         .or(driver.rightBumper())

src/main/java/org/sciborgs1155/robot/drive/Drive.java

@@ -4,7 +4,6 @@
 import static org.sciborgs1155.robot.Ports.Drive.*;
 import static org.sciborgs1155.robot.drive.DriveConstants.*;
 
-import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -13,7 +12,6 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -23,7 +21,6 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import java.util.List;
-import java.util.function.Supplier;
 import monologue.Annotations.IgnoreLogged;
 import monologue.Annotations.Log;
 import monologue.Logged;
@@ -153,34 +150,6 @@ public Command drive(InputStream vx, InputStream vy, InputStream vOmega) {
     return run(() -> driveFieldRelative(new ChassisSpeeds(vx.get(), vy.get(), vOmega.get())));
   }
 
-  /**
-   * Drives the robot based on a {@link InputStream} for field relative x y and omega velocities.
-   *
-   * @param vx A supplier for the velocity of the robot along the x axis (perpendicular to the
-   *     alliance side).
-   * @param vy A supplier for the velocity of the robot along the y axis (parallel to the alliance
-   *     side).
-   * @param heading A supplier for the field relative heading of the robot.
-   * @return The driving command.
-   */
-  public Command drive(InputStream vx, InputStream vy, Supplier<Rotation2d> heading) {
-    var pid =
-        new ProfiledPIDController(
-            Rotation.P,
-            Rotation.I,
-            Rotation.D,
-            new TrapezoidProfile.Constraints(MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL));
-
-    return run(
-        () ->
-            driveFieldRelative(
-                new ChassisSpeeds(
-                    vx.get(),
-                    vy.get(),
-                    pid.calculate(
-                        getPose().getRotation().getRadians(), heading.get().getRadians()))));
-  }
-
   /**
    * Drives the robot relative to field based on provided {@link ChassisSpeeds} and current heading.
    *

src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java

@@ -2,7 +2,6 @@
 
 import static edu.wpi.first.units.Units.*;
 
-import com.pathplanner.lib.path.PathConstraints;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.Angle;
@@ -54,13 +53,6 @@ public static final class Rotation {
     public static final double D = 0.0;
   }
 
-  public static final PathConstraints CONSTRAINTS =
-      new PathConstraints(
-          MAX_SPEED.in(MetersPerSecond),
-          MAX_ACCEL.in(MetersPerSecondPerSecond),
-          MAX_ANGULAR_SPEED.in(RadiansPerSecond),
-          MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second)));
-
   public static final class ModuleConstants {
     public static final double COUPLING_RATIO = 0;
 

src/main/java/org/sciborgs1155/robot/shooter/NoWheel.java

@@ -1,11 +1,13 @@
 package org.sciborgs1155.robot.shooter;
 
+/** NoWheel */
 public class NoWheel implements WheelIO {
+
   @Override
-  public void setVoltage(double voltage) {}
+  public void setVoltage(double volts) {}
 
   @Override
-  public double getSpeed() {
+  public double getVelocity() {
     return 0;
   }
 }

src/main/java/org/sciborgs1155/robot/shooter/RealWheel.java

@@ -1,7 +1,10 @@
 package org.sciborgs1155.robot.shooter;
 
 import static org.sciborgs1155.robot.Ports.Wheels.*;
+import static org.sciborgs1155.robot.shooter.ShooterConstants.POSITION_FACTOR;
+import static org.sciborgs1155.robot.shooter.ShooterConstants.VELOCITY_FACTOR;
 
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
@@ -10,18 +13,24 @@ public class RealWheel implements WheelIO {
   private final CANSparkMax motor;
   private final RelativeEncoder encoder;
 
-  public RealWheel(int id, boolean inverted) {
-    motor = new CANSparkMax(id, MotorType.kBrushless);
+  public RealWheel(boolean inverted, int motorID) {
+    motor = new CANSparkMax(motorID, MotorType.kBrushless);
+    motor.setIdleMode(IdleMode.kBrake);
     motor.setInverted(inverted);
+    motor.setSmartCurrentLimit(30);
+    motor.burnFlash();
     encoder = motor.getEncoder();
+    encoder.setPositionConversionFactor(POSITION_FACTOR);
+    encoder.setVelocityConversionFactor(VELOCITY_FACTOR);
   }
 
   @Override
-  public void setVoltage(double voltage) {
-    motor.setVoltage(voltage);
+  public double getVelocity() {
+    return encoder.getVelocity();
   }
 
-  public double getSpeed() {
-    return encoder.getVelocity();
+  @Override
+  public void setVoltage(double voltage) {
+    motor.setVoltage(voltage);
   }
 }