completed collision simulation of chassis

tested, but not used in the actual simulation (activated only in test mode)
Shenzhen-Robotics-Alliance · Jul 13, 2024 · b1cb4e8 · b1cb4e8
1 parent 5778ae2
commit b1cb4e8
Show file tree

Hide file tree

Showing 5 changed files with 140 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -143,8 +143,8 @@ public static final class RobotPhysicsSimulationConfigs {
         public static final double MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = Math.toRadians(1200);
         public static final double TIME_CHASSIS_STOPS_ROTATING_NO_POWER_SEC = 0.3;
         public static final double DEFAULT_ROBOT_MASS = 60;
-        public static final double DEFAULT_BUMPER_WIDTH_METERS = 0.876; // 34.5 inch
-        public static final double DEFAULT_BUMPER_LENGTH_METERS = 0.876; // 34.5 inch
+        public static final double DEFAULT_BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5);
+        public static final double DEFAULT_BUMPER_LENGTH_METERS = Units.inchesToMeters(36);
 
         /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */
         public static final double ROBOT_BUMPER_COEFFICIENT_OF_FRICTION = 0.85;

diff --git a/src/main/java/frc/robot/tests/PhysicsSimulationTest.java b/src/main/java/frc/robot/tests/PhysicsSimulationTest.java
@@ -1,15 +1,21 @@
 package frc.robot.tests;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.utils.CompetitionFieldUtils.Simulation.Crescendo2024FieldSimulation;
 import frc.robot.utils.CompetitionFieldUtils.Simulation.OpponentRobotSimulation;
 
 public class PhysicsSimulationTest implements UnitTest {
-    private final OpponentRobotSimulation opponentRobotSimulation = new OpponentRobotSimulation();
+    private final OpponentRobotSimulation opponentRobotSimulation = new OpponentRobotSimulation(0);
     private final Crescendo2024FieldSimulation fieldSimulation = new Crescendo2024FieldSimulation(opponentRobotSimulation);
+
+    public PhysicsSimulationTest() {
+        fieldSimulation.addRobot(opponentRobotSimulation);
+    }
     @Override
     public void testStart() {
-        fieldSimulation.addRobot(opponentRobotSimulation);
+        opponentRobotSimulation.setPose(new Pose2d(15.2, 2.5, Rotation2d.fromRotations(0.5)));
     }
 
     private final XboxController xboxController = new XboxController(1);

diff --git a/...n/java/frc/robot/utils/CompetitionFieldUtils/Simulation/Crescendo2024FieldSimulation.java b/...n/java/frc/robot/utils/CompetitionFieldUtils/Simulation/Crescendo2024FieldSimulation.java
@@ -1,6 +1,12 @@
 package frc.robot.utils.CompetitionFieldUtils.Simulation;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import frc.robot.utils.CompetitionFieldUtils.FieldObjects.RobotOnField;
+
+import static frc.robot.Constants.CrescendoField2024Constants.*;
+
 public class Crescendo2024FieldSimulation extends CompetitionFieldSimulation {
     public Crescendo2024FieldSimulation(RobotOnField robot) {
         super(robot, new CrescendoFieldObstaclesMap());
@@ -11,8 +17,105 @@ public Crescendo2024FieldSimulation(RobotOnField robot) {
     public static final class CrescendoFieldObstaclesMap extends FieldObstaclesMap {
         public CrescendoFieldObstaclesMap() {
             super();
-            // super.addBorderLine(new Translation2d(1,2), new Translation2d(1,5));
-            // TODO: crescendo field
+
+            //left wall
+            super.addBorderLine(
+                    new Translation2d(0, 1),
+                    new Translation2d(0, 4.51)
+            );
+            super.addBorderLine(
+                    new Translation2d(0, 4.51),
+                    new Translation2d(0.9, 5)
+            );
+
+            super.addBorderLine(
+                    new Translation2d(0.9, 5),
+                    new Translation2d(0.9, 6.05)
+            );
+
+            super.addBorderLine(
+                    new Translation2d(0.9, 6.05),
+                    new Translation2d(0, 6.5)
+            );
+            super.addBorderLine(
+                    new Translation2d(0, 6.5),
+                    new Translation2d(0, 8.2)
+            );
+
+
+            // upper wall
+            super.addBorderLine(
+                    new Translation2d(0, 8.12),
+                    new Translation2d(FIELD_WIDTH, 8.12)
+            );
+
+            // righter wall 
+            super.addBorderLine(
+                    new Translation2d(FIELD_WIDTH, 1),
+                    new Translation2d(FIELD_WIDTH, 4.51)
+            );
+            super.addBorderLine(
+                    new Translation2d(FIELD_WIDTH, 4.51),
+                    new Translation2d(FIELD_WIDTH-0.9, 5)
+            );
+            super.addBorderLine(
+                    new Translation2d(FIELD_WIDTH-0.9, 5),
+                    new Translation2d(FIELD_WIDTH-0.9, 6.05)
+            );
+            super.addBorderLine(
+                    new Translation2d(FIELD_WIDTH-0.9, 6.05),
+                    new Translation2d(FIELD_WIDTH, 6.5)
+            );
+            super.addBorderLine(
+                    new Translation2d(FIELD_WIDTH, 6.5),
+                    new Translation2d(FIELD_WIDTH, 8.2)
+            );
+
+            // lower wall
+            super.addBorderLine(
+                    new Translation2d(1.92, 0),
+                    new Translation2d(FIELD_WIDTH-1.92, 0)
+            );
+
+            // red source wall
+            super.addBorderLine(
+                    new Translation2d(1.92, 0),
+                    new Translation2d(0, 1)
+            );
+
+            // blue source wall
+            super.addBorderLine(
+                    new Translation2d(FIELD_WIDTH-1.92, 0),
+                    new Translation2d(FIELD_WIDTH, 1)
+            );
+
+            // blue state
+            super.addRectangularObstacle(
+                    0.35, 0.35,
+                    new Pose2d(3.4, 4.1, new Rotation2d())
+            );
+            super.addRectangularObstacle(
+                    0.35, 0.35,
+                    new Pose2d(5.62, 4.1-1.28, Rotation2d.fromDegrees(30))
+            );
+            super.addRectangularObstacle(
+                    0.35, 0.35,
+                    new Pose2d(5.62, 4.1+1.28, Rotation2d.fromDegrees(60))
+            );
+
+            // red stage
+            super.addRectangularObstacle(
+                    0.35, 0.35,
+                    new Pose2d(FIELD_WIDTH-3.4, 4.1, new Rotation2d())
+            );
+            super.addRectangularObstacle(
+                    0.35, 0.35,
+                    new Pose2d(FIELD_WIDTH-5.62, 4.1-1.28, Rotation2d.fromDegrees(60))
+            );
+            super.addRectangularObstacle(
+                    0.35, 0.35,
+                    new Pose2d(FIELD_WIDTH-5.62, 4.1+1.28, Rotation2d.fromDegrees(30))
+            );
         }
     }
 }
diff --git a/...ain/java/frc/robot/utils/CompetitionFieldUtils/Simulation/HolonomicChassisSimulation.java b/...ain/java/frc/robot/utils/CompetitionFieldUtils/Simulation/HolonomicChassisSimulation.java
@@ -22,8 +22,11 @@ public abstract class HolonomicChassisSimulation extends Body implements RobotOn
     public HolonomicChassisSimulation(RobotProfile profile) {
         this.profile = profile;
 
+        /* the bumper needs to be rotated 90 degrees */
+        final double WIDTH_IN_WORLD_REFERENCE = profile.height,
+                HEIGHT_IN_WORLD_REFERENCE = profile.width;
         super.addFixture(
-                Geometry.createRectangle(profile.width, profile.height),
+                Geometry.createRectangle(WIDTH_IN_WORLD_REFERENCE, HEIGHT_IN_WORLD_REFERENCE),
                 profile.robotMass / (profile.height * profile.width),
                 Constants.RobotPhysicsSimulationConfigs.ROBOT_BUMPER_COEFFICIENT_OF_FRICTION,
                 Constants.RobotPhysicsSimulationConfigs.ROBOT_BUMPER_COEFFICIENT_OF_RESTITUTION

diff --git a/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulation/OpponentRobotSimulation.java b/src/main/java/frc/robot/utils/CompetitionFieldUtils/Simulation/OpponentRobotSimulation.java
@@ -10,6 +10,18 @@
 import org.ejml.simple.UnsupportedOperation;
 
 public class OpponentRobotSimulation extends HolonomicChassisSimulation implements HolonomicDrive {
+    public enum Behavior {
+        JOYSTICK_CONTROL,
+        AUTO_CYCLE,
+        QUEEN
+    }
+    private static final Pose2d[] redRobotsStartingPositions = new Pose2d[] {
+            new Pose2d(15.2, 7, Rotation2d.fromRotations(0.5)),
+            new Pose2d(15.2, 4, Rotation2d.fromRotations(0.5)),
+            new Pose2d(15.2, 2.5, Rotation2d.fromRotations(0.5))
+    };
+    /* if an opponent robot is not requested to be on field, it queens outside the field for performance */
+    private static final Pose2d robotQueeningPosition = new Pose2d(-5, 5, new Rotation2d());
     private static final HolonomicChassisSimulation.RobotProfile opponentRobotProfile = new RobotProfile(
             4,
             10,
@@ -18,8 +30,16 @@ public class OpponentRobotSimulation extends HolonomicChassisSimulation implemen
             Constants.RobotPhysicsSimulationConfigs.DEFAULT_BUMPER_WIDTH_METERS,
             Constants.RobotPhysicsSimulationConfigs.DEFAULT_BUMPER_LENGTH_METERS
     );
-    public OpponentRobotSimulation() {
+
+    private final int id;
+
+    /**
+     *
+     * */
+    public OpponentRobotSimulation(int id) {
         super(opponentRobotProfile);
+        this.id = id;
+        super.setSimulationWorldPose(robotQueeningPosition);
     }
 
     @Override
@@ -29,20 +49,6 @@ public void setPose(Pose2d currentPose) {
 
     @Override public void addVisionMeasurement(Pose2d visionPose, double timestamp) {throw new UnsupportedOperation("an opponent robot does not support vision measurement"); }
 
-    public enum Behavior {
-        JOYSTICK_CONTROL,
-        AUTO_CYCLE,
-        QUEEN
-    }
-
-    private static final Pose2d[] redRobotsStartingPositions = new Pose2d[] {
-            new Pose2d(15.2, 7, Rotation2d.fromRotations(0.5)),
-            new Pose2d(15.2, 4, Rotation2d.fromRotations(0.5)),
-            new Pose2d(15.2, 2.5, Rotation2d.fromRotations(0.5))
-    };
-    /* if an opponent robot is not requested to be on field, it queens outside the field for performance */
-    private static final Pose2d robotQueeningPosition = new Pose2d(-5, 5, new Rotation2d());
-
     // TODO using holonomic drive commands, make the opponent robots drive in the intended modes
 
     /**