Using an ADIS16448_IMU in a simulation

[Liran]

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?

[calcmogul]

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

[Liran]

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

[Liran]

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

[calcmogul]

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?

[Liran]

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());
  // }
}

[Joe_Ross]

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.