odometry and sim work well

Team766 · Nov 7, 2024 · 7f836f6 · 7f836f6
1 parent 01f437a
commit 7f836f6
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 14 deletions.
diff --git a/simgui.json b/simgui.json
@@ -5,6 +5,9 @@
         "header": {
           "open": true
         }
+      },
+      "window": {
+        "visible": false
       }
     }
   },
@@ -44,8 +47,10 @@
         "open": true
       },
       "open": true
-    },
-    "visible": true
+    }
+  },
+  "NetworkTables View": {
+    "visible": false
   },
   "Plot": {
     "Plot <0>": {

diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveDrive.java
@@ -71,6 +71,9 @@ public class SwerveDrive extends Mechanism {
     private final StructPublisher<Pose2d> simPosePublisher =
             NetworkTableInstance.getDefault().getStructTopic("SimRobotPose", Pose2d.struct).publish();
 
+    private final StructPublisher<Pose2d> odometryPosePublisher =
+            NetworkTableInstance.getDefault().getStructTopic("OdometryRobotPose", Pose2d.struct).publish();
+
     private PIDController rotationPID;
 
     private boolean movingToTarget = false;
@@ -177,14 +180,13 @@ public SwerveDrive(SwerveConfig config) {
                 Parameters.ROBOT_MASS,
                 Parameters.ROBOT_MOMENT_OF_INERTIA,
                 List.of(
-                        swerveFR.getSim(),
                         swerveFL.getSim(),
-                        swerveBR.getSim(),
-                        swerveBL.getSim()));
+                        swerveFR.getSim(),
+                        swerveBL.getSim(),
+                        swerveBR.getSim()));
         simPrevTime = RobotProvider.instance.getClock().getTime();
         m_field = new Field2d();
         SmartDashboard.putData("Field", m_field);
-        // SmartDashboard.putData("SwerveStates", swerveModuleStates);
     }
 
     /**
@@ -243,12 +245,14 @@ public void controlRobotOriented(double x, double y, double turn) {
     private void controlFieldOrientedBase(double x, double y, double turn) {
         checkContextOwnership();
 
+        SmartDashboard.putString("Swerve Commands", "x: " + x + ", y: " + y + ", turn: " + turn);
         double yawRad =
                 Math.toRadians(
                         getHeading()
                                 + (alliance.isPresent() && alliance.get() == Alliance.Blue
                                         ? 0
                                         : 180));
+        SmartDashboard.putNumber("rotate by: ", Math.toDegrees(yawRad));
         // Applies a rotational translation to controlRobotOriented
         // Counteracts the forward direction changing when the robot turns
         // TODO: change to inverse rotation matrix (rather than negative angle)
@@ -357,7 +361,8 @@ public void setCross() {
      * Sets to 180 degrees if the driver is on red (facing backwards)
      */
     public void resetGyro() {
-        resetGyro(alliance.isPresent() && alliance.get() == Alliance.Blue ? 0 : 180);
+        alliance = DriverStation.getAlliance();
+        resetGyro(alliance.isPresent() && alliance.get().equals(Alliance.Blue) ? 0 : 180);
     }
 
     /**
@@ -482,9 +487,12 @@ public void runSim() {
         final Pose2d pose = sim.getCurPose();
         simPosePublisher.set(pose);
 
+        odometryPosePublisher.set(getCurrentPosition());
+
         final double yaw = pose.getRotation().getDegrees();
         gyroSimState.addYaw(normalizeAngleDegrees(yaw - simPrevYaw));
         simPrevYaw = yaw;
+        SmartDashboard.putNumber("sim yaw", yaw);
         m_field.setRobotPose(pose);
     }
 }
diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java
@@ -1,10 +1,13 @@
 package com.team766.robot.common.mechanisms;
 
 import static com.team766.robot.common.constants.SwerveConstants.*;
-
+import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.ctre.phoenix6.sim.CANcoderSimState;
 import com.ctre.phoenix6.sim.TalonFXSimState;
 import com.team766.hal.MotorController;
@@ -17,7 +20,7 @@
 import com.team766.robot.reva.mechanisms.MotorUtil;
 import com.team766.simulator.Parameters;
 import com.team766.simulator.PhysicalConstants;
-
+import de.erichseifert.gral.graphics.Orientation;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -85,6 +88,8 @@ public SwerveModule(
         this.driveSimState = ((TalonFX) drive).getSimState();
         this.steerSimState = ((TalonFX) steer).getSimState();
         this.encoderSimState = encoder.getSimState();
+
+        encoder.getConfigurator().apply(new CANcoderConfiguration().withMagnetSensor(new MagnetSensorConfigs().withMagnetOffset(0.25)));
     }
 
     private double computeEncoderOffset() {
@@ -109,9 +114,9 @@ private double computeEncoderOffset() {
      */
     public void steer(Vector2D vector) {
         boolean reversed = false;
-        // SmartDashboard.putString(
-        //         "[" + modulePlacement + "]" + "x, y",
-        //         String.format("%.2f, %.2f", vector.getX(), vector.getY()));
+        SmartDashboard.putString(
+                "[" + modulePlacement + "]" + "x, y",
+                String.format("%.2f, %.2f", vector.getX(), vector.getY()));
 
         // Calculates the angle of the vector from -180° to 180°
         final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX()));
@@ -144,11 +149,11 @@ public void steer(Vector2D vector) {
         // Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor
         // understands
         // SmartDashboard.putNumber(
-        //         "[" + modulePlacement + "]" + "Steer", ENCODER_CONVERSION_FACTOR * angleDegrees);
+        //         "[" + modulePlacement + "]" + "Steer", angleDegrees);
 
         steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees);
 
-        // SmartDashboard.putNumber("[" + modulePlacement + "]" + "TargetAngle", vectorTheta);
+        SmartDashboard.putNumber("[" + modulePlacement + "]" + "TargetAngle", vectorTheta);
         // SmartDashboard.putNumber(
         //         "[" + modulePlacement + "]" + "RelativeAngle",
         //         (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR - offset) % 360);
@@ -228,6 +233,8 @@ public void runSim() {
         encoderSimState.setRawPosition(sim.getAzimuthEncoderPositionRev());
         encoderSimState.setVelocity(sim.getAzimuthEncoderVelocityRPM() / 60.);
 
+        SmartDashboard.putNumber("[" + modulePlacement + "]" + " absolute encoder", (360 * encoder.getAbsolutePosition().getValueAsDouble()));
+        SmartDashboard.putNumber("[" + modulePlacement + "]" + " offset", offset);
         sim.setInputVoltages(
                 driveSimState.getMotorVoltage(),
                 steerSimState.getMotorVoltage());

diff --git a/src/main/java/com/team766/robot/reva/OI.java b/src/main/java/com/team766/robot/reva/OI.java
@@ -7,6 +7,8 @@
 import com.team766.logging.Category;
 import com.team766.robot.common.DriverOI;
 import com.team766.robot.reva.constants.InputConstants;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 /**
  * This class is the glue that binds the controls on the physical operator
@@ -52,6 +54,8 @@ public void run(Context context) {
 
             // Driver OI: take input from left, right joysticks.  control drive.
             driverOI.runOI(context);
+            if (leftJoystick.getButtonPressed(1)) {Robot.drive.setCurrentPosition(new Pose2d(1, 3, new Rotation2d()));}
+            if (leftJoystick.getButtonPressed(2)) {Robot.drive.resetGyro();}
             // Debug OI: allow for finer-grain testing of each mechanism.
             // debugOI.runOI(context);