Untitled


package org.usfirst.frc.team3501.robot;

import java.util.Arrays;

import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;

public class Robot extends IterativeRobot {

  static I2C Wire = new I2C(Port.kOnboard, 42);

  @Override
  public void robotInit() {

  }

  @Override
  public void disabledInit() {

  }

  @Override
  public void disabledPeriodic() {
    Scheduler.getInstance().run();
  }

  @Override
  public void autonomousInit() {
  }

  @Override
  public void autonomousPeriodic() {
    Scheduler.getInstance().run();
  }

  @Override
  public void teleopInit() {
    System.out.println("start");
    for (int i = 1; i < 128; i++) {
      I2C a = new I2C(I2C.Port.kOnboard, i);
      if (!a.addressOnly()) {
        System.out.println(i);
      } else {
        System.out.println("fail, " + i);
      }
    }
  }

  @Override
  public void teleopPeriodic() {
    Scheduler.getInstance().run();

    // sendMessage();
    // receiveMessage();

  }

  public void sendMessage() {
    String WriteString = "go";
    char[] CharArray = WriteString.toCharArray();
    byte[] WriteData = new byte[CharArray.length];
    for (int i = 0; i < CharArray.length; i++) {
      WriteData[i] = (byte) CharArray[i];
    }
    Wire.transaction(WriteData, WriteData.length, null, 0);
  }

  public void receiveMessage() {
    byte[] dataReceived = new byte[10];

    if (!Wire.transaction(null, 0, dataReceived, dataReceived.length)) {
      System.out.println(Arrays.toString(dataReceived));
    }

  }

  @Override
  public void testPeriodic() {
    LiveWindow.run();
  }
}