Using an ADIS16448_IMU in a simulation

Hey There! my name is Liran and I am front Neat Team#1943.
Currently I am trying to convert our simulation code to use an ADIS16448_IMU instead of a WPI_PigeonIMU. until now I’ve managed to find suitable replacement methods to most of the code but I am a bit struggling finding a replacement to the setRawHeading the the pigeon is using.

    m_pigeonSim.setRawHeading(m_driveSim.getHeading().getDegrees());

this is the whole simulation subsystem:

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.TalonFXSimCollection;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.BasePigeonSimCollection;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.simulation.ADIS16448_IMUSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.DriveSimulation;

public class SimDrive extends SubsystemBase {
  /** Creates a new DriveTrain. */
  private WPI_TalonFX m_rearLeft;
  private WPI_TalonFX m_rearRight;
  private WPI_TalonFX m_frontLeft;
  private WPI_TalonFX m_frontRight;
  private MotorControllerGroup m_leftMotorGroup;      
  private MotorControllerGroup m_rightGroup;
  // delete 
  private WPI_PigeonIMU m_pigeon;

  private ADIS16448_IMU m_imu;
  private ADIS16448_IMUSim m_imuSim; 
  private DifferentialDrive m_drive;
  private edu.wpi.first.math.kinematics.DifferentialDriveOdometry m_odometry;
  private TalonFXSimCollection m_frontLeftSim;
  private TalonFXSimCollection m_frontRightSim;
  private TalonFXSimCollection m_rearLeftSim;
  private TalonFXSimCollection m_rearRightSim;
  // delete
  private BasePigeonSimCollection m_pigeonSim;
  
  private DifferentialDrivetrainSim m_driveSim;
  private Field2d m_fieldSim;

  // temp objects
  private Pose2d m_resetLocation; 

  public SimDrive() {
    m_rearLeft = new WPI_TalonFX(DriveSimulation.kBackLeftMotor);
    m_rearRight = new WPI_TalonFX(DriveSimulation.kBackRightMotor);
    m_frontLeft = new WPI_TalonFX(DriveSimulation.kFrontLeftMotor);
    m_frontRight = new WPI_TalonFX(DriveSimulation.kFrontRightMotor);

    m_rearLeftSim = m_frontLeft.getSimCollection();
    m_rearRightSim = m_frontRight.getSimCollection();
    m_frontLeftSim = m_rearLeft.getSimCollection();
    m_frontRightSim = m_rearRight.getSimCollection();

    m_pigeon = new WPI_PigeonIMU(0);
    m_pigeonSim = m_pigeon.getSimCollection(); 

    m_imu = new ADIS16448_IMU();
    m_imuSim = new ADIS16448_IMUSim(m_imu);

    m_leftMotorGroup = new MotorControllerGroup(m_rearLeft, m_frontLeft);
    m_rightGroup = new MotorControllerGroup(m_rearRight, m_frontRight);

    m_drive = new DifferentialDrive(m_frontLeft, m_frontRight);
    m_driveSim = new DifferentialDrivetrainSim(DCMotor.getCIM(2), DriveSimulation.kGearRatio, 2.1, 25,
    Units.inchesToMeters(DriveSimulation.kWheelRadiusInches), 0.546, null);

    m_odometry = new edu.wpi.first.math.kinematics.DifferentialDriveOdometry(new Rotation2d(m_imu.getAngle()), 0, 0);

    m_fieldSim = new Field2d();
    m_resetLocation = m_fieldSim.getRobotPose();
    SmartDashboard.putData("Field", m_fieldSim);
  }

  public WPI_TalonFX getM_rearLeft() {
    return m_rearLeft;
  }

  public TalonFXSimCollection getBackLeftMotorSim() {
    return m_rearLeft.getSimCollection();
  }

  public TalonFXSimCollection getBackRightMotorSim() {
    return m_rearRight.getSimCollection();
  }

  public TalonFXSimCollection getFrontLeftMotorSim() {
    return m_frontLeft.getSimCollection();
  }

  public TalonFXSimCollection getFrontRightMotorSim() {
    return m_frontRight.getSimCollection();
  }

  public DifferentialDrivetrainSim getDifferentialDriveSim() {
    return m_driveSim;
  }

  public ADIS16448_IMU getImu() {
    return m_imu;
  }

  public void robotArcadeDrive(double move, double rot) {
    m_drive.arcadeDrive(-move, rot);
  }

  public void robotTankDrive(double leftMove, double rightMoveStick) {
    m_drive.tankDrive(leftMove, rightMoveStick);
  }

