Public Pastes
SHARE
TWEET

FRC I2C

a guest Jan 28th, 2015 423 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. package org.usfirst.frc.team228.robot;
  3.  
  4. import edu.wpi.first.wpilibj.I2C;
  5. import edu.wpi.first.wpilibj.SampleRobot;
  6. import edu.wpi.first.wpilibj.RobotDrive;
  7. import edu.wpi.first.wpilibj.Joystick;
  8. import edu.wpi.first.wpilibj.Timer;
  9. import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  10.  
  11. public class Robot extends SampleRobot {
  12.         RobotDrive Drive;
  13.         Joystick stick;
  14.         I2C I2CBus;
  15.  
  16.         int cX = 0, cY = 0, cZ = 0;
  17.         byte[] compBuffer = new byte[6];    
  18.  
  19.         public Robot() {
  20.  
  21.                 //Drive = new RobotDrive(0, 1);
  22.                 //Drive.setExpiration(0.1);
  23.                 //stick = new Joystick(0);
  24.                 I2CBus = new I2C(I2C.Port.kOnboard, 0x1E);
  25.  
  26.         }
  27.         // Drive left & right motors for 2 seconds then stop
  28.  
  29.         public void autonomous() {
  30.  
  31.         }
  32.         // Runs the motors with arcade steering.
  33.  
  34.         public void operatorControl() {
  35.                 //Drive.setSafetyEnabled(true);
  36.  
  37.                 I2CBus.write(0x02, 1); //select mode register
  38.                 I2CBus.write(0x00, 1); //continuous measurement mode
  39.  
  40.  
  41.                 while (isOperatorControl() && isEnabled()) {
  42.  
  43.                         I2CBus.write(0x03, 1); //select register 3, X MSB register
  44.                         I2CBus.read(0x03, 6, compBuffer);  //Read data from each axis, 2 registers per axis
  45.  
  46.                         cX = ((((int)compBuffer[1]) << 8) | compBuffer[0]);
  47.                         cY = ((((int)compBuffer[3]) << 8) | compBuffer[2]);
  48.                         cZ = ((((int)compBuffer[5]) << 8) | compBuffer[4]);
  49.  
  50.                         SmartDashboard.putNumber("buffer0", ((int)compBuffer[0]));
  51.                         SmartDashboard.putNumber("buffer1", ((int)compBuffer[1]));
  52.                         SmartDashboard.putNumber("buffer2", ((int)compBuffer[2]));
  53.                         SmartDashboard.putNumber("buffer3", ((int)compBuffer[3]));
  54.                         SmartDashboard.putNumber("buffer4", ((int)compBuffer[4]));
  55.                         SmartDashboard.putNumber("buffer5", ((int)compBuffer[5]));
  56.  
  57.  
  58.                         SmartDashboard.putNumber("CompX", cX);
  59.                         SmartDashboard.putNumber("CompY", cY);
  60.                         SmartDashboard.putNumber("CompZ", cZ);
  61.  
  62.  
  63.                         Timer.delay(0.005);             // wait for a motor update time
  64.                 }
  65.         }
  66.  
  67.  
  68.         //Runs during test mode
  69.  
  70.         public void test() {
  71.         }
  72. }
RAW Paste Data
Top