diff --git a/build.gradle b/build.gradle index 489820a..ce266fe 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.1.1" + id 'com.diffplug.spotless' version '6.20.0' } java { @@ -77,7 +78,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE @@ -92,3 +97,37 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + + +spotless { + java { + target fileTree('.') { + include '**/*.java' + exclude '**/build/**', '**/build-*/**' + } + toggleOffOn() + googleJavaFormat() + removeUnusedImports() + trimTrailingWhitespace() + endWithNewline() + } + groovyGradle { + target fileTree('.') { + include '**/*.gradle' + exclude '**/build/**', '**/build-*/**' + } + greclipse() + indentWithSpaces(4) + trimTrailingWhitespace() + endWithNewline() + } + format 'misc', { + target fileTree('.') { + include '**/*.md', '**/.gitignore' + exclude '**/build/**', '**/build-*/**' + } + trimTrailingWhitespace() + indentWithSpaces(2) + endWithNewline() + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c03fa0e..4fd03b3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -53,17 +53,17 @@ public static class Physical { public static final double SWERVE_DRIVE_GEAR_RATIO = 6.75; public static final double SWERVE_DRIVE_WHEEL_RADIUS = 0.1; - public static final double SWERVE_DRIVE_WHEEL_CIRCUMFERENCE = 2 * Math.PI * SWERVE_DRIVE_WHEEL_RADIUS; - + public static final double SWERVE_DRIVE_WHEEL_CIRCUMFERENCE = + 2 * Math.PI * SWERVE_DRIVE_WHEEL_RADIUS; public static final double TALON_TICKS_PER_REVOLUTION = 2048; - public static final SwerveDriveKinematics SWERVE_KINEMATICS = new SwerveDriveKinematics( - new Translation2d(0.292, 0.292), - new Translation2d(0.292, -0.292), - new Translation2d(-0.292, -0.292), - new Translation2d(-0.292, 0.292) - ); + public static final SwerveDriveKinematics SWERVE_KINEMATICS = + new SwerveDriveKinematics( + new Translation2d(0.292, 0.292), + new Translation2d(0.292, -0.292), + new Translation2d(-0.292, -0.292), + new Translation2d(-0.292, 0.292)); } public static class Offsets { @@ -71,7 +71,6 @@ public static class Offsets { public static final double ENCODER_FRONT_RIGHT = -58; public static final double ENCODER_BACK_RIGHT = 10; public static final double ENCODER_BACK_LEFT = 43; - } public static class Ports { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5441e8a..ab185a0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -3,8 +3,6 @@ package frc.robot; -import com.ctre.phoenix6.Orchestra; -import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -20,10 +18,8 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; - /* Talon FXs to play music through. - More complex music MIDIs will contain several tracks, requiring multiple instruments. */ - - + /* Talon FXs to play music through. + More complex music MIDIs will contain several tracks, requiring multiple instruments. */ /** * This function is run when the robot is first started up and should be used for any diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b43a96..bb8a192 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,24 +5,21 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; - import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller; import frc.robot.commands.shooter.Shooter; import frc.robot.commands.swerve.BrakeSwerve; -import frc.robot.commands.swerve.ResetGyro; import frc.robot.commands.swerve.ResetPose; import frc.robot.commands.swerve.TeleopDrive; import frc.robot.commands.swerve.ToggleSwerveOrient; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.subsystems.VisionSubsystem; -import frc.robot.subsystems.ShooterSubsystem; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -36,7 +33,8 @@ public class RobotContainer { private final SwerveDriveSubsystem _swerveDrive = new SwerveDriveSubsystem(_visionSubsystem); private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem(); // controllers (for driver and operator) - private final CommandPS4Controller _driveController = new CommandPS4Controller(Constants.Ports.DRIVER_CONTROLLER); + private final CommandPS4Controller _driveController = + new CommandPS4Controller(Constants.Ports.DRIVER_CONTROLLER); // slew rate limiters applied to joysticks private final SlewRateLimiter _driveFilterLeftX = new SlewRateLimiter(4); @@ -55,12 +53,12 @@ public RobotContainer() { NamedCommands.registerCommand("waitCommand", new WaitCommand(3)); NamedCommands.registerCommand("brakeSwerve", brakeSwerve); - _swerveDrive.setDefaultCommand(new TeleopDrive( - _swerveDrive, - () -> -_driveFilterLeftY.calculate(_driveController.getLeftY()), - () -> -_driveFilterLeftX.calculate(_driveController.getLeftX()), - () -> -_driveFilterRightX.calculate(_driveController.getRightX()) - )); + _swerveDrive.setDefaultCommand( + new TeleopDrive( + _swerveDrive, + () -> -_driveFilterLeftY.calculate(_driveController.getLeftY()), + () -> -_driveFilterLeftX.calculate(_driveController.getLeftX()), + () -> -_driveFilterRightX.calculate(_driveController.getRightX()))); // configure trigger bindings configureBindings(); @@ -78,9 +76,12 @@ private void configureBindings() { _driveController.cross().whileTrue(new BrakeSwerve(_swerveDrive, 0)); } - /** @return The Command to schedule for auton. */ + /** + * @return The Command to schedule for auton. + */ public Command getAutonCommand() { - _swerveDrive.fieldOriented = false; // make sure swerve is robot-relative for pathplanner to work + _swerveDrive.fieldOriented = + false; // make sure swerve is robot-relative for pathplanner to work return autonChooser.getSelected(); } diff --git a/src/main/java/frc/robot/commands/swerve/BrakeSwerve.java b/src/main/java/frc/robot/commands/swerve/BrakeSwerve.java index 4257cd7..9a26d0f 100644 --- a/src/main/java/frc/robot/commands/swerve/BrakeSwerve.java +++ b/src/main/java/frc/robot/commands/swerve/BrakeSwerve.java @@ -48,9 +48,7 @@ public void execute() { new SwerveModuleState(0, Rotation2d.fromDegrees(-45)) }; - _swerveDrive.setStates( - states - ); + _swerveDrive.setStates(states); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/swerve/ResetGyro.java b/src/main/java/frc/robot/commands/swerve/ResetGyro.java index ebc74ed..74afafd 100644 --- a/src/main/java/frc/robot/commands/swerve/ResetGyro.java +++ b/src/main/java/frc/robot/commands/swerve/ResetGyro.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.SwerveDriveSubsystem; - /** * @author Elvis Osmanov */ diff --git a/src/main/java/frc/robot/commands/swerve/TeleopDrive.java b/src/main/java/frc/robot/commands/swerve/TeleopDrive.java index 302641b..dca0b38 100644 --- a/src/main/java/frc/robot/commands/swerve/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swerve/TeleopDrive.java @@ -3,13 +3,12 @@ package frc.robot.commands.swerve; -import java.util.function.DoubleSupplier; - import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.SwerveDriveSubsystem; import frc.robot.utils.UtilFuncs; +import java.util.function.DoubleSupplier; /** * Drive the swerve chassis based on teleop joystick input @@ -26,7 +25,11 @@ public class TeleopDrive extends Command { private final DoubleSupplier _rotationSpeed; /** Creates a new TeleopDrive. */ - public TeleopDrive(SwerveDriveSubsystem swerveDrive, DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotationSpeed) { + public TeleopDrive( + SwerveDriveSubsystem swerveDrive, + DoubleSupplier xSpeed, + DoubleSupplier ySpeed, + DoubleSupplier rotationSpeed) { _swerveDrive = swerveDrive; _xSpeed = xSpeed; @@ -40,8 +43,7 @@ public TeleopDrive(SwerveDriveSubsystem swerveDrive, DoubleSupplier xSpeed, Doub // Called when the command is initially scheduled. @Override - public void initialize() { - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override @@ -52,11 +54,11 @@ public void execute() { double rotationSpeed = UtilFuncs.ApplyDeadband(_rotationSpeed.getAsDouble(), 0.1); // drive the swerve chassis subsystem - _swerveDrive.driveChassis(new ChassisSpeeds( - xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, - rotationSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED - )); + _swerveDrive.driveChassis( + new ChassisSpeeds( + xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF, + rotationSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b01d2dc..44efac2 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,37 +3,36 @@ package frc.robot.subsystems; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkLowLevel.MotorType; - /** * @author Elvis Osmanov * @author Peleh Liu * @author Cherine Soewingjo */ public class ShooterSubsystem extends SubsystemBase { - private CANSparkMax _leftMotor = new CANSparkMax(Constants.CAN.SHOOTER_LEFT, MotorType.kBrushless); - private CANSparkMax _rightMotor = new CANSparkMax(Constants.CAN.SHOOTER_RIGHT, MotorType.kBrushless); + private CANSparkMax _leftMotor = + new CANSparkMax(Constants.CAN.SHOOTER_LEFT, MotorType.kBrushless); + private CANSparkMax _rightMotor = + new CANSparkMax(Constants.CAN.SHOOTER_RIGHT, MotorType.kBrushless); /** Creates a new ShooterSubsystem. */ - public ShooterSubsystem() { - } - + public ShooterSubsystem() {} @Override public void periodic() { // This method will be called once per scheduler run } - public void spinMotor(){ + public void spinMotor() { _leftMotor.set(-1.0); _rightMotor.set(1.0); } - public void stopMotors(){ + public void stopMotors() { _leftMotor.set(0); _rightMotor.set(0); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 8a2bde2..c50e1f3 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -3,16 +3,11 @@ package frc.robot.subsystems; -import java.util.ArrayList; -import java.util.Optional; - import com.ctre.phoenix6.Orchestra; -import com.ctre.phoenix6.hardware.TalonFX; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; - import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -22,14 +17,15 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.BuiltInAccelerometer; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; 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; import frc.robot.utils.BNO055; import frc.robot.utils.SwerveModule; +import java.util.Optional; /** * @author Peter Gutkovich @@ -39,25 +35,48 @@ */ public class SwerveDriveSubsystem extends SubsystemBase { // each swerve module - private final SwerveModule _frontLeft = new SwerveModule(Constants.CAN.DRIVE_FRONT_LEFT, Constants.CAN.ROT_FRONT_LEFT, - Constants.CAN.ENC_FRONT_LEFT, Constants.Offsets.ENCODER_FRONT_LEFT, 0.015, 0.15); - private final SwerveModule _frontRight = new SwerveModule(Constants.CAN.DRIVE_FRONT_RIGHT, - Constants.CAN.ROT_FRONT_RIGHT, Constants.CAN.ENC_FRONT_RIGHT, Constants.Offsets.ENCODER_FRONT_RIGHT, 0.015, 0.17); - private final SwerveModule _backRight = new SwerveModule(Constants.CAN.DRIVE_BACK_RIGHT, Constants.CAN.ROT_BACK_RIGHT, - Constants.CAN.ENC_BACK_RIGHT, Constants.Offsets.ENCODER_BACK_RIGHT, 0.015, 0.18); - private final SwerveModule _backLeft = new SwerveModule(Constants.CAN.DRIVE_BACK_LEFT, Constants.CAN.ROT_BACK_LEFT, - Constants.CAN.ENC_BACK_LEFT, Constants.Offsets.ENCODER_BACK_LEFT, 0.015, 0.17); - - private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, - BNO055.vector_type_t.VECTOR_EULER); - + private final SwerveModule _frontLeft = + new SwerveModule( + Constants.CAN.DRIVE_FRONT_LEFT, + Constants.CAN.ROT_FRONT_LEFT, + Constants.CAN.ENC_FRONT_LEFT, + Constants.Offsets.ENCODER_FRONT_LEFT, + 0.015, + 0.15); + private final SwerveModule _frontRight = + new SwerveModule( + Constants.CAN.DRIVE_FRONT_RIGHT, + Constants.CAN.ROT_FRONT_RIGHT, + Constants.CAN.ENC_FRONT_RIGHT, + Constants.Offsets.ENCODER_FRONT_RIGHT, + 0.015, + 0.17); + private final SwerveModule _backRight = + new SwerveModule( + Constants.CAN.DRIVE_BACK_RIGHT, + Constants.CAN.ROT_BACK_RIGHT, + Constants.CAN.ENC_BACK_RIGHT, + Constants.Offsets.ENCODER_BACK_RIGHT, + 0.015, + 0.18); + private final SwerveModule _backLeft = + new SwerveModule( + Constants.CAN.DRIVE_BACK_LEFT, + Constants.CAN.ROT_BACK_LEFT, + Constants.CAN.ENC_BACK_LEFT, + Constants.Offsets.ENCODER_BACK_LEFT, + 0.015, + 0.17); + + private final BNO055 _gyro = + BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, BNO055.vector_type_t.VECTOR_EULER); private VisionSubsystem _visionSubsystem; private double _robotSpeed = 0; Orchestra _orchestra; - String song = "output.chrp"; + String song = "output.chrp"; // estimated pose private Pose2d _pose = new Pose2d(); @@ -70,19 +89,19 @@ public class SwerveDriveSubsystem extends SubsystemBase { private final BuiltInAccelerometer _imu = new BuiltInAccelerometer(); // Pose Estimator -> Has built in odometry and uses supplied vision measurements - private final SwerveDrivePoseEstimator _estimator = new SwerveDrivePoseEstimator( - Constants.Physical.SWERVE_KINEMATICS, - getHeadingRaw(), - new SwerveModulePosition[] { - _frontLeft.getPosition(), - _frontRight.getPosition(), - _backRight.getPosition(), - _backLeft.getPosition() - }, - new Pose2d(), - VecBuilder.fill(0.008, 0.008, 0.0075), - VecBuilder.fill(0.2, .2, .75) - ); + private final SwerveDrivePoseEstimator _estimator = + new SwerveDrivePoseEstimator( + Constants.Physical.SWERVE_KINEMATICS, + getHeadingRaw(), + new SwerveModulePosition[] { + _frontLeft.getPosition(), + _frontRight.getPosition(), + _backRight.getPosition(), + _backLeft.getPosition() + }, + new Pose2d(), + VecBuilder.fill(0.008, 0.008, 0.0075), + VecBuilder.fill(0.2, .2, .75)); /** Return the estimated pose of the swerve chassis. */ public Pose2d getPose() { @@ -92,11 +111,7 @@ public Pose2d getPose() { /** Get the drive's chassis speeds (robot relative). */ public ChassisSpeeds getRobotRelativeSpeeds() { return Constants.Physical.SWERVE_KINEMATICS.toChassisSpeeds( - _frontLeft.getState(), - _frontRight.getState(), - _backRight.getState(), - _backLeft.getState() - ); + _frontLeft.getState(), _frontRight.getState(), _backRight.getState(), _backLeft.getState()); } /** Creates a new SwerveDriveSubsystem. */ @@ -110,38 +125,36 @@ public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) { modules[3] = _backLeft; for (int i = 0; i < modules.length; i++) { - for (int j = 0; j < 2; j++){ - _orchestra.addInstrument(modules[i].returnTalons()[j]); - } - } + for (int j = 0; j < 2; j++) { + _orchestra.addInstrument(modules[i].returnTalons()[j]); + } + } _orchestra.loadMusic(song); _orchestra.play(); - + // pathplannerlib setup AutoBuilder.configureHolonomic( - this::getPose, - this::resetPose, - this::getRobotRelativeSpeeds, - this::driveChassis, - new HolonomicPathFollowerConfig( - new PIDConstants(2.5, 0, 0), - new PIDConstants(5.0, 0, 0), - Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, - Constants.Physical.SWERVE_DRIVE_BASE_RADIUS, - new ReplanningConfig() - ), - () -> { - Optional alliance = DriverStation.getAlliance(); - - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this - ); + this::getPose, + this::resetPose, + this::getRobotRelativeSpeeds, + this::driveChassis, + new HolonomicPathFollowerConfig( + new PIDConstants(2.5, 0, 0), + new PIDConstants(5.0, 0, 0), + Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, + Constants.Physical.SWERVE_DRIVE_BASE_RADIUS, + new ReplanningConfig()), + () -> { + Optional alliance = DriverStation.getAlliance(); + + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this); } @Override @@ -159,16 +172,18 @@ public void periodic() { SmartDashboard.putNumber("Front Left Velocity", _frontLeft.getDriveVelocity()); SmartDashboard.putNumber("Front Right Velocity", _frontRight.getDriveVelocity()); SmartDashboard.putNumber("Back Left Velocity", _backLeft.getDriveVelocity()); - SmartDashboard.putNumber("Back Right Velocity", _backRight.getDriveVelociy()); - + SmartDashboard.putNumber("Back Right Velocity", _backRight.getDriveVelocity()); // Update the bot's pose - _pose = _estimator.update(getHeadingRaw(), new SwerveModulePosition[] { - _frontLeft.getPosition(), - _frontRight.getPosition(), - _backRight.getPosition(), - _backLeft.getPosition() - }); + _pose = + _estimator.update( + getHeadingRaw(), + new SwerveModulePosition[] { + _frontLeft.getPosition(), + _frontRight.getPosition(), + _backRight.getPosition(), + _backLeft.getPosition() + }); if (_visionSubsystem.isApriltagVisible()) { _estimator.addVisionMeasurement(_visionSubsystem.getBotpose(), Timer.getFPGATimestamp()); @@ -177,10 +192,14 @@ public void periodic() { _field.setRobotPose(_pose); SmartDashboard.putData("FIELD", _field); - _robotSpeed = Math.sqrt(Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2) + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2)); + _robotSpeed = + Math.sqrt( + Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2) + + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2)); - SmartDashboard.putNumber("ANGULAR SPEED", getRobotRelativeSpeeds().omegaRadiansPerSecond / Math.PI); - SmartDashboard.putNumber("DRIVE SPEED (m/s)", _robotSpeed); + SmartDashboard.putNumber( + "ANGULAR SPEED", getRobotRelativeSpeeds().omegaRadiansPerSecond / Math.PI); + SmartDashboard.putNumber("DRIVE SPEED (m/s)", _robotSpeed); SmartDashboard.putNumber("Front Left Module Speed", _frontLeft.getDriveVelocity()); } @@ -188,8 +207,8 @@ public void periodic() { /** * Set the chassis speed of the swerve drive. * - * Chassis speed will be treated as field oriented if the fieldOriented class attribute is set to true, - * otherwise it will be robot-relative. + *

