// RobotBuilder Version: 1.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.
package org.usfirst.frc330.Beachbot2014Java.subsystems;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.I2C;
import org.usfirst.frc330.Beachbot2014Java.RobotMap;
import org.usfirst.frc330.Beachbot2014Java.commands.*;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
 *
 */
public class LEDs extends Subsystem {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
    final int ArduinoAddress = 33<<1;
    
    I2C arduino = new I2C(DigitalModule.getInstance(1),ArduinoAddress);  
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    public void initDefaultCommand() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
        setDefaultCommand(new HandleLEDs());
    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DEFAULT_COMMAND
	
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    int data = 0;
    byte[] receivedData = new byte[4];
    
    public void readSmartDashboard() {
        
        data = (int) SmartDashboard.getNumber("Arduino/Value",0);
        if (data != 0) {
            receivedData[0] = (byte)((data & 0xFF000000) >> 24);
            receivedData[1] = (byte)((data & 0x00FF0000) >> 16);
            receivedData[2] = (byte)((data & 0x0000FF00) >> 8);
            receivedData[3] = (byte)((data & 0x000000FF));
            System.out.println("ReceivedData: " + data + " " + receivedData[0] + " " + receivedData[1] + " " + receivedData[2] + " " + receivedData[3]);
            arduino.transaction(receivedData, receivedData.length, null, 0);
            SmartDashboard.putNumber("Arduino/Value",0);
        }
    }
}

@@ -1131,24 +1131,167 @@
 ss);  %0A    %0A
+    public LEDs() %7B%0A        prevData%5B0%5D = 1;%0A        prevData%5B1%5D = -1;%0A        prevData%5B2%5D = 114;%0A        prevData%5B3%5D = 0;%0A        %0A    %7D%0A    %0A
     // Put m
@@ -1328,16 +1328,16 @@
 bsystem%0A
-
     // h
@@ -1777,16 +1777,51 @@
 yte%5B4%5D;%0A
+    byte%5B%5D prevData = new byte%5B4%5D;%0A
     %0A   
@@ -2500,22 +2500,458 @@
 0);%0A        
-%7D
+    if (receivedData%5B0%5D == 1)%0A                prevData = (byte%5B%5D) receivedData.clone();%0A        %7D%0A    %7D%0A    %0A    public void setRed() %7B%0A        receivedData%5B0%5D = 1;%0A        receivedData%5B1%5D = -1;%0A        receivedData%5B2%5D = 0;%0A        receivedData%5B3%5D = 0;%0A        arduino.transaction(receivedData, receivedData.length, null, 0);%0A    %7D%0A    %0A    public void restorePrevious() %7B%0A        arduino.transaction(prevData, prevData.length, null, 0);
 %0A    %7D%0A%7D%0A

add method for setting LEDs to red and restoring previous command