  public static int distanceToNativeUnits(double positionMeters) {
    double wheelRotations = positionMeters / (2 * Math.PI * Units.inchesToMeters(DriveSimulation.kWheelRadiusInches));
    double motorRotations = wheelRotations * DriveSimulation.kSensorGearRatio;
    int sensorCounts = (int) (motorRotations * DriveSimulation.kCountsPerRev);
    return sensorCounts;
  }

  public static double nativeUnitsToDistanceMeters(double sensorCounts) {
    double motorRotations = (double) sensorCounts / DriveSimulation.kCountsPerRev;
    double wheelRotations = motorRotations / DriveSimulation.kSensorGearRatio;
    double positionMeters = wheelRotations * (2 * Math.PI * Units.inchesToMeters(DriveSimulation.kWheelRadiusInches));
    return positionMeters;
  }

  public static int velocityToNativeUnits(double velocityMetersPerSecond) {
    double wheelRotationsPerSecond = velocityMetersPerSecond
        / (2 * Math.PI * Units.inchesToMeters(DriveSimulation.kWheelRadiusInches));
    double motorRotationsPerSecond = wheelRotationsPerSecond * DriveSimulation.kSensorGearRatio;
    double motorRotationsPer100ms = motorRotationsPerSecond / DriveSimulation.k100msPerSecond;
    int sensorCountsPer100ms = (int) (motorRotationsPer100ms * DriveSimulation.kCountsPerRev);
    return sensorCountsPer100ms;
  }

  @Override
  public void periodic() {
    m_odometry.update(new Rotation2d(m_imu.getAngle()),
        nativeUnitsToDistanceMeters(m_frontLeft.getSelectedSensorPosition()),
        nativeUnitsToDistanceMeters(m_frontRight.getSelectedSensorPosition()));
    m_fieldSim.setRobotPose(m_odometry.getPoseMeters());
  }

  public void resetRobotPosition(){
    m_fieldSim.setRobotPose(0, 0, new Rotation2d(m_imu.getAngle()));
    m_frontLeft.setSelectedSensorPosition(0);
    m_frontRight.setSelectedSensorPosition(0);
    m_pigeon.reset();
    m_odometry.update(new Rotation2d(m_imu.getAngle()),
    0, 0);
    m_odometry.resetPosition(new Rotation2d(m_imu.getAngle()), 0, 0, m_resetLocation);
  }

  public void simulationInit() {}
  
  public void simulationPeriodic() {
    m_rearLeftSim.setBusVoltage(RobotController.getBatteryVoltage());
    m_rearRightSim.setBusVoltage(RobotController.getBatteryVoltage());

    m_frontLeftSim.setBusVoltage(RobotController.getBatteryVoltage());
    m_frontRightSim.setBusVoltage(RobotController.getBatteryVoltage());

    m_driveSim.setInputs(
    m_rearLeftSim.getMotorOutputLeadVoltage(),
    m_rearRightSim.getMotorOutputLeadVoltage() * -1); 

    m_driveSim.update(DriveSimulation.kUpdateTime);

    m_rearLeftSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getLeftPositionMeters() * -1));
    m_rearLeftSim
        .setIntegratedSensorVelocity(velocityToNativeUnits(m_driveSim.getLeftVelocityMetersPerSecond()));

    m_rearRightSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getRightPositionMeters() * -1));
    m_rearRightSim.setIntegratedSensorVelocity(
        velocityToNativeUnits(m_driveSim.getRightVelocityMetersPerSecond()));

    m_frontLeftSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getLeftPositionMeters() * -1));
    m_frontLeftSim
        .setIntegratedSensorVelocity(velocityToNativeUnits(m_driveSim.getLeftVelocityMetersPerSecond()));

    m_frontRightSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getRightPositionMeters() * -1));
    m_frontRightSim.setIntegratedSensorVelocity(
        velocityToNativeUnits(m_driveSim.getRightVelocityMetersPerSecond() * -1));

    m_pigeonSim.setRawHeading(m_driveSim.getHeading().getDegrees());
  }
}

Is there any suitable replacement?

ADIS16448_IMUSim.setGyroAngleZ() from edu.wpi.first.wpilibj.simulation.ADIS16448_IMUSim is probably what you’re looking for.

Hey, thank you for the response. I already tried the setGyroAngleZ() function… it doesn’t work…

I am currently tring to create a function that is trying to mimic setRawHeading… but I am pretty sure that I am not on the right track

PigeonSim.setRawHeading() sets the yaw in sim, right? That’s what ADIS16448_IMUSim.setGyroAngleZ() is supposed to do, so it should work as long as you initialized the sim object correctly. Could you post the code you used in the attempt?

Thank you for the help.

this is the code:

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.TalonFXSimCollection;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.BasePigeonSimCollection;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.ADIS16448_IMU.IMUAxis;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
import edu.wpi.first.wpilibj.simulation.ADIS16448_IMUSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.DriveSimulation;

