FRC I2C

package org.usfirst.frc.team228.robot;

import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends SampleRobot {
    RobotDrive Drive;
    Joystick stick;
    I2C I2CBus;

    int cX = 0, cY = 0, cZ = 0;
    byte[] compBuffer = new byte[6];

    public Robot() {

        //Drive = new RobotDrive(0, 1);
        //Drive.setExpiration(0.1);
        //stick = new Joystick(0);
        I2CBus = new I2C(I2C.Port.kOnboard, 0x1E);

    }
    // Drive left & right motors for 2 seconds then stop

    public void autonomous() {

    }
    // Runs the motors with arcade steering.

    public void operatorControl() {
        //Drive.setSafetyEnabled(true);

        I2CBus.write(0x02, 1); //select mode register
        I2CBus.write(0x00, 1); //continuous measurement mode


        while (isOperatorControl() && isEnabled()) {

            I2CBus.write(0x03, 1); //select register 3, X MSB register
            I2CBus.read(0x03, 6, compBuffer);  //Read data from each axis, 2 registers per axis

            cX = ((((int)compBuffer[1]) << 8) | compBuffer[0]);
            cY = ((((int)compBuffer[3]) << 8) | compBuffer[2]);
            cZ = ((((int)compBuffer[5]) << 8) | compBuffer[4]);

            SmartDashboard.putNumber("buffer0", ((int)compBuffer[0]));
            SmartDashboard.putNumber("buffer1", ((int)compBuffer[1]));
            SmartDashboard.putNumber("buffer2", ((int)compBuffer[2]));
            SmartDashboard.putNumber("buffer3", ((int)compBuffer[3]));
            SmartDashboard.putNumber("buffer4", ((int)compBuffer[4]));
            SmartDashboard.putNumber("buffer5", ((int)compBuffer[5]));


            SmartDashboard.putNumber("CompX", cX);
            SmartDashboard.putNumber("CompY", cY);
            SmartDashboard.putNumber("CompZ", cZ);


            Timer.delay(0.005);     // wait for a motor update time
        }
    }


    //Runs during test mode

    public void test() {
    }
}