1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
4 import org
.usfirst
.frc
.team3501
.robot
.utils
.BNO055
;
6 import edu
.wpi
.first
.wpilibj
.I2C
.Port
;
7 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
8 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
10 public class Robot
extends IterativeRobot
{
11 private static DriveTrain driveTrain
;
13 private static BNO055 imu
;
16 public void robotInit() {
17 driveTrain
= DriveTrain
.getDriveTrain();
19 // declares IMU object to the connected port
20 imu
= BNO055
.getInstance(BNO055
.opmode_t
.OPERATION_MODE_IMUPLUS
,
21 BNO055
.vector_type_t
.VECTOR_EULER
, Port
.kOnboard
, (byte) 0x28);
23 // check if the robot is at the reference angle
24 if (imu
.getHeading() == 0) {
25 System
.out
.println("At zero reference");
29 public static DriveTrain
getDriveTrain() {
30 return DriveTrain
.getDriveTrain();
33 public static OI
getOI() {
38 public void autonomousInit() {
43 public void autonomousPeriodic() {
44 Scheduler
.getInstance().run();
50 public void teleopInit() {
55 public void teleopPeriodic() {
56 Scheduler
.getInstance().run();
57 if (imu
.isInitialized()) {
58 System
.out
.println(imu
.getHeading());
60 // System.out.println("Heading: "
61 // + imu.getHeading() + " Init: "
62 // + imu.isInitialized() + " Calib: " + imu.isCalibrated());