Package ftclib.sensor

Class GoBildaPinpointDriver

java.lang.Object
com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice<com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple>
ftclib.sensor.GoBildaPinpointDriver
All Implemented Interfaces:
com.qualcomm.robotcore.hardware.HardwareDevice, com.qualcomm.robotcore.hardware.usb.RobotArmingStateNotifier.Callback
@I2cDeviceType @DeviceProperties(name="goBILDA\u00ae Pinpoint Odometry Computer", xmlTag="goBILDAPinpoint", description="goBILDA\u00ae Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)") public class GoBildaPinpointDriver extends com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice<com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple>

  • Field Details

    • deviceStatus

      private int deviceStatus

    • loopTime

      private int loopTime

    • xEncoderValue

      private int xEncoderValue

    • yEncoderValue

      private int yEncoderValue

    • xPosition

      private float xPosition

    • yPosition

      private float yPosition

    • hOrientation

      private float hOrientation

    • xVelocity

      private float xVelocity

    • yVelocity

      private float yVelocity

    • hVelocity

      private float hVelocity

    • goBILDA_SWINGARM_POD

      private static final float goBILDA_SWINGARM_POD
      See Also:

    • goBILDA_4_BAR_POD

      private static final float goBILDA_4_BAR_POD
      See Also:

    • DEFAULT_ADDRESS

      public static final byte DEFAULT_ADDRESS
      See Also:

  • Constructor Details

    • GoBildaPinpointDriver

      public GoBildaPinpointDriver(com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned)

  • Method Details

    • getManufacturer

      public com.qualcomm.robotcore.hardware.HardwareDevice.Manufacturer getManufacturer()

    • doInitialize

      protected boolean doInitialize()
      Specified by:
      doInitialize in class com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice<com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple>

    • getDeviceName

      public String getDeviceName()

    • writeInt

      private void writeInt(GoBildaPinpointDriver.Register reg, int i)
      Writes an int to the i2c device
      Parameters:
      reg - the register to write the int to
      i - the integer to write to the register

    • readInt

      private int readInt(GoBildaPinpointDriver.Register reg)
      Reads an int from a register of the i2c device
      Parameters:
      reg - the register to read from
      Returns:
      returns an int that contains the value stored in the read register

    • byteArrayToFloat

      private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder)
      Converts a byte array to a float value
      Parameters:
      byteArray - byte array to transform
      byteOrder - order of byte array to convert
      Returns:
      the float value stored by the byte array

    • readFloat

      private float readFloat(GoBildaPinpointDriver.Register reg)
      Reads a float from a register
      Parameters:
      reg - the register to read
      Returns:
      the float value stored in that register

    • floatToByteArray

      private byte[] floatToByteArray(float value, ByteOrder byteOrder)
      Converts a float to a byte array
      Parameters:
      value - the float array to convert
      Returns:
      the byte array converted from the float

    • writeByteArray

      private void writeByteArray(GoBildaPinpointDriver.Register reg, byte[] bytes)
      Writes a byte array to a register on the i2c device
      Parameters:
      reg - the register to write to
      bytes - the byte array to write

    • writeFloat

      private void writeFloat(GoBildaPinpointDriver.Register reg, float f)
      Writes a float to a register on the i2c device
      Parameters:
      reg - the register to write to
      f - the float to write

    • lookupStatus

      private GoBildaPinpointDriver.DeviceStatus lookupStatus(int s)
      Looks up the DeviceStatus enum corresponding with an int value
      Parameters:
      s - int to lookup
      Returns:
      the Odometry Computer state

    • update

      public void update()
      Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called.

    • update

      public void update(GoBildaPinpointDriver.readData data)
      Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING is supported.
      Parameters:
      data - GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING

    • setOffsets

      public void setOffsets(double xOffset, double yOffset)
      Sets the odometry pod positions relative to the point that the odometry computer tracks around.

      The most common tracking position is the center of the robot.

      The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
      the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
      Parameters:
      xOffset - how sideways from the center of the robot is the X (forward) pod? Left increases
      yOffset - how far forward from the center of the robot is the Y (Strafe) pod? forward increases

    • recalibrateIMU

      public void recalibrateIMU()
      Recalibrates the Odometry Computer's internal IMU.

      Robot MUST be stationary

      Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds.

    • resetPosAndIMU

      public void resetPosAndIMU()
      Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

      Robot MUST be stationary

      Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds.

    • setEncoderDirections

      public void setEncoderDirections(GoBildaPinpointDriver.EncoderDirection xEncoder, GoBildaPinpointDriver.EncoderDirection yEncoder)
      Can reverse the direction of each encoder.
      Parameters:
      xEncoder - FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward
      yEncoder - FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left

    • setEncoderResolution

      public void setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods pods)
      If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

      Parameters:
      pods - goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD

    • setEncoderResolution

      public void setEncoderResolution(double ticks_per_mm)
      Sets the encoder resolution in ticks per mm of the odometry pods.
      You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel.
      Parameters:
      ticks_per_mm - should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192

    • setYawScalar

      public void setYawScalar(double yawOffset)
      Tuning this value should be unnecessary.
      The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

      This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

      You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

      If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com
      Parameters:
      yawOffset - A scalar for the robot's heading.

    • setPosition

      public org.firstinspires.ftc.robotcore.external.navigation.Pose2D setPosition(org.firstinspires.ftc.robotcore.external.navigation.Pose2D pos)
      Send a position that the Pinpoint should use to track your robot relative to. You can use this to update the estimated position of your robot with new external sensor data, or to run a robot in field coordinates.

      This overrides the current position.

      Using this feature to track your robot's position in field coordinates:
      When you start your code, send a Pose2D that describes the starting position on the field of your robot.
      Say you're on the red alliance, your robot is against the wall and closer to the audience side, and the front of your robot is pointing towards the center of the field. You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always keep track of how far away from the center of the field you are.

      Using this feature to update your position with additional sensors:
      Some robots have a secondary way to locate their robot on the field. This is commonly Apriltag localization in FTC, but it can also be something like a distance sensor. Often these external sensors are absolute (meaning they measure something about the field) so their data is very accurate. But they can be slower to read, or you may need to be in a very specific position on the field to use them. In that case, spend most of your time relying on the Pinpoint to determine your location. Then when you pull a new position from your secondary sensor, send a setPosition command with the new position. The Pinpoint will then track your movement relative to that new, more accurate position.
      Parameters:
      pos - a Pose2D describing the robot's new position.

    • getDeviceID

      public int getDeviceID()
      Checks the deviceID of the Odometry Computer. Should return 1.
      Returns:
      1 if device is functional.

    • getDeviceVersion

      public int getDeviceVersion()
      Returns:
      the firmware version of the Odometry Computer

    • getYawScalar

      public float getYawScalar()

    • getDeviceStatus

      public GoBildaPinpointDriver.DeviceStatus getDeviceStatus()
      Device Status stores any faults the Odometry Computer may be experiencing. These faults include:
      Returns:
      one of the following states:
      NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
      READY - The device is currently functioning as normal. GREEN LED
      CALIBRATING - The device is currently recalibrating the gyro. RED LED
      FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
      FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
      FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED

    • getLoopTime

      public int getLoopTime()
      Checks the Odometry Computer's most recent loop time.

      If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com
      Returns:
      loop time in microseconds (1/1,000,000 seconds)

    • getFrequency

      public double getFrequency()
      Checks the Odometry Computer's most recent loop frequency.

      If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com
      Returns:
      Pinpoint Frequency in Hz (loops per second),

    • getEncoderX

      public int getEncoderX()
      Returns:
      the raw value of the X (forward) encoder in ticks

    • getEncoderY

      public int getEncoderY()
      Returns:
      the raw value of the Y (strafe) encoder in ticks

    • getPosX

      public double getPosX()
      Returns:
      the estimated X (forward) position of the robot in mm

    • getPosY

      public double getPosY()
      Returns:
      the estimated Y (Strafe) position of the robot in mm

    • getHeading

      public double getHeading()
      Returns:
      the estimated H (heading) position of the robot in Radians

    • getVelX

      public double getVelX()
      Returns:
      the estimated X (forward) velocity of the robot in mm/sec

    • getVelY

      public double getVelY()
      Returns:
      the estimated Y (strafe) velocity of the robot in mm/sec

    • getHeadingVelocity

      public double getHeadingVelocity()
      Returns:
      the estimated H (heading) velocity of the robot in radians/sec

    • getXOffset

      public float getXOffset()
      This uses its own I2C read, avoid calling this every loop.
      Returns:
      the user-set offset for the X (forward) pod

    • getYOffset

      public float getYOffset()
      This uses its own I2C read, avoid calling this every loop.
      Returns:
      the user-set offset for the Y (strafe) pod

    • getPosition

      public org.firstinspires.ftc.robotcore.external.navigation.Pose2D getPosition()
      Returns:
      a Pose2D containing the estimated position of the robot

    • getVelocity

      public org.firstinspires.ftc.robotcore.external.navigation.Pose2D getVelocity()
      Returns:
      a Pose2D containing the estimated velocity of the robot, velocity is unit per second