feat: Add shrimp vision map

🦐🦐🦐
chopshop-166 · Jan 18, 2025 · ec43086 · ec43086
1 parent 3d10e27
commit ec43086
Showing 1 changed file with 23 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/maps/Shrimp.java b/src/main/java/frc/robot/maps/Shrimp.java
@@ -6,25 +6,23 @@
 
 import com.chopshop166.chopshoplib.drive.SDSSwerveModule;
 import com.chopshop166.chopshoplib.drive.SDSSwerveModule.Configuration;
-import com.chopshop166.chopshoplib.leds.SegmentConfig;
+import com.chopshop166.chopshoplib.maps.CameraSource;
 import com.chopshop166.chopshoplib.maps.RobotMapFor;
 import com.chopshop166.chopshoplib.maps.SwerveDriveMap;
-import com.chopshop166.chopshoplib.motors.CSSpark;
-import com.chopshop166.chopshoplib.motors.CSSparkFlex;
+import com.chopshop166.chopshoplib.maps.VisionMap;
 import com.chopshop166.chopshoplib.motors.CSSparkMax;
-import com.chopshop166.chopshoplib.sensors.CtreEncoder;
 import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro;
-import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro2;
 import com.chopshop166.chopshoplib.states.PIDValues;
 import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix6.configs.CANcoderConfiguration;
-import com.ctre.phoenix6.hardware.CANcoder;
 import com.pathplanner.lib.config.ModuleConfig;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.AnalogEncoder;
@@ -109,6 +107,24 @@ SDSSwerveModule.MK4_V2.wheelDiameter, new PIDValues(0.004, 0.00,
                 config, holonomicDrive);
     }
 
+    @Override
+    public VisionMap getVisionMap() {
+        // Cam mounted 9.029 sideways of center, 9.029 in. forward of center, 9.75 in.
+        // up from center.
+        Transform3d robotToCamFL = new Transform3d(
+                new Translation3d(Units.inchesToMeters(-9.029), Units.inchesToMeters(9.029),
+                        Units.inchesToMeters(9.75)),
+                new Rotation3d(0, Units.degreesToRadians(-45), Units.degreesToRadians(65.752)));
+
+        // Cam mounted facing __, __ forward of center, __ up from center.
+        Transform3d robotToCamFR = new Transform3d(
+                new Translation3d(Units.inchesToMeters(9.029), Units.inchesToMeters(9.029),
+                        Units.inchesToMeters(9.75)),
+                new Rotation3d(0, Units.degreesToRadians(45), Units.degreesToRadians(65.752)));
+        return new VisionMap(new CameraSource("ShrimpCamFL", robotToCamFL),
+                new CameraSource("ShrimpCamFR", robotToCamFR));
+    }
+
     @Override
     public void setupLogging() {
         Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick