did SwerveDriveWhileShooting

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "FMS": {
+    "window": {
+      "visible": false
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [

simgui.json

@@ -1,16 +1,18 @@
 {
+  "HALProvider": {
+    "Other Devices": {
+      "window": {
+        "visible": false
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Autonomous": "String Chooser",
       "/SmartDashboard/Field": "Field2d"
     },
     "windows": {
-      "/SmartDashboard/Autonomous": {
-        "window": {
-          "visible": true
-        }
-      },
       "/SmartDashboard/Field": {
         "Back Left-2d": {
           "arrowSize": 25,
@@ -75,26 +77,92 @@
           "weight": 2.0,
           "width": 0.6600000262260437
         },
-        "bottom": 544,
-        "builtin": "2024 Crescendo",
-        "height": 8.013679504394531,
-        "left": 46,
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
         "path": {
           "arrows": false,
           "selectable": false,
           "style": "Line",
           "weight": 2.0
         },
-        "right": 1088,
-        "top": 36,
+        "right": 2961,
+        "top": 79,
         "width": 16.541748046875,
         "window": {
           "visible": true
         }
       }
     }
   },
+  "NetworkTables": {
+    "retained": {
+      "SmartDashboard": {
+        "open": true
+      }
+    },
+    "transitory": {
+      "SmartDashboard": {
+        "Swerve": {
+          "Modules": {
+            "open": true
+          },
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
   "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "Server": {
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
     "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "axis": [
+            {
+              "autoFit": true
+            }
+          ],
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 383,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/Swerve/adjusted (subtracted) angle"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/Swerve/unadjusted angle"
+            }
+          ]
+        }
+      ]
+    }
   }
 }

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -13,6 +13,7 @@
 import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading;
 import com.stuypulse.robot.commands.swerve.SwerveDriveToPose;
 import com.stuypulse.robot.commands.swerve.SwerveDriveToScore;
+import com.stuypulse.robot.commands.swerve.SwerveDriveWhileShooting;
 import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming;
 import com.stuypulse.robot.constants.Field;
 import com.stuypulse.robot.constants.Ports;
@@ -29,6 +30,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -72,7 +74,8 @@ private void configureNamedCommands() {
     /****************/
 
     private void configureDefaultCommands() {
-        swerve.setDefaultCommand(new SwerveDriveDrive(driver));
+        swerve.setDefaultCommand(new SwerveDriveWhileShooting(new Pose2d(Field.WIDTH / 2, Field.HEIGHT / 2, new Rotation2d()), driver));
+        //swerve.setDefaultCommand(new SwerveDriveWithAiming(new Pose2d(Field.WIDTH / 2, Field.HEIGHT / 2, new Rotation2d()), driver));
     }
 
     /***************/

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWhileShooting.java

@@ -0,0 +1,116 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Alignment;
+import com.stuypulse.robot.constants.Settings.Driver.Drive;
+import com.stuypulse.robot.subsystems.odometry.AbstractOdometry;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.control.angle.AngleController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.Angle;
+import com.stuypulse.stuylib.math.Vector2D;
+import com.stuypulse.stuylib.streams.vectors.VStream;
+import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
+import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
+import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveWhileShooting extends Command {
+
+    private final SwerveDrive swerve;
+    private final AbstractOdometry odometry;
+
+    private VStream speed;
+    private final AngleController controller;
+    private final Pose2d target;
+
+    private VStream robotSpeed;
+
+    public SwerveDriveWhileShooting(Pose2d target, Gamepad driver) {
+        this.swerve = SwerveDrive.getInstance();
+        this.odometry = AbstractOdometry.getInstance();
+
+        this.speed = VStream.create(driver::getLeftStick)
+                .filtered(
+                    new VDeadZone(Drive.DEADBAND),
+                    x -> x.clamp(1.0),
+                    x -> Settings.vpow(x, Drive.POWER.get()),
+                    x -> x.mul(Drive.MAX_TELEOP_SPEED.get()),
+                    new VRateLimit(Drive.MAX_TELEOP_ACCEL),
+                    new VLowPassFilter(Drive.RC)
+                );
+
+        this.controller = new AnglePIDController(Alignment.Rotation.P, Alignment.Rotation.I, Alignment.Rotation.D);
+        this.target = target;
+
+        this.robotSpeed = VStream.create(() -> chassisSpeedsVector())
+            .filtered(new VLowPassFilter(Settings.Swerve.VELOCITY_RC.get()))
+            .filtered(new VRateLimit(3));
+
+        odometry.getField().getObject("Target").setPose(target);
+
+        addRequirements(swerve);
+    }
+
+    private Vector2D chassisSpeedsVector() {
+        ChassisSpeeds speeds = swerve.getChassisSpeeds();
+        return new Vector2D(
+            speeds.vxMetersPerSecond,
+            speeds.vyMetersPerSecond
+        );
+    }
+
+    public Vector2D getVelocity(){
+        return robotSpeed.get();
+    }
+
+    public Vector2D getFieldRelativeSpeed(){
+        return robotSpeed.get().rotate(Angle.fromRotation2d(odometry.getPose().getRotation()));
+    }
+
+    @Override
+    public void execute() {
+
+        //suspicion: getVelocity is wrong
+        Vector2D velNoteInitial = new Vector2D(new Translation2d(Settings.Swerve.NOTE_VELOCITY, odometry.getPose().getRotation())); 
+
+
+
+
+        Pose2d robotPose = odometry.getPose();
+        Vector2D targetVector = new Vector2D(
+                            this.target.getX() - robotPose.getX(), 
+                            this.target.getY() - robotPose.getY());
+
+        Vector2D velocityDifference = targetVector.sub(getFieldRelativeSpeed()); 
+
+        double angularVel = -controller.update(
+                            velocityDifference.getAngle(),
+                            Angle.fromRotation2d(odometry.getRotation()));
+
+
+        swerve.drive(speed.get(), angularVel);
+
+        SmartDashboard.putNumber("Swerve/unadjusted angle", targetVector.getAngle().toDegrees());
+        SmartDashboard.putNumber("Swerve/targetVector x", targetVector.x);
+        SmartDashboard.putNumber("Swerve/targetVector y", targetVector.y);
+
+        SmartDashboard.putNumber("Swerve/adjusted (subtracted) angle", velocityDifference.getAngle().toDegrees());
+        SmartDashboard.putNumber("Swerve/angle setpoint (from odometry)", odometry.getRotation().getDegrees());
+        SmartDashboard.putNumber("Swerve/getFieldRelativeSpeed x", getFieldRelativeSpeed().x);
+        SmartDashboard.putNumber("Swerve/getFieldRelativeSpeed y", getFieldRelativeSpeed().y);
+
+        //optimal velocity = target vector.sub(getFieldRelativeSpeedPoints)
+
+
+
+    }
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveWithAiming.java

@@ -9,13 +9,16 @@
 import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.math.Angle;
+import com.stuypulse.stuylib.math.Vector2D;
 import com.stuypulse.stuylib.streams.vectors.VStream;
 import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
 import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
 import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class SwerveDriveWithAiming extends Command {
@@ -25,6 +28,7 @@ public class SwerveDriveWithAiming extends Command {
     private VStream speed;
     private final AngleController gyroFeedback;
     private final Pose2d target;
+    private VStream robotSpeed;
 
     public SwerveDriveWithAiming(Pose2d target, Gamepad driver) {
         this.swerve = SwerveDrive.getInstance();
@@ -42,23 +46,41 @@ public SwerveDriveWithAiming(Pose2d target, Gamepad driver) {
         this.gyroFeedback = new AnglePIDController(Alignment.Gyro.P.get(), Alignment.Gyro.I.get(), Alignment.Gyro.D.get());
         this.target = target;
 
+        //testing SwerveDriveWhileShooting
+        this.robotSpeed = VStream.create(() -> chassisSpeedsVector())
+            .filtered(new VLowPassFilter(Settings.Swerve.VELOCITY_RC.get()))
+            .filtered(new VRateLimit(3));
+
         addRequirements(swerve);
     }
 
+    //testing
+    private Vector2D chassisSpeedsVector() {
+        ChassisSpeeds speeds = swerve.getChassisSpeeds();
+        return new Vector2D(
+            speeds.vxMetersPerSecond,
+            speeds.vyMetersPerSecond
+        );
+    }
+
     @Override
     public void execute() {
 
         Pose2d robotPose = AbstractOdometry.getInstance().getPose();
 
         Rotation2d target = new Rotation2d(
-                                robotPose.getX() - this.target.getX(), 
-                                robotPose.getY() - this.target.getY())
-                                .minus(Rotation2d.fromDegrees(180));
+                                this.target.getX()- robotPose.getX(), 
+                                this.target.getY() - robotPose.getY());
 
         double angularVel = -gyroFeedback.update(
-                            Angle.fromRotation2d(target.plus(Rotation2d.fromDegrees(180))),
+                            Angle.fromRotation2d(target),
                             Angle.fromRotation2d(swerve.getGyroAngle()));
 
-        swerve.drive(speed.get(), angularVel);
+        swerve.drive(speed.get(), 0);
+
+        //testing
+        SmartDashboard.putNumber("Swerve/getVelocityX", chassisSpeedsVector().x);
+        SmartDashboard.putNumber("Swerve/getVelocityY", chassisSpeedsVector().y);
+
     }
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -52,6 +52,9 @@ public interface Swerve {
 		SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.02);
 		double MAX_MODULE_SPEED = 5.88;
 
+        double NOTE_VELOCITY = Units.inchesToMeters(600);
+        SmartNumber VELOCITY_RC = new SmartNumber("Swerve/Velocity RC", 0.01);
+
         public interface Turn {
             double kP = 6.0;
             double kI = 0.0;
@@ -172,7 +175,7 @@ public interface Translation {
         }
 
         public interface Rotation {
-            SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 1);
+            SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 10);
             SmartNumber I = new SmartNumber("Alignment/Rotation/kI", 0);
             SmartNumber D = new SmartNumber("Alignment/Rotation/kD", 0);
         }

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java

@@ -279,6 +279,9 @@ public void periodic() {
         SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getWorldLinearAccelX());
         SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getWorldLinearAccelY());
         SmartDashboard.putNumber("Swerve/Z Acceleration (Gs)", gyro.getWorldLinearAccelZ());
+
+        SmartDashboard.putNumber("Swerve/Chassis Speed vx", getChassisSpeeds().vxMetersPerSecond);
+         SmartDashboard.putNumber("Swerve/Chassis Speed vy", getChassisSpeeds().vyMetersPerSecond);
     }
 
     @Override

src/main/java/com/stuypulse/robot/subsystems/swerve/module/SimModule.java

@@ -61,12 +61,13 @@ public SimModule(String id, Translation2d translationOffset) {
         this.id = id;
         this.translationOffset = translationOffset;         
 
-        driveController = new PIDController(Drive.kP, Drive.kI, Drive.kD)
+        driveController = new PIDController(0.1, 0, 0)
                 .setOutputFilter(x -> Robot.getMatchState() == Robot.MatchState.TELEOP ? 0 : x)
             .add(new MotorFeedforward(Drive.kS, Drive.kV, Drive.kA).velocity());
 
-        turnController = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD)
-            .setSetpointFilter(new ARateLimit(Driver.Turn.MAX_TELEOP_TURNING));
+        turnController = new AnglePIDController(0.1, 0, 0)
+            .setSetpointFilter(new ARateLimit(Driver.Turn.MAX_TELEOP_TURNING))
+            .setOutputFilter(x -> x);
 
         driveSim = new LinearSystemSim<>(identifyVelocityPositionSystem(Drive.kV, Drive.kA));
         turnSim = new LinearSystemSim<N2, N1, N1>(LinearSystemId.identifyPositionSystem(Turn.kV, Turn.kA));