From e12d6901671044dfcf7eb0380c8193ec69bbc63d Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Wed, 25 Jan 2017 21:16:49 -0800 Subject: [PATCH] finish transferring code --- .../usfirst/frc/team3501/robot/MathLib.java | 8 + .../robot/commands/driving/DriveDistance.java | 73 +- .../team3501/robot/subsystems/DriveTrain.java | 24 + .../frc/team3501/robot/utils/BNO055.java | 827 ++++++++++++++++++ 4 files changed, 921 insertions(+), 11 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/utils/BNO055.java diff --git a/src/org/usfirst/frc/team3501/robot/MathLib.java b/src/org/usfirst/frc/team3501/robot/MathLib.java index e870d58..5269304 100644 --- a/src/org/usfirst/frc/team3501/robot/MathLib.java +++ b/src/org/usfirst/frc/team3501/robot/MathLib.java @@ -101,4 +101,12 @@ public class MathLib { return val; } } + + public static double calcLeftTankDrive(double x, double y) { + return limitValue(y + x); + } + + public static double calcRightTankDrive(double x, double y) { + return limitValue(y - x); + } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java index 8af1bcb..47fc46e 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java @@ -1,47 +1,98 @@ package org.usfirst.frc.team3501.robot.commands.driving; +import org.usfirst.frc.team3501.robot.MathLib; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; +import org.usfirst.frc.team3501.robot.utils.PIDController; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.command.Command; /** * This command makes the robot drive a specified distance using encoders on the * robot and using a feedback loop * - * parameters: - * distance the robot will move in inches - * motorVal: the motor input to set the motors to + * parameters: distance the robot will move in inches motorVal: the motor input + * to set the motors to */ public class DriveDistance extends Command { + private DriveTrain driveTrain = Robot.getDriveTrain(); + private double maxTimeOut; + private PIDController driveController; + private PIDController gyroController; + private Preferences prefs; + + private double target; + private double gyroP; + private double gyroI; + private double gyroD; + + private double driveP; + private double driveI; + private double driveD; public DriveDistance(double distance, double motorVal) { - requires(Robot.getDriveTrain()); + requires(driveTrain); + this.driveP = driveTrain.driveP; + this.driveI = driveTrain.driveI; + this.driveD = driveTrain.driveD; + this.gyroP = driveTrain.defaultGyroP; + this.gyroI = driveTrain.defaultGyroI; + this.gyroD = driveTrain.defaultGyroD; + this.driveController = new PIDController(this.driveP, this.driveI, + this.driveD); + this.driveController.setDoneRange(0.5); + this.driveController.setMaxOutput(1.0); + this.driveController.setMinDoneCycles(5); + + this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD); + this.gyroController.setDoneRange(1); + this.gyroController.setMinDoneCycles(5); } - // Called just before this Command runs the first time @Override protected void initialize() { + this.driveTrain.resetEncoders(); + this.driveTrain.resetGyro(); + this.driveController.setSetPoint(this.target); + this.gyroController.setSetPoint(this.driveTrain.getZeroAngle()); } - // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + double xVal = 0; + double yVal = this.driveController + .calcPID(this.driveTrain.getAvgEncoderDistance()); + + if (this.driveTrain.getAngle() - this.driveTrain.getZeroAngle() < 30) { + xVal = -this.gyroController + .calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle()); + } + // System.out.println("turn: " + xVal); + double leftDrive = MathLib.calcLeftTankDrive(-xVal, yVal); + double rightDrive = MathLib.calcRightTankDrive(xVal, -yVal); + + this.driveTrain.setMotorValues(leftDrive, rightDrive); + + System.out.println(driveTrain.getAvgEncoderDistance()); + // System.out.println("motorval: " + yVal); } - // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + boolean isDone = this.driveController.isDone(); + if (timeSinceInitialized() >= maxTimeOut || isDone) + System.out.println("time: " + timeSinceInitialized()); + return timeSinceInitialized() >= maxTimeOut || isDone; } - // Called once after isFinished returns true @Override protected void end() { + driveTrain.stop(); } - // Called when another command which requires one or more of the same - // subsystems is scheduled to run @Override protected void interrupted() { + end(); } } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 364340e..ba5392a 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -2,6 +2,7 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; +import org.usfirst.frc.team3501.robot.utils.BNO055; import com.ctre.CANTalon; @@ -10,6 +11,11 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.Subsystem; public class DriveTrain extends Subsystem { + public static double driveP = 0.008, driveI = 0.001, driveD = -0.002; + public static double defaultGyroP = 0.009, defaultGyroI = 0.00000, + defaultGyroD = -0.000; + private double gyroZero = 0; + public static final double WHEEL_DIAMETER = 6; // inches public static final int ENCODER_PULSES_PER_REVOLUTION = 256; public static final double INCHES_PER_PULSE = WHEEL_DIAMETER * Math.PI @@ -20,6 +26,8 @@ public class DriveTrain extends Subsystem { private final RobotDrive robotDrive; private final Encoder leftEncoder, rightEncoder; + private BNO055 imu; + private DriveTrain() { // MOTOR CONTROLLERS frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); @@ -111,6 +119,22 @@ public class DriveTrain extends Subsystem { return (getLeftSpeed() + getRightSpeed()) / 2.0; } + // ------Gyro------// + public double getAngle() { + if (!this.imu.isInitialized()) + return -1; + return this.imu.getHeading() - this.gyroZero; + } + + public void resetGyro() { + this.gyroZero = this.getAngle(); + + } + + public double getZeroAngle() { + return this.gyroZero; + } + @Override protected void initDefaultCommand() { setDefaultCommand(new JoystickDrive()); diff --git a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java new file mode 100644 index 0000000..9c6de3d --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java @@ -0,0 +1,827 @@ +package org.usfirst.frc.team3501.robot.utils; + +import java.util.TimerTask; + +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.Timer; + +/** + * BNO055 IMU for the FIRST Robotics Competition. References throughout the code + * are to the following sensor documentation: http://git.io/vuOl1 + * + * To use the sensor, wire up to it over I2C on the roboRIO. Creating an + * instance of this class will cause communications with the sensor to being.All + * communications with the sensor occur in a separate thread from your robot + * code to avoid blocking the main robot program execution. + * + * Example: private static BNO055 imu; + * + * public Robot() { imu = + * BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, + * BNO055.vector_type_t.VECTOR_EULER); } + * + * You can check the status of the sensor by using the following methods: + * isSensorPresent(); //Checks if the code can talk to the sensor over I2C // If + * this returns false, check your wiring. isInitialized(); //Checks if the + * sensor initialization has completed. // Initialization takes about 3 seconds. + * You won't get // position data back from the sensor until its init'd. + * isCalibrated(); //The BNO055 will return position data after its init'd, // + * but the position data may be inaccurate until all // required sensors report + * they are calibrated. Some // Calibration sequences require you to move the + * BNO055 // around. See the method comments for more info. + * + * Once the sensor calibration is complete , you can get position data by by + * using the getVector() method. See this method definiton for usage info. + * + * This code was originally ported from arduino source developed by Adafruit. + * See the original comment header below. + * + * @author james@team2168.org + * + * + * ORIGINAL ADAFRUIT HEADER - + * https://github.com/adafruit/Adafruit_BNO055/ + * ==================================================================== + * === + * This is a library for the BNO055 orientation sensor + * + * Designed specifically to work with the Adafruit BNO055 Breakout. + * + * Pick one up today in the adafruit shop! ------> + * http://www.adafruit.com/products + * + * These sensors use I2C to communicate, 2 pins are required to + * interface. + * + * Adafruit invests time and resources providing this open source code, + * please support Adafruit and open-source hardware by purchasing + * products from Adafruit! + * + * Written by KTOWN for Adafruit Industries. + * + * MIT license, all text above must be included in any redistribution + * + */ +public class BNO055 { + // Tread variables + private java.util.Timer executor; + private static final long THREAD_PERIOD = 20; // ms - max poll rate on sensor. + + public static final byte BNO055_ADDRESS_A = 0x28; + public static final byte BNO055_ADDRESS_B = 0x29; + public static final int BNO055_ID = 0xA0; + + private static BNO055 instance; + + private static I2C imu; + private static int _mode; + private static opmode_t requestedMode; // user requested mode of operation. + private static vector_type_t requestedVectorType; + + // State machine variables + private volatile int state = 0; + private volatile boolean sensorPresent = false; + private volatile boolean initialized = false; + private volatile double currentTime; // seconds + private volatile double nextTime; // seconds + private volatile byte[] positionVector = new byte[6]; + private volatile long turns = 0; + private volatile double[] xyz = new double[3]; + + private double zeroReferenceConst = 0; + + public class SystemStatus { + public int system_status; + public int self_test_result; + public int system_error; + } + + public enum reg_t { + /* Page id register definition */ + BNO055_PAGE_ID_ADDR(0X07), + + /* PAGE0 REGISTER DEFINITION START */ + BNO055_CHIP_ID_ADDR(0x00), BNO055_ACCEL_REV_ID_ADDR( + 0x01), BNO055_MAG_REV_ID_ADDR(0x02), BNO055_GYRO_REV_ID_ADDR( + 0x03), BNO055_SW_REV_ID_LSB_ADDR(0x04), BNO055_SW_REV_ID_MSB_ADDR( + 0x05), BNO055_BL_REV_ID_ADDR(0X06), + + /* Accel data register */ + BNO055_ACCEL_DATA_X_LSB_ADDR(0X08), BNO055_ACCEL_DATA_X_MSB_ADDR( + 0X09), BNO055_ACCEL_DATA_Y_LSB_ADDR(0X0A), BNO055_ACCEL_DATA_Y_MSB_ADDR( + 0X0B), BNO055_ACCEL_DATA_Z_LSB_ADDR( + 0X0C), BNO055_ACCEL_DATA_Z_MSB_ADDR(0X0D), + + /* Mag data register */ + BNO055_MAG_DATA_X_LSB_ADDR(0X0E), BNO055_MAG_DATA_X_MSB_ADDR( + 0X0F), BNO055_MAG_DATA_Y_LSB_ADDR(0X10), BNO055_MAG_DATA_Y_MSB_ADDR( + 0X11), BNO055_MAG_DATA_Z_LSB_ADDR( + 0X12), BNO055_MAG_DATA_Z_MSB_ADDR(0X13), + + /* Gyro data registers */ + BNO055_GYRO_DATA_X_LSB_ADDR(0X14), BNO055_GYRO_DATA_X_MSB_ADDR( + 0X15), BNO055_GYRO_DATA_Y_LSB_ADDR(0X16), BNO055_GYRO_DATA_Y_MSB_ADDR( + 0X17), BNO055_GYRO_DATA_Z_LSB_ADDR( + 0X18), BNO055_GYRO_DATA_Z_MSB_ADDR(0X19), + + /* Euler data registers */ + BNO055_EULER_H_LSB_ADDR(0X1A), BNO055_EULER_H_MSB_ADDR( + 0X1B), BNO055_EULER_R_LSB_ADDR(0X1C), BNO055_EULER_R_MSB_ADDR( + 0X1D), BNO055_EULER_P_LSB_ADDR(0X1E), BNO055_EULER_P_MSB_ADDR(0X1F), + + /* Quaternion data registers */ + BNO055_QUATERNION_DATA_W_LSB_ADDR(0X20), BNO055_QUATERNION_DATA_W_MSB_ADDR( + 0X21), BNO055_QUATERNION_DATA_X_LSB_ADDR( + 0X22), BNO055_QUATERNION_DATA_X_MSB_ADDR( + 0X23), BNO055_QUATERNION_DATA_Y_LSB_ADDR( + 0X24), BNO055_QUATERNION_DATA_Y_MSB_ADDR( + 0X25), BNO055_QUATERNION_DATA_Z_LSB_ADDR( + 0X26), BNO055_QUATERNION_DATA_Z_MSB_ADDR(0X27), + + /* Linear acceleration data registers */ + BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR( + 0X28), BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR( + 0X29), BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR( + 0X2A), BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR( + 0X2B), BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR( + 0X2C), BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR(0X2D), + + /* Gravity data registers */ + BNO055_GRAVITY_DATA_X_LSB_ADDR(0X2E), BNO055_GRAVITY_DATA_X_MSB_ADDR( + 0X2F), BNO055_GRAVITY_DATA_Y_LSB_ADDR( + 0X30), BNO055_GRAVITY_DATA_Y_MSB_ADDR( + 0X31), BNO055_GRAVITY_DATA_Z_LSB_ADDR( + 0X32), BNO055_GRAVITY_DATA_Z_MSB_ADDR(0X33), + + /* Temperature data register */ + BNO055_TEMP_ADDR(0X34), + + /* Status registers */ + BNO055_CALIB_STAT_ADDR(0X35), BNO055_SELFTEST_RESULT_ADDR( + 0X36), BNO055_INTR_STAT_ADDR(0X37), + + BNO055_SYS_CLK_STAT_ADDR(0X38), BNO055_SYS_STAT_ADDR( + 0X39), BNO055_SYS_ERR_ADDR(0X3A), + + /* Unit selection register */ + BNO055_UNIT_SEL_ADDR(0X3B), BNO055_DATA_SELECT_ADDR(0X3C), + + /* Mode registers */ + BNO055_OPR_MODE_ADDR(0X3D), BNO055_PWR_MODE_ADDR(0X3E), + + BNO055_SYS_TRIGGER_ADDR(0X3F), BNO055_TEMP_SOURCE_ADDR(0X40), + + /* Axis remap registers */ + BNO055_AXIS_MAP_CONFIG_ADDR(0X41), BNO055_AXIS_MAP_SIGN_ADDR(0X42), + + /* SIC registers */ + BNO055_SIC_MATRIX_0_LSB_ADDR(0X43), BNO055_SIC_MATRIX_0_MSB_ADDR( + 0X44), BNO055_SIC_MATRIX_1_LSB_ADDR(0X45), BNO055_SIC_MATRIX_1_MSB_ADDR( + 0X46), BNO055_SIC_MATRIX_2_LSB_ADDR( + 0X47), BNO055_SIC_MATRIX_2_MSB_ADDR( + 0X48), BNO055_SIC_MATRIX_3_LSB_ADDR( + 0X49), BNO055_SIC_MATRIX_3_MSB_ADDR( + 0X4A), BNO055_SIC_MATRIX_4_LSB_ADDR( + 0X4B), BNO055_SIC_MATRIX_4_MSB_ADDR( + 0X4C), BNO055_SIC_MATRIX_5_LSB_ADDR( + 0X4D), BNO055_SIC_MATRIX_5_MSB_ADDR( + 0X4E), BNO055_SIC_MATRIX_6_LSB_ADDR( + 0X4F), BNO055_SIC_MATRIX_6_MSB_ADDR( + 0X50), BNO055_SIC_MATRIX_7_LSB_ADDR( + 0X51), BNO055_SIC_MATRIX_7_MSB_ADDR( + 0X52), BNO055_SIC_MATRIX_8_LSB_ADDR( + 0X53), BNO055_SIC_MATRIX_8_MSB_ADDR( + 0X54), + + /* Accelerometer Offset registers */ + ACCEL_OFFSET_X_LSB_ADDR(0X55), ACCEL_OFFSET_X_MSB_ADDR( + 0X56), ACCEL_OFFSET_Y_LSB_ADDR(0X57), ACCEL_OFFSET_Y_MSB_ADDR( + 0X58), ACCEL_OFFSET_Z_LSB_ADDR(0X59), ACCEL_OFFSET_Z_MSB_ADDR(0X5A), + + /* Magnetometer Offset registers */ + MAG_OFFSET_X_LSB_ADDR(0X5B), MAG_OFFSET_X_MSB_ADDR( + 0X5C), MAG_OFFSET_Y_LSB_ADDR(0X5D), MAG_OFFSET_Y_MSB_ADDR( + 0X5E), MAG_OFFSET_Z_LSB_ADDR(0X5F), MAG_OFFSET_Z_MSB_ADDR(0X60), + + /* Gyroscope Offset register s */ + GYRO_OFFSET_X_LSB_ADDR(0X61), GYRO_OFFSET_X_MSB_ADDR( + 0X62), GYRO_OFFSET_Y_LSB_ADDR(0X63), GYRO_OFFSET_Y_MSB_ADDR( + 0X64), GYRO_OFFSET_Z_LSB_ADDR(0X65), GYRO_OFFSET_Z_MSB_ADDR(0X66), + + /* Radius registers */ + ACCEL_RADIUS_LSB_ADDR(0X67), ACCEL_RADIUS_MSB_ADDR( + 0X68), MAG_RADIUS_LSB_ADDR(0X69), MAG_RADIUS_MSB_ADDR(0X6A); + + private final int val; + + reg_t(int val) { + this.val = val; + } + + public int getVal() { + return val; + } + }; + + public enum powermode_t { + POWER_MODE_NORMAL(0X00), POWER_MODE_LOWPOWER(0X01), POWER_MODE_SUSPEND( + 0X02); + + private final int val; + + powermode_t(int val) { + this.val = val; + } + + public int getVal() { + return val; + } + }; + + public enum opmode_t { + /* Operation mode settings */ + OPERATION_MODE_CONFIG(0X00), OPERATION_MODE_ACCONLY( + 0X01), OPERATION_MODE_MAGONLY(0X02), OPERATION_MODE_GYRONLY( + 0X03), OPERATION_MODE_ACCMAG(0X04), OPERATION_MODE_ACCGYRO( + 0X05), OPERATION_MODE_MAGGYRO( + 0X06), OPERATION_MODE_AMG(0X07), OPERATION_MODE_IMUPLUS( + 0X08), OPERATION_MODE_COMPASS(0X09), OPERATION_MODE_M4G( + 0X0A), OPERATION_MODE_NDOF_FMC_OFF( + 0X0B), OPERATION_MODE_NDOF(0X0C); + + private final int val; + + opmode_t(int val) { + this.val = val; + } + + public int getVal() { + return val; + } + } + + public class RevInfo { + public byte accel_rev; + public byte mag_rev; + public byte gyro_rev; + public short sw_rev; + public byte bl_rev; + } + + public class CalData { + public byte sys; + public byte gyro; + public byte accel; + public byte mag; + } + + public enum vector_type_t { + VECTOR_ACCELEROMETER( + reg_t.BNO055_ACCEL_DATA_X_LSB_ADDR.getVal()), VECTOR_MAGNETOMETER( + reg_t.BNO055_MAG_DATA_X_LSB_ADDR.getVal()), VECTOR_GYROSCOPE( + reg_t.BNO055_GYRO_DATA_X_LSB_ADDR.getVal()), VECTOR_EULER( + reg_t.BNO055_EULER_H_LSB_ADDR.getVal()), VECTOR_LINEARACCEL( + reg_t.BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR + .getVal()), VECTOR_GRAVITY( + reg_t.BNO055_GRAVITY_DATA_X_LSB_ADDR.getVal()); + + private final int val; + + vector_type_t(int val) { + this.val = val; + } + + public int getVal() { + return val; + } + }; + + /** + * Instantiates a new BNO055 class. + * + * @param port + * the physical port the sensor is plugged into on the roboRio + * @param address + * the address the sensor is at (0x28 or 0x29) + */ + private BNO055(I2C.Port port, byte address) { + imu = new I2C(port, address); + + executor = new java.util.Timer(); + executor.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD); + } + + /** + * Get an instance of the IMU object. + * + * @param mode + * the operating mode to run the sensor in. + * @param port + * the physical port the sensor is plugged into on the roboRio + * @param address + * the address the sensor is at (0x28 or 0x29) + * @return the instantiated BNO055 object + */ + public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType, + I2C.Port port, byte address) { + + if (instance == null) { + + instance = new BNO055(port, address); + } + + requestedMode = mode; + requestedVectorType = vectorType; + return instance; + } + + /** + * Get an instance of the IMU object plugged into the onboard I2C header. + * Using the default address (0x28) + * + * @param mode + * the operating mode to run the sensor in. + * @param vectorType + * the format the position vector data should be returned in (if you + * don't know use VECTOR_EULER). + * @return the instantiated BNO055 object + */ + public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType) { + return getInstance(mode, vectorType, I2C.Port.kOnboard, BNO055_ADDRESS_A); + } + + /** + * Called periodically. Communicates with the sensor, and checks its state. + */ + private void update() { + currentTime = Timer.getFPGATimestamp(); // seconds + if (!initialized) { + // System.out.println("State: " + state + ". curr: " + currentTime + // + ", next: " + nextTime); + + // Step through process of initializing the sensor in a non- + // blocking manner. This sequence of events follows the process + // defined in the original adafruit source as closely as possible. + // XXX: It's likely some of these delays can be optimized out. + switch (state) { + case 0: + // Wait for the sensor to be present + if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) != BNO055_ID) { + // Sensor not present, keep trying + sensorPresent = false; + } else { + // Sensor present, go to next state + sensorPresent = true; + state++; + nextTime = Timer.getFPGATimestamp() + 0.050; + } + break; + case 1: + if (currentTime >= nextTime) { + // Switch to config mode (just in case since this is the default) + setMode(opmode_t.OPERATION_MODE_CONFIG.getVal()); + nextTime = Timer.getFPGATimestamp() + 0.050; + state++; + } + break; + case 2: + // Reset + if (currentTime >= nextTime) { + write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x20); + state++; + } + break; + case 3: + // Wait for the sensor to be present + if ((0xFF & read8(reg_t.BNO055_CHIP_ID_ADDR)) == BNO055_ID) { + // Sensor present, go to next state + state++; + // Log current time + nextTime = Timer.getFPGATimestamp() + 0.050; + } + break; + case 4: + // Wait at least 50ms + if (currentTime >= nextTime) { + /* Set to normal power mode */ + write8(reg_t.BNO055_PWR_MODE_ADDR, + (byte) powermode_t.POWER_MODE_NORMAL.getVal()); + nextTime = Timer.getFPGATimestamp() + 0.050; + state++; + } + break; + case 5: + // Use external crystal - 32.768 kHz + if (currentTime >= nextTime) { + write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00); + nextTime = Timer.getFPGATimestamp() + 0.050; + state++; + } + break; + case 6: + if (currentTime >= nextTime) { + write8(reg_t.BNO055_SYS_TRIGGER_ADDR, (byte) 0x80); + nextTime = Timer.getFPGATimestamp() + 0.500; + state++; + } + break; + case 7: + // Set operating mode to mode requested at instantiation + if (currentTime >= nextTime) { + setMode(requestedMode); + nextTime = Timer.getFPGATimestamp() + 1.05; + state++; + } + break; + case 8: + if (currentTime >= nextTime) { + nextTime = Timer.getFPGATimestamp() + 1.05; + state++; + } + case 9: + if (currentTime >= nextTime) { + calculateVector(); + zeroReferenceConst = getDefaultHeading(); + initialized = true; + } + break; + default: + // Should never get here - Fail safe + initialized = false; + } + } else { + // Sensor is initialized, periodically query position data + calculateVector(); + } + } + + /** + * Query the sensor for position data. + */ + private void calculateVector() { + double[] pos = new double[3]; + short x = 0, y = 0, z = 0; + double headingDiff = 0.0; + + // Read vector data (6 bytes) + readLen(requestedVectorType.getVal(), positionVector); + + x = (short) ((positionVector[0] & 0xFF) + | ((positionVector[1] << 8) & 0xFF00)); + y = (short) ((positionVector[2] & 0xFF) + | ((positionVector[3] << 8) & 0xFF00)); + z = (short) ((positionVector[4] & 0xFF) + | ((positionVector[5] << 8) & 0xFF00)); + + /* Convert the value to an appropriate range (section 3.6.4) */ + /* and assign the value to the Vector type */ + switch (requestedVectorType) { + case VECTOR_MAGNETOMETER: + /* 1uT = 16 LSB */ + pos[0] = (x) / 16.0; + pos[1] = (y) / 16.0; + pos[2] = (z) / 16.0; + break; + case VECTOR_GYROSCOPE: + /* 1rps = 900 LSB */ + pos[0] = (x) / 900.0; + pos[1] = (y) / 900.0; + pos[2] = (z) / 900.0; + break; + case VECTOR_EULER: + /* 1 degree = 16 LSB */ + pos[0] = (x) / 16.0; + pos[1] = (y) / 16.0; + pos[2] = (z) / 16.0; + break; + case VECTOR_ACCELEROMETER: + case VECTOR_LINEARACCEL: + case VECTOR_GRAVITY: + /* 1m/s^2 = 100 LSB */ + pos[0] = (x) / 100.0; + pos[1] = (y) / 100.0; + pos[2] = (z) / 100.0; + break; + } + + // calculate turns + headingDiff = xyz[0] - pos[0]; + if (Math.abs(headingDiff) >= 350) { + // We've traveled past the zero heading position + if (headingDiff > 0) { + turns++; + } else { + turns--; + } + } + + // Update position vectors + xyz = pos; + } + + /** + * Puts the chip in the specified operating mode + * + * @param mode + */ + public void setMode(opmode_t mode) { + setMode(mode.getVal()); + } + + private void setMode(int mode) { + _mode = mode; + write8(reg_t.BNO055_OPR_MODE_ADDR, (byte) _mode); + } + + /** + * Gets the latest system status info + * + * @return + */ + public SystemStatus getSystemStatus() { + SystemStatus status = new SystemStatus(); + + write8(reg_t.BNO055_PAGE_ID_ADDR, (byte) 0x00); + + /* + * System Status (see section 4.3.58) --------------------------------- 0 = + * Idle 1 = System Error 2 = Initializing Peripherals 3 = System + * Initalization 4 = Executing Self-Test 5 = Sensor fusion algorithm running + * 6 = System running without fusion algorithms + */ + + status.system_status = read8(reg_t.BNO055_SYS_STAT_ADDR); + + /* + * Self Test Results (see section ) -------------------------------- 1 = + * test passed, 0 = test failed + * + * Bit 0 = Accelerometer self test Bit 1 = Magnetometer self test Bit 2 = + * Gyroscope self test Bit 3 = MCU self test + * + * 0x0F = all good! + */ + + status.self_test_result = read8(reg_t.BNO055_SELFTEST_RESULT_ADDR); + + /* + * System Error (see section 4.3.59) --------------------------------- 0 = + * No error 1 = Peripheral initialization error 2 = System initialization + * error 3 = Self test result failed 4 = Register map value out of range 5 = + * Register map address out of range 6 = Register map write error 7 = BNO + * low power mode not available for selected operation mode 8 = + * Accelerometer power mode not available 9 = Fusion algorithm configuration + * error A = Sensor configuration error + */ + status.system_error = read8(reg_t.BNO055_SYS_ERR_ADDR); + return status; + } + + /** + * Gets the chip revision numbers + * + * @return the chips revision information + */ + public RevInfo getRevInfo() { + int a = 0, b = 0; + RevInfo info = new RevInfo(); + + /* Check the accelerometer revision */ + info.accel_rev = read8(reg_t.BNO055_ACCEL_REV_ID_ADDR); + + /* Check the magnetometer revision */ + info.mag_rev = read8(reg_t.BNO055_MAG_REV_ID_ADDR); + + /* Check the gyroscope revision */ + info.gyro_rev = read8(reg_t.BNO055_GYRO_REV_ID_ADDR); + + /* Check the SW revision */ + info.bl_rev = read8(reg_t.BNO055_BL_REV_ID_ADDR); + + a = read8(reg_t.BNO055_SW_REV_ID_LSB_ADDR); + b = read8(reg_t.BNO055_SW_REV_ID_MSB_ADDR); + info.sw_rev = (short) ((b << 8) | a); + + return info; + } + + /** + * Diagnostic method to determine if communications with the sensor are + * active. Note this method returns true after first establishing + * communications with the sensor. Communications are not actively monitored + * once sensor initialization has started. + * + * @return true if the sensor is found on the I2C bus + */ + public boolean isSensorPresent() { + return sensorPresent; + } + + /** + * After power is applied, the sensor needs to be configured for use. During + * this initialization period the sensor will not return position vector data. + * Once initialization is complete, data can be read, although the sensor may + * not have completed calibration. See isCalibrated. + * + * @return true when the sensor is initialized. + */ + public boolean isInitialized() { + return initialized; + } + + /** + * Gets current calibration state. + * + * @return each value will be set to 0 if not calibrated, 3 if fully + * calibrated. + */ + public CalData getCalibration() { + CalData data = new CalData(); + int rawCalData = read8(reg_t.BNO055_CALIB_STAT_ADDR); + + data.sys = (byte) ((rawCalData >> 6) & 0x03); + data.gyro = (byte) ((rawCalData >> 4) & 0x03); + data.accel = (byte) ((rawCalData >> 2) & 0x03); + data.mag = (byte) (rawCalData & 0x03); + + return data; + } + + /** + * Returns true if all required sensors (accelerometer, magnetometer, + * gyroscope) have completed their respective calibration sequence. Only + * sensors required by the current operating mode are checked. See Section + * 3.3. + * + * @return true if calibration is complete for all sensors required for the + * mode the sensor is currently operating in. + */ + public boolean isCalibrated() { + boolean retVal = true; + + // Per Table 3-3 + boolean[][] sensorModeMap = new boolean[][] { + // {accel, mag, gyro} + { false, false, false }, // OPERATION_MODE_CONFIG + { true, false, false }, // OPERATION_MODE_ACCONLY + { false, true, false }, // OPERATION_MODE_MAGONLY + { false, false, true }, // OPERATION_MODE_GYRONLY + { true, true, false }, // OPERATION_MODE_ACCMAG + { true, false, true }, // OPERATION_MODE_ACCGYRO + { false, true, true }, // OPERATION_MODE_MAGGYRO + { true, true, true }, // OPERATION_MODE_AMG + { true, false, true }, // OPERATION_MODE_IMUPLUS + { true, true, false }, // OPERATION_MODE_COMPASS + { true, true, false }, // OPERATION_MODE_M4G + { true, true, true }, // OPERATION_MODE_NDOF_FMC_OFF + { true, true, true } // OPERATION_MODE_NDOF + }; + + CalData data = getCalibration(); + + if (sensorModeMap[_mode][0]) // Accelerometer used + retVal = retVal && (data.accel >= 3); + if (sensorModeMap[_mode][1]) // Magnetometer used + retVal = retVal && (data.mag >= 3); + if (sensorModeMap[_mode][2]) // Gyroscope used + retVal = retVal && (data.gyro >= 3); + + return retVal; + } + + /** + * Get the sensors internal temperature. + * + * @return temperature in degrees celsius. + */ + public int getTemp() { + return (read8(reg_t.BNO055_TEMP_ADDR)); + } + + /** + * Gets a vector representing the sensors position (heading, roll, pitch). + * heading: 0 to 360 degrees roll: -90 to +90 degrees pitch: -180 to +180 + * degrees + * + * For continuous rotation heading (doesn't roll over between 360/0) see the + * getHeading() method. + * + * Maximum data output rates for Fusion modes - See 3.6.3 + * + * Operating Mode Data Output Rate IMU 100 Hz COMPASS 20 Hz M4G 50 Hz + * NDOF_FMC_OFF 100 Hz NDOF 100 Hz + * + * @return a vector [heading, roll, pitch] + */ + public double[] getVector() { + return xyz; + } + + /** + * The default sensor heading not relative to the starting angle of the robot. + * + * @return + */ + public double getDefaultHeading() { + return xyz[0] + turns * 360; + } + + /** + * The heading of the sensor (x axis) in continuous format relative to the + * initial heading of the robot. Eg rotating the sensor clockwise two full + * rotations will return a value of 720 degrees. The getVector method will + * return heading in a constrained 0 - 360 deg format if required. + * + * @return heading in degrees + */ + public double getHeading() { + double reading = getDefaultHeading(); + + return reading - zeroReferenceConst; + } + + /** + * Writes an 8 bit value over I2C + * + * @param reg + * the register to write the data to + * @param value + * a byte of data to write + * @return whatever I2CJNI.i2CWrite returns. It's not documented in the wpilib + * javadocs! + */ + private boolean write8(reg_t reg, byte value) { + boolean retVal = false; + + retVal = imu.write(reg.getVal(), value); + + return retVal; + } + + /** + * Reads an 8 bit value over I2C + * + * @param reg + * the register to read from. + * @return + */ + private byte read8(reg_t reg) { + byte[] vals = new byte[1]; + + readLen(reg, vals); + return vals[0]; + } + + /** + * Reads the specified number of bytes over I2C + * + * @param reg + * the address to read from + * @param buffer + * to store the read data into + * @return true on success + */ + private boolean readLen(reg_t reg, byte[] buffer) { + return readLen(reg.getVal(), buffer); + } + + /** + * Reads the specified number of bytes over I2C + * + * @param reg + * the address to read from + * @param buffer + * the size of the data to read + * @return true on success + */ + private boolean readLen(int reg, byte[] buffer) { + boolean retVal = true; + + if (buffer == null || buffer.length < 1) { + return false; + } + + retVal = !imu.read(reg, buffer.length, buffer); + + return retVal; + } + + private class BNO055UpdateTask extends TimerTask { + private BNO055 imu; + + private BNO055UpdateTask(BNO055 imu) { + if (imu == null) { + throw new NullPointerException("BNO055 pointer null"); + } + this.imu = imu; + } + + /** + * Called periodically in its own thread + */ + @Override + public void run() { + imu.update(); + } + } +} -- 2.30.2