public class SimDrive extends SubsystemBase {
  /** Creates a new DriveTrain. */
  private WPI_TalonFX m_rearLeft;
  private WPI_TalonFX m_rearRight;
  private WPI_TalonFX m_frontLeft;
  private WPI_TalonFX m_frontRight;
  
  private MotorControllerGroup m_leftMotorGroup;      
  private MotorControllerGroup m_rightGroup;

  private ADIS16448_IMU m_imu;
  private ADIS16448_IMUSim m_imuSim; 

  private DifferentialDrive m_drive;
  private edu.wpi.first.math.kinematics.DifferentialDriveOdometry m_odometry;

  private TalonFXSimCollection m_frontLeftSim;
  private TalonFXSimCollection m_frontRightSim;
  private TalonFXSimCollection m_rearLeftSim;
  private TalonFXSimCollection m_rearRightSim;

  private DifferentialDrivetrainSim m_driveSim;

  private Field2d m_fieldSim;

  // temp objects
  private Pose2d m_resetLocation; 

  public SimDrive() {
    m_rearLeft = new WPI_TalonFX(DriveSimulation.kBackLeftMotor);
    m_rearRight = new WPI_TalonFX(DriveSimulation.kBackRightMotor);
    m_frontLeft = new WPI_TalonFX(DriveSimulation.kFrontLeftMotor);
    m_frontRight = new WPI_TalonFX(DriveSimulation.kFrontRightMotor);

    m_rearLeftSim = m_frontLeft.getSimCollection();
    m_rearRightSim = m_frontRight.getSimCollection();
    m_frontLeftSim = m_rearLeft.getSimCollection();
    m_frontRightSim = m_rearRight.getSimCollection();

    m_imu = new ADIS16448_IMU();
    m_imuSim = new ADIS16448_IMUSim(m_imu);

    m_leftMotorGroup = new MotorControllerGroup(m_rearLeft, m_frontLeft);
    m_rightGroup = new MotorControllerGroup(m_rearRight, m_frontRight);

    m_drive = new DifferentialDrive(m_frontLeft, m_frontRight);
    m_driveSim = new DifferentialDrivetrainSim(DCMotor.getCIM(2), DriveSimulation.kGearRatio, 2.1, 25,
    Units.inchesToMeters(DriveSimulation.kWheelRadiusInches), 0.546, null);

    m_odometry = new edu.wpi.first.math.kinematics.DifferentialDriveOdometry(new Rotation2d(m_imu.getAngle()), 0, 0);

    m_fieldSim = new Field2d();
    m_resetLocation = m_fieldSim.getRobotPose();
    SmartDashboard.putData("Field", m_fieldSim);
  }

  public WPI_TalonFX getM_rearLeft() {
    return m_rearLeft;
  }

  public TalonFXSimCollection getBackLeftMotorSim() {
    return m_rearLeft.getSimCollection();
  }

  public TalonFXSimCollection getBackRightMotorSim() {
    return m_rearRight.getSimCollection();
  }

  public TalonFXSimCollection getFrontLeftMotorSim() {
    return m_frontLeft.getSimCollection();
  }

  public TalonFXSimCollection getFrontRightMotorSim() {
    return m_frontRight.getSimCollection();
  }

  public DifferentialDrivetrainSim getDifferentialDriveSim() {
    return m_driveSim;
  }

  public ADIS16448_IMU getImu() {
    return m_imu;
  }

  public void robotArcadeDrive(double move, double rot) {
    m_drive.arcadeDrive(-move, rot);
  }

  public void robotTankDrive(double leftMove, double rightMoveStick) {
    m_drive.tankDrive(leftMove, rightMoveStick);
  }

  public static int distanceToNativeUnits(double positionMeters) {
    double wheelRotations = positionMeters / (2 * Math.PI * Units.inchesToMeters(DriveSimulation.kWheelRadiusInches));
    double motorRotations = wheelRotations * DriveSimulation.kSensorGearRatio;
    int sensorCounts = (int) (motorRotations * DriveSimulation.kCountsPerRev);
    return sensorCounts;
  }

  public static double nativeUnitsToDistanceMeters(double sensorCounts) {
    double motorRotations = (double) sensorCounts / DriveSimulation.kCountsPerRev;
    double wheelRotations = motorRotations / DriveSimulation.kSensorGearRatio;
    double positionMeters = wheelRotations * (2 * Math.PI * Units.inchesToMeters(DriveSimulation.kWheelRadiusInches));
    return positionMeters;
  }

