No matter what we’ve tried, our code:
Subsystem:
package frc.robot.subsystems;
import frc.robot.Robot;
import edu.wpi.first.wpilibj.I2C;
import frc.robot.OI;
import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.smartdashboard.*;
import edu.wpi.first.wpilibj.command.Subsystem;
import frc.robot.commands.ArduinoCommand;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
public class ArduinoSubsystem extends Subsystem {
private static I2C Wire = new I2C(Port.kOnboard, 4);//uses the i2c port on the RoboRIO //uses address 4, must match arduino
private static final int MAX_BYTES = 32;
//String status = "";
public Alliance getAlliance() {
return DriverStation.getInstance().getAlliance();
}
public void write(){//writes to the arduino
String LEDColor = "none";
if (getAlliance() == Alliance.Red) {
LEDColor = "red";
}
if (getAlliance() == Alliance.Blue) {
LEDColor = "blue";
}
if (getAlliance() == Alliance.Invalid) {
LEDColor = "none";
}
LEDColor = "blue";
char[] CharArray = LEDColor.toCharArray();//creates a char array from the input string
byte[] WriteData = new byte[CharArray.length];//creates a byte array from the char array
for (int i = 0; i < CharArray.length; i++) {//writes each byte to the arduino
WriteData[i] = (byte)CharArray[i];//adds the char elements to the byte array
}
Wire.writeBulk(WriteData, WriteData.length);
//Wire.transaction(WriteData, WriteData.length, null, 0);//sends each byte to arduino
}
public Arduino getArduino(){//reads the data from arduino and saves it
String info[] = read().split("\\|");//everytime a "|" is used it splits the data,
Arduino arduino = new Arduino(); //creates a new packet to hold the data
if(info[0].equals("none") || info[0].equals("")){//checks to make sure there is data
arduino.x = -1;//the x val will never be -1 so we can text later in code to make sure
arduino.y = -1;
arduino.area = -1;
if (info.length >= 2) {
arduino.distance = Double.parseDouble(info[1]);
}
}else if(info.length >= 3){//if there is an x, y, and area value the length equals 3
arduino.x = Double.parseDouble(info[0]);//set x
arduino.y = Double.parseDouble(info[1]);//set y
arduino.area = Double.parseDouble(info[2]);//set area
arduino.distance = Double.parseDouble(info[3]);
}
return arduino;
}
public void getData() {
Arduino receive = getArduino();
//System.out.println(receive.distance);
if (OI.leftJoystick.getRawButtonPressed(12)) {
System.out.println("Test:" + receive.x + " | " + receive.y + " | " + receive.area);
System.out.println("Distance: " + receive.distance);
}
}
private String read(){//function to read the data from arduino
byte[] data = new byte[MAX_BYTES];//create a byte array to hold the incoming data
Wire.read(4, MAX_BYTES, data);//use address 4 on i2c and store it in data
String output = new String(data);//create a string from the byte array
int pt = output.indexOf((char)255);
return (String) output.subSequence(0, pt < 0 ? 0 : pt);
}
public void initDefaultCommand() {
setDefaultCommand(new ArduinoCommand());
}
}
Command:
import edu.wpi.first.wpilibj.command.Command;
import frc.robot.Robot;
import frc.robot.RobotMap;
import static frc.robot.OI.*;
public class ArduinoCommand extends Command {
public ArduinoCommand() {
requires(Robot.LED);
}
@Override
protected void initialize() {
}
@Override
protected void execute() {
Robot.LED.write();
}
@Override
protected boolean isFinished() {
return false;
}
@Override
protected void end() {
}
@Override
protected void interrupted() {
}
}
Arduino:
#include <FastLED.h>
#include <SPI.h>
#include <Wire.h>
#define LED_PIN 5
#define NUM_LEDS 300
#define BRIGHTNESS 64
#define LED_TYPE WS2811
#define COLOR_ORDER GRB
CRGB leds[NUM_LEDS];
#define UPDATES_PER_SECOND 100
// This example shows several ways to set up and use 'palettes' of colors
// with FastLED.
//
// These compact palettes provide an easy way to re-colorize your
// animation on the fly, quickly, easily, and with low overhead.
//
// USING palettes is MUCH simpler in practice than in theory, so first just
// run this sketch, and watch the pretty lights as you then read through
// the code. Although this sketch has eight (or more) different color schemes,
// the entire sketch compiles down to about 6.5K on AVR.
//
// FastLED provides a few pre-configured color palettes, and makes it
// extremely easy to make up your own color schemes with palettes.
//
// Some notes on the more abstract 'theory and practice' of
// FastLED compact palettes are at the bottom of this file.
//plug sda on RoboRIO into A4
//plug scl on RoboRIO into A5
//connect the two grounds
String piOutput = "none";//string to be sent to the robot
String input = "blank"; //string received from the robot
String x;
// Get x value from Shuffleboard.
int y = 1;
CRGBPalette16 currentPalette;
TBlendType currentBlending;
extern CRGBPalette16 myRedWhiteBluePalette;
extern const TProgmemPalette16 myRedWhiteBluePalette_p PROGMEM;
void setup() {
Serial.begin(9600);
Wire.begin(4);
Wire.onReceive(receiveEvent);
delay( 3000 ); // power-up safety delay
FastLED.addLeds<LED_TYPE, LED_PIN, COLOR_ORDER>(leds, NUM_LEDS).setCorrection( TypicalLEDStrip );
FastLED.setBrightness( BRIGHTNESS );
x = "none";
currentPalette = RainbowColors_p;
currentBlending = LINEARBLEND;
}
void loop()
{
static uint8_t startIndex = 0;
startIndex = startIndex + 1; /* motion speed */
if (!x.equals("red") && !x.equals("blue")) {
y = 0;
for( int i = 0; y = 1; i++) {
//recieve x from RoboRIO via I2C
leds[i] = CRGB::White;
//leds[i] = CRGB::Green;
//if White turns Red
//change to latter code
FastLED.show();
if (x.equals("red") || x.equals("blue")) {
int y = 1;
}
}
}
else if (x.equals("red")) {
y = 0;
for( int i = 0; y = 1; i++) {
//recieve x from RoboRIO via I2C
leds[i] = CRGB::Red;
FastLED.show();
}
}
else if (x.equals("blue")) {
y = 0;
for( int i = 0; y = 1; i++) {
//recieve x from RoboRIO via I2C
leds[i] = CRGB::Blue;
FastLED.show();
}
}
/*Was rainbow palette but doesn't work,
FillLEDS to UPDATES_PER_SECONDS) };
needs to be in a void loop(), but
a void loop() can't be in another
void loop, which we already have.
So this code only runs red and blue.
while (x.equals("2")) {
FillLEDsFromPaletteColors( startIndex);
FastLED.show();
FastLED.delay(1000 / UPDATES_PER_SECOND);
}*/
}
void FillLEDsFromPaletteColors( uint8_t colorIndex)
{
uint8_t brightness = 255;
}
void ChangePalettePeriodically()
{
uint8_t secondHand = (millis() / 1000) % 60;
static uint8_t lastSecond = 99;
}
void receiveEvent(int bytes) {//called when RoboRIO "gives" this device a message
String LED = "";
while ( Wire.available() > 0 )
{
char n=(char)Wire.read();
if(((int)n)>((int)(' ')))
LED += n;
}
x = LED;
}
const TProgmemPalette16 myRedWhiteBluePalette_p PROGMEM =
{
CRGB::Red,
CRGB::Gray, // 'white' is too bright compared to red and blue
CRGB::Blue,
CRGB::Black,
CRGB::Red,
CRGB::Gray,
CRGB::Blue,
CRGB::Black,
CRGB::Red,
CRGB::Red,
CRGB::Gray,
CRGB::Gray,
CRGB::Blue,
CRGB::Blue,
CRGB::Black,
CRGB::Black
};
Hardware:
LEDs are wired into pwm port 5 on the arduino
The arduino is connected to the roborio by the usb port
i2c: scl = A5, sda = A4