001/* Copyright (C) Cross The Road Electronics 2024 */
002package com.ctre.phoenix.sensors;
003
004import com.ctre.phoenix.ErrorCode;
005import com.ctre.phoenix.platform.DeviceType;
006import com.ctre.phoenix.platform.PlatformJNI;
007
008public class BasePigeonSimCollection{
009    private int _id;
010    private DeviceType _type;
011
012    public BasePigeonSimCollection(BasePigeon pigeon, boolean isRibbonCable){
013        _id = pigeon.getDeviceID();
014        _type = isRibbonCable ? DeviceType.RibbonPigeonIMU : DeviceType.PigeonIMU;
015    }
016
017    /**
018     * Sets the simulated input heading position of the Pigeon IMU.
019     * <p>
020     * The Pigeon IMU integrates the delta between each new raw heading value and uses
021     * this to calculate the true reported yaw and fused heading.
022     * <p>
023     * When using the WPI Sim GUI, you will notice a readonly 'yaw' and
024     * settable 'RawHeading'.  The readonly signal is the emulated yaw
025     * which will match self-test in Tuner and the hardware API.  Changes to
026     * 'RawHeading' will be integrated into the emulated yaw.  This way
027     * a simulator can modify the heading without overriding your
028     * hardware API calls for home-ing your sensor.
029     * <p>
030     * Inputs to this function over time should be continuous,
031     * as user calls of setYaw() or setFusedHeading()
032     * will be accounted for in the calculation.
033     *
034     * @param newHeading the new input heading in degrees
035     *
036     * @return  error code
037     */
038    public ErrorCode setRawHeading(double newHeading)
039    {
040        int retval = PlatformJNI.JNI_SimSetPhysicsInput(_type.value, _id, "HeadingRaw", newHeading);
041        return ErrorCode.valueOf(retval);
042    }
043
044    /**
045     * Adds to the simulated heading of the Pigeon IMU
046     *
047     * @param dHeading the change in heading in degrees
048     *
049     * @return error code
050     */
051    public ErrorCode addHeading(double dHeading)
052    {
053        int retval = PlatformJNI.JNI_SimSetPhysicsInput(_type.value, _id, "HeadingAdd", dHeading);
054        return ErrorCode.valueOf(retval);
055    }
056}