Chassis speed will be treated as field oriented if the fieldOriented class attribute is set + * to true, otherwise it will be robot-relative. * * @see ChassisSpeeds (wpilib chassis speeds class) */ @@ -199,14 +218,15 @@ public void driveChassis(ChassisSpeeds chassisSpeeds) { chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, getHeading()); } - SwerveModuleState[] moduleStates = Constants.Physical.SWERVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); + SwerveModuleState[] moduleStates = + Constants.Physical.SWERVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); setStates(moduleStates); } /** * Sets the state of each SwerveModule through an array. * - * Order -> front left, front right, back right, back left + *

Order -> front left, front right, back right, back left */ public void setStates(SwerveModuleState[] states) { _frontLeft.setState(states[0]); @@ -217,49 +237,42 @@ public void setStates(SwerveModuleState[] states) { /** Resets the pose estimator's heading of the drive to 0. */ public void resetGyro() { - Pose2d new_pose = new Pose2d( - _pose.getTranslation().getX(), - _pose.getTranslation().getY(), - Rotation2d.fromDegrees(0)); + Pose2d new_pose = + new Pose2d( + _pose.getTranslation().getX(), + _pose.getTranslation().getY(), + Rotation2d.fromDegrees(0)); resetPose(new_pose); } /** Resets pose estimator's translation of the drive to (0, 0). */ public void resetTranslation() { - Pose2d new_pose = new Pose2d( - 0, - 0, - _pose.getRotation()); + Pose2d new_pose = new Pose2d(0, 0, _pose.getRotation()); resetPose(new_pose); } /** Resets the pose estimator to the supplied new pose. */ public void resetPose(Pose2d newPose) { - _estimator.resetPosition(getHeadingRaw(), + _estimator.resetPosition( + getHeadingRaw(), new SwerveModulePosition[] { - _frontLeft.getPosition(), - _frontRight.getPosition(), - _backRight.getPosition(), - _backLeft.getPosition() - }, newPose); + _frontLeft.getPosition(), + _frontRight.getPosition(), + _backRight.getPosition(), + _backLeft.getPosition() + }, + newPose); } - /** - * Get heading of the drive from the pose estimator. - */ + /** Get heading of the drive from the pose estimator. */ public Rotation2d getHeading() { return _estimator.getEstimatedPosition().getRotation(); } - /** - * Get heading DIRECTLY from the BNO055 gyro as a Rotation2d. - */ + /** Get heading DIRECTLY from the BNO055 gyro as a Rotation2d. */ public Rotation2d getHeadingRaw() { return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360)); - } - - - + } } diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index 7545645..c5ab209 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -3,14 +3,12 @@ package frc.robot.subsystems; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; - /** * @author Lucas Ou * @author Alex Reyes @@ -24,7 +22,6 @@ public class VisionSubsystem extends SubsystemBase { /** Creates a new VisionSubsystem. */ public VisionSubsystem() {} - @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/utils/BNO055.java b/src/main/java/frc/robot/utils/BNO055.java index a4d82e3..1620e1c 100644 --- a/src/main/java/frc/robot/utils/BNO055.java +++ b/src/main/java/frc/robot/utils/BNO055.java @@ -3,835 +3,801 @@ package frc.robot.utils; -import java.util.TimerTask; - import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.Timer; +import java.util.TimerTask; /** - * BNO055 IMU for the FIRST Robotics Competition. - * References throughout the code are to the following sensor documentation: - * http://git.io/vuOl1 + * 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. + *

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; + *

Example: private static BNO055 imu; * - * public Robot() { - * imu = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, - * BNO055.vector_type_t.VECTOR_EULER); - * } + *

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. + *

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. + *

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. + *

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 - * + *

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]; - - public class SystemStatus { - public int system_status; - public int self_test_result; - public int system_error; + // 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]; + + 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 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 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 enum powermode_t { + POWER_MODE_NORMAL(0X00), + POWER_MODE_LOWPOWER(0X01), + POWER_MODE_SUSPEND(0X02); - public class CalData { - public byte sys; - public byte gyro; - public byte accel; - public byte mag; - } + private final int val; - 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); + powermode_t(int val) { + this.val = val; } - /** - * 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; + public int getVal() { + return val; } - - /** - * 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); + }; + + 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; } - - /** - * 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) { - state++; - } - case 9: - initialized = true; - break; - default: - //Should never get here - Fail safe - initialized = false; - } - } else { - //Sensor is initialized, periodically query position data - calculateVector(); - } + public int getVal() { + return val; } - - /** - * 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] = ((double)x)/16.0; - pos[1] = ((double)y)/16.0; - pos[2] = ((double)z)/16.0; - break; - case VECTOR_GYROSCOPE: - /* 1rps = 900 LSB */ - pos[0] = ((double)x)/900.0; - pos[1] = ((double)y)/900.0; - pos[2] = ((double)z)/900.0; - break; - case VECTOR_EULER: - /* 1 degree = 16 LSB */ - pos[0] = ((double)x)/16.0; - pos[1] = ((double)y)/16.0; - pos[2] = ((double)z)/16.0; - break; - case VECTOR_ACCELEROMETER: - case VECTOR_LINEARACCEL: - case VECTOR_GRAVITY: - /* 1m/s^2 = 100 LSB */ - pos[0] = ((double)x)/100.0; - pos[1] = ((double)y)/100.0; - pos[2] = ((double)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; + } + + 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; } - /** - * Puts the chip in the specified operating mode - * @param mode - */ - public void setMode(opmode_t mode) { - setMode(mode.getVal()); + public int getVal() { + return val; } - - private void setMode(int mode) { - _mode = mode; - write8(reg_t.BNO055_OPR_MODE_ADDR, (byte) _mode); + }; + + /** + * 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); } - - /** - * 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; + 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) { + state++; + } + case 9: + initialized = true; + break; + default: + // Should never get here - Fail safe + initialized = false; + } + } else { + // Sensor is initialized, periodically query position data + calculateVector(); } - - /** - * 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; + } + + /** 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] = ((double) x) / 16.0; + pos[1] = ((double) y) / 16.0; + pos[2] = ((double) z) / 16.0; + break; + case VECTOR_GYROSCOPE: + /* 1rps = 900 LSB */ + pos[0] = ((double) x) / 900.0; + pos[1] = ((double) y) / 900.0; + pos[2] = ((double) z) / 900.0; + break; + case VECTOR_EULER: + /* 1 degree = 16 LSB */ + pos[0] = ((double) x) / 16.0; + pos[1] = ((double) y) / 16.0; + pos[2] = ((double) z) / 16.0; + break; + case VECTOR_ACCELEROMETER: + case VECTOR_LINEARACCEL: + case VECTOR_GRAVITY: + /* 1m/s^2 = 100 LSB */ + pos[0] = ((double) x) / 100.0; + pos[1] = ((double) y) / 100.0; + pos[2] = ((double) z) / 100.0; + break; } - /** - * 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; + // 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--; + } } - /** - * 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 + // 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 heading of the sensor (x axis) in continuous format. 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() { - return xyz[0] + turns * 360; - } - - /** - * 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); + 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 heading of the sensor (x axis) in continuous format. 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() { + return xyz[0] + turns * 360; + } + + /** + * 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; } - /** - * 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; + retVal = !imu.read(reg, buffer.length, buffer); - if (buffer == null || buffer.length < 1) { - return false; - } + return retVal; + } - retVal = !imu.read(reg, buffer.length, buffer); + private class BNO055UpdateTask extends TimerTask { + private BNO055 imu; - return retVal; + private BNO055UpdateTask(BNO055 imu) { + if (imu == null) { + throw new NullPointerException("BNO055 pointer null"); + } + this.imu = imu; } - 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 - */ - public void run() { - imu.update(); - } + /** Called periodically in its own thread */ + public void run() { + imu.update(); } + } } diff --git a/src/main/java/frc/robot/utils/SwerveModule.java b/src/main/java/frc/robot/utils/SwerveModule.java index 105790b..99543b9 100644 --- a/src/main/java/frc/robot/utils/SwerveModule.java +++ b/src/main/java/frc/robot/utils/SwerveModule.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -21,113 +20,133 @@ * @author Elvis Osmanov */ public class SwerveModule { - private final TalonFX _driveMotor; - private final TalonFX _rotationMotor; - - private final PIDController _driveController; - private final PIDController _rotationController; - - private final CANcoder _encoder; - - - - /** - * Represents a single swerve module built with talons for rotation and drive control, and a cancoder for angle. - * @param driveMotorId CAN ID of drive motor. - * @param rotationMotorId CAN ID of rotation motor. - * @param encoderId CAN ID of cancoder. - * @param angleOffset Angle offset to add to the absolute cancoder. - * @param driveP kP for the drive controller. - * @param rotationP kP for the rotation controller. - */ - public SwerveModule(int driveMotorId, int rotationMotorId, int encoderId, double angleOffset, double driveP, double rotationP) { - _driveMotor = new TalonFX(driveMotorId); - _rotationMotor = new TalonFX(rotationMotorId); - - // new stuff because CTRE update - MagnetSensorConfigs encoderConfig = new MagnetSensorConfigs(); - encoderConfig.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - encoderConfig.MagnetOffset = (angleOffset / 180) / 2; - - _encoder = new CANcoder(encoderId); - _encoder.getConfigurator().apply(encoderConfig); - - _driveController = new PIDController(driveP, 0, 0); - - _rotationController = new PIDController(rotationP, 0, 0); - _rotationController.enableContinuousInput(-180, 180); - - TalonFXConfig.configureFalcon(_driveMotor, false); - TalonFXConfig.configureFalcon(_rotationMotor, false); - } - - /** Set the percentage output of the drive motor. */ - public void drive(double speed) { - _driveMotor.set(speed); - } - - /** Set the percentage output of the rotation motor. */ - public void rotate(double speed) { - _rotationMotor.set(speed); - } - - /** Get the absolute angle of the module (-180 to 180 degrees). */ - public double getAngle() { - return _encoder.getAbsolutePosition().getValueAsDouble() * 2 * 180; // ctre update - } - - /** Get the velocity of the drive wheel (meters per second). */ - public double getDriveVelocity() { - double talon_rps = _driveMotor.getRotorVelocity().getValueAsDouble(); // ctre update - - // return the speed of the drive wheel itself (talon rps times gear ratio time wheel size) in m/s - return (talon_rps / Constants.Physical.SWERVE_DRIVE_GEAR_RATIO) * Constants.Physical.SWERVE_DRIVE_WHEEL_CIRCUMFERENCE; - } - - /** Get the position of this module (distance traveled by wheel + angle). - * @see SwerveModulePosition - */ - public SwerveModulePosition getPosition() { - double talon_rotations = _driveMotor.getPosition().getValueAsDouble(); - double distance = (talon_rotations / Constants.Physical.SWERVE_DRIVE_GEAR_RATIO) * Constants.Physical.SWERVE_DRIVE_WHEEL_CIRCUMFERENCE; - - return new SwerveModulePosition( - distance, - Rotation2d.fromDegrees(getAngle()) - ); - } - - /** Set the state of this module. - * @see SwerveModuleState */ - public void setState(SwerveModuleState state) { - // current system for setting the state of a module - // rotation: pure pid control - // velocity: feedforward control mainly along with pid control for small disturbances - - state = SwerveModuleState.optimize(state, new Rotation2d(Math.toRadians(getAngle()))); - double speed = MathUtil.clamp(state.speedMetersPerSecond, -Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); - - double rotation_volts = -MathUtil.clamp(_rotationController.calculate(getAngle(), state.angle.getDegrees()), -1.5, 1.5); - - double drive_pid = _driveController.calculate(getDriveVelocity(), speed); - double drive_output = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); - drive_output += drive_pid; - - rotate(rotation_volts / RobotController.getBatteryVoltage()); - drive(drive_output); - } - - /** Get the state of this module. - * @see SwerveModuleState - */ - public SwerveModuleState getState() { - return new SwerveModuleState(getDriveVelocity(), Rotation2d.fromDegrees(getAngle())); - } - - public TalonFX[] returnTalons(){ - TalonFX[] fxes = new TalonFX[2]; - fxes[0] = _driveMotor; - fxes[1] = _rotationMotor; - return fxes; - } + private final TalonFX _driveMotor; + private final TalonFX _rotationMotor; + + private final PIDController _driveController; + private final PIDController _rotationController; + + private final CANcoder _encoder; + + /** + * Represents a single swerve module built with talons for rotation and drive control, and a + * cancoder for angle. + * + * @param driveMotorId CAN ID of drive motor. + * @param rotationMotorId CAN ID of rotation motor. + * @param encoderId CAN ID of cancoder. + * @param angleOffset Angle offset to add to the absolute cancoder. + * @param driveP kP for the drive controller. + * @param rotationP kP for the rotation controller. + */ + public SwerveModule( + int driveMotorId, + int rotationMotorId, + int encoderId, + double angleOffset, + double driveP, + double rotationP) { + _driveMotor = new TalonFX(driveMotorId); + _rotationMotor = new TalonFX(rotationMotorId); + + // new stuff because CTRE update + MagnetSensorConfigs encoderConfig = new MagnetSensorConfigs(); + encoderConfig.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + encoderConfig.MagnetOffset = (angleOffset / 180) / 2; + + _encoder = new CANcoder(encoderId); + _encoder.getConfigurator().apply(encoderConfig); + + _driveController = new PIDController(driveP, 0, 0); + + _rotationController = new PIDController(rotationP, 0, 0); + _rotationController.enableContinuousInput(-180, 180); + + TalonFXConfig.configureFalcon(_driveMotor, false); + TalonFXConfig.configureFalcon(_rotationMotor, false); + } + + /** Set the percentage output of the drive motor. */ + public void drive(double speed) { + _driveMotor.set(speed); + } + + /** Set the percentage output of the rotation motor. */ + public void rotate(double speed) { + _rotationMotor.set(speed); + } + + /** Get the absolute angle of the module (-180 to 180 degrees). */ + public double getAngle() { + return _encoder.getAbsolutePosition().getValueAsDouble() * 2 * 180; // ctre update + } + + /** Get the velocity of the drive wheel (meters per second). */ + public double getDriveVelocity() { + double talon_rps = _driveMotor.getRotorVelocity().getValueAsDouble(); // ctre update + + // return the speed of the drive wheel itself (talon rps times gear ratio time wheel size) in + // m/s + return (talon_rps / Constants.Physical.SWERVE_DRIVE_GEAR_RATIO) + * Constants.Physical.SWERVE_DRIVE_WHEEL_CIRCUMFERENCE; + } + + /** + * Get the position of this module (distance traveled by wheel + angle). + * + * @see SwerveModulePosition + */ + public SwerveModulePosition getPosition() { + double talon_rotations = _driveMotor.getPosition().getValueAsDouble(); + double distance = + (talon_rotations / Constants.Physical.SWERVE_DRIVE_GEAR_RATIO) + * Constants.Physical.SWERVE_DRIVE_WHEEL_CIRCUMFERENCE; + + return new SwerveModulePosition(distance, Rotation2d.fromDegrees(getAngle())); + } + + /** + * Set the state of this module. + * + * @see SwerveModuleState + */ + public void setState(SwerveModuleState state) { + // current system for setting the state of a module + // rotation: pure pid control + // velocity: feedforward control mainly along with pid control for small disturbances + + state = SwerveModuleState.optimize(state, new Rotation2d(Math.toRadians(getAngle()))); + double speed = + MathUtil.clamp( + state.speedMetersPerSecond, + -Constants.Speeds.SWERVE_DRIVE_MAX_SPEED, + Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); + + double rotation_volts = + -MathUtil.clamp( + _rotationController.calculate(getAngle(), state.angle.getDegrees()), -1.5, 1.5); + + double drive_pid = _driveController.calculate(getDriveVelocity(), speed); + double drive_output = (speed / Constants.Speeds.SWERVE_DRIVE_MAX_SPEED); + drive_output += drive_pid; + + rotate(rotation_volts / RobotController.getBatteryVoltage()); + drive(drive_output); + } + + /** + * Get the state of this module. + * + * @see SwerveModuleState + */ + public SwerveModuleState getState() { + return new SwerveModuleState(getDriveVelocity(), Rotation2d.fromDegrees(getAngle())); + } + + public TalonFX[] returnTalons() { + TalonFX[] fxes = new TalonFX[2]; + fxes[0] = _driveMotor; + fxes[1] = _rotationMotor; + return fxes; + } } diff --git a/src/main/java/frc/robot/utils/TalonFXConfig.java b/src/main/java/frc/robot/utils/TalonFXConfig.java index 1c66b2e..5c99c7e 100644 --- a/src/main/java/frc/robot/utils/TalonFXConfig.java +++ b/src/main/java/frc/robot/utils/TalonFXConfig.java @@ -3,13 +3,11 @@ package frc.robot.utils; - import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; - import frc.robot.Constants; /** @@ -17,50 +15,48 @@ * @author Peter Gutkovich */ -/** - * For configuring Falcons. - */ +/** For configuring Falcons. */ public class TalonFXConfig { - /** - * Basic Falcon config, sets Falcon to factory defaults, sets encoder to 0, - * and sets Falcon deadband and sets Falcon to Brake neutral mode. - * - * @param falcon - The Falcon to config. - */ - public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean invert) { - TalonFXConfiguration config = new TalonFXConfiguration(); - - falcon.getConfigurator().DefaultTimeoutSeconds = Constants.CAN.CAN_TIMEOUT; - // falcon.getConfigurator().apply(config); // FACTORY RESET - - falcon.getConfigurator().refresh(config); - - - config.MotorOutput.DutyCycleNeutralDeadband = 0.01; - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = invert ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; - - // config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; - - falcon.getConfigurator().apply(config); - - return config; - } - - - /** - * Configure a follower of a master Falcon motor. - * - * @param falcon - The follower motor to config. - * @param master - The master motor. - * @param opposeMaster - Boolean for whether the follower motor inverted to the master. - */ - public static void configureDriveFollowerFalcon(TalonFX falcon, TalonFX master, boolean opposeMaster) { - configureFalcon(falcon, false); - falcon.setControl(new Follower(master.getDeviceID(), opposeMaster)); - } + /** + * Basic Falcon config, sets Falcon to factory defaults, sets encoder to 0, and sets Falcon + * deadband and sets Falcon to Brake neutral mode. + * + * @param falcon - The Falcon to config. + */ + public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean invert) { + TalonFXConfiguration config = new TalonFXConfiguration(); + + falcon.getConfigurator().DefaultTimeoutSeconds = Constants.CAN.CAN_TIMEOUT; + // falcon.getConfigurator().apply(config); // FACTORY RESET + + falcon.getConfigurator().refresh(config); + + config.MotorOutput.DutyCycleNeutralDeadband = 0.01; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = + invert ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + // config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; + + falcon.getConfigurator().apply(config); + + return config; + } + + /** + * Configure a follower of a master Falcon motor. + * + * @param falcon - The follower motor to config. + * @param master - The master motor. + * @param opposeMaster - Boolean for whether the follower motor inverted to the master. + */ + public static void configureDriveFollowerFalcon( + TalonFX falcon, TalonFX master, boolean opposeMaster) { + configureFalcon(falcon, false); + falcon.setControl(new Follower(master.getDeviceID(), opposeMaster)); + } } diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java index 100c304..33453e2 100644 --- a/src/main/java/frc/robot/utils/UtilFuncs.java +++ b/src/main/java/frc/robot/utils/UtilFuncs.java @@ -5,17 +5,18 @@ /** Any utility functions for anything. */ public final class UtilFuncs { - /** Applies deadband to a certain value. - * @param val The value to deadband. - * @param deadband The deadband to apply. - * - * @return The new value with deadband applied. - */ - public static double ApplyDeadband(double val, double deadband) { - if (Math.abs(val) > deadband) { - return val; - } - - return 0; + /** + * Applies deadband to a certain value. + * + * @param val The value to deadband. + * @param deadband The deadband to apply. + * @return The new value with deadband applied. + */ + public static double ApplyDeadband(double val, double deadband) { + if (Math.abs(val) > deadband) { + return val; } + + return 0; + } }