  public static int velocityToNativeUnits(double velocityMetersPerSecond) {
    double wheelRotationsPerSecond = velocityMetersPerSecond
        / (2 * Math.PI * Units.inchesToMeters(DriveSimulation.kWheelRadiusInches));
    double motorRotationsPerSecond = wheelRotationsPerSecond * DriveSimulation.kSensorGearRatio;
    double motorRotationsPer100ms = motorRotationsPerSecond / DriveSimulation.k100msPerSecond;
    int sensorCountsPer100ms = (int) (motorRotationsPer100ms * DriveSimulation.kCountsPerRev);
    return sensorCountsPer100ms;
  }

  @Override
  public void periodic() {
    m_odometry.update(new Rotation2d(m_imu.getAngle()),
        nativeUnitsToDistanceMeters(m_frontLeft.getSelectedSensorPosition()),
        nativeUnitsToDistanceMeters(m_frontRight.getSelectedSensorPosition()));
    m_fieldSim.setRobotPose(m_odometry.getPoseMeters());
  }

  public void resetRobotPosition(){
    m_fieldSim.setRobotPose(0, 0, new Rotation2d(m_imu.getAngle()));

    m_frontLeft.setSelectedSensorPosition(0);
    m_frontRight.setSelectedSensorPosition(0);
    m_rearLeft.setSelectedSensorPosition(0);
    m_rearRight.setSelectedSensorPosition(0);

    m_imu.reset();
    
    m_odometry.update(new Rotation2d(m_imu.getAngle()),
    0, 0);
    m_odometry.resetPosition(new Rotation2d(m_imu.getAngle()), 0, 0, m_resetLocation);
  }

  public void simulationInit() {}
  
  public void simulationPeriodic() {
    m_rearLeftSim.setBusVoltage(RobotController.getBatteryVoltage());
    m_rearRightSim.setBusVoltage(RobotController.getBatteryVoltage());

    m_frontLeftSim.setBusVoltage(RobotController.getBatteryVoltage());
    m_frontRightSim.setBusVoltage(RobotController.getBatteryVoltage());

    m_driveSim.setInputs(
    m_rearLeftSim.getMotorOutputLeadVoltage(),
    m_rearRightSim.getMotorOutputLeadVoltage() * -1); 

    m_driveSim.update(DriveSimulation.kUpdateTime);

    m_rearLeftSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getLeftPositionMeters() * -1));
    m_rearLeftSim
        .setIntegratedSensorVelocity(velocityToNativeUnits(m_driveSim.getLeftVelocityMetersPerSecond()));

    m_rearRightSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getRightPositionMeters() * -1));
    m_rearRightSim.setIntegratedSensorVelocity(
        velocityToNativeUnits(m_driveSim.getRightVelocityMetersPerSecond()));

    m_frontLeftSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getLeftPositionMeters() * -1));
    m_frontLeftSim
        .setIntegratedSensorVelocity(velocityToNativeUnits(m_driveSim.getLeftVelocityMetersPerSecond()));

    m_frontRightSim
        .setIntegratedSensorRawPosition(distanceToNativeUnits(m_driveSim.getRightPositionMeters() * -1));
    m_frontRightSim.setIntegratedSensorVelocity(
        velocityToNativeUnits(m_driveSim.getRightVelocityMetersPerSecond() * -1));
        
    m_imuSim.setGyroAngleZ(m_driveSim.getHeading().getDegrees());

    // tests
    // m_pigeonSim.setRawHeading(m_driveSim.getHeading().getDegrees());
    // imuSetRawHeading();
  }

  // public void imuSetRawHeading(){
  //   m_imuSim.setGyroAngleY(m_driveSim.getHeading().getDegrees());
  //   m_imuSim.setGyroAngleX(m_driveSim.getHeading().getDegrees());
  //   m_imuSim.setGyroAngleZ(m_driveSim.getHeading().getDegrees());
  // }
}

The following code works for me. Can you explain what you mean it doesn’t work? What data did you expect, what data did you get?

public class Robot extends TimedRobot {

  private ADIS16448_IMU m_imu;
  private ADIS16448_IMUSim m_imuSim; 

  @Override
  public void robotInit() {
    m_imu = new ADIS16448_IMU();
    m_imuSim = new ADIS16448_IMUSim(m_imu);
  }

  @Override
  public void robotPeriodic() {
    SmartDashboard.putNumber("Angle", m_imu.getAngle());
    SmartDashboard.putNumber("Z", m_imu.getGyroAngleZ());
  }

  private double count = 0.0;

  @Override
  public void simulationPeriodic() {
    m_imuSim.setGyroAngleZ(count++);
  }
}

Edit: I also took the StateSpaceDriveSimulation example and changed the ADXRS450 gyro to ADIS16448 and it worked also.