And heres my class code
package frc.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import com.revrobotics.ColorSensorV3;
import com.revrobotics.ColorMatchResult;
import com.revrobotics.ColorMatch;
/**
-
This is a simple example to show how the REV Color Sensor V3 can be used to
-
detect pre-configured colors.
*/
final public class ColorSensor extends TimedRobot {
private static ColorSensor instance_;
final public static ColorSensor getInstance() {
if(instance_ == null) {
instance_ = new ColorSensor();
}
return instance_ ;
}
private final I2C.Port i2cPort = I2C.Port.kOnboard;
/**
-
A Rev Color Sensor V3 object is constructed with an I2C port as a
-
parameter. The device will be automatically initialized with default
-
parameters.
*/
private final ColorSensorV3 m_colorSensor = new ColorSensorV3(i2cPort);
/**
-
A Rev Color Match object is used to register and detect known colors. This can
-
be calibrated ahead of time or during operation.
-
This object uses a simple euclidian distance to estimate the closest match
-
with given confidence range.
*/
private final ColorMatch m_colorMatcher = new ColorMatch();
private final Color kBlueTarget = ColorMatch.makeColor(0.143, 0.427, 0.429);
private final Color kGreenTarget = ColorMatch.makeColor(0.197, 0.561, 0.240);
private final Color kRedTarget = ColorMatch.makeColor(0.561, 0.232, 0.114);
private final Color kYellowTarget = ColorMatch.makeColor(0.361, 0.524, 0.113);
@Override
final public void robotInit() {
m_colorMatcher.addColorMatch(kBlueTarget);
m_colorMatcher.addColorMatch(kGreenTarget);
m_colorMatcher.addColorMatch(kRedTarget);
m_colorMatcher.addColorMatch(kYellowTarget);
}
@Override
final public void robotPeriodic() {
/**
* The method GetColor() returns a normalized color value from the sensor and can be
* useful if outputting the color to an RGB LED or similar. To
* read the raw color, use GetRawColor().
*
* The color sensor works best when within a few inches from an object in
* well lit conditions (the built in LED is a big help here!). The farther
* an object is the more light from the surroundings will bleed into the
* measurements and make it difficult to accurately determine its color.
*/
Color detectedColor = m_colorSensor.getColor();
/**
* Run the color match algorithm on our detected color
*/
String colorString;
ColorMatchResult match = m_colorMatcher.matchClosestColor(detectedColor);
if (match.color == kBlueTarget) {
colorString = "Blue";
} else if (match.color == kRedTarget) {
colorString = "Red";
} else if (match.color == kGreenTarget) {
colorString = "Green";
} else if (match.color == kYellowTarget) {
colorString = "Yellow";
} else {
colorString = "Unknown";
}
/**
* Open Smart Dashboard or Shuffleboard to see the color detected by the
* sensor.
*/
SmartDashboard.putNumber("Red", detectedColor.red);
SmartDashboard.putNumber("Green", detectedColor.green);
SmartDashboard.putNumber("Blue", detectedColor.blue);
SmartDashboard.putNumber("Confidence", match.confidence);
SmartDashboard.putString("Detected Color", colorString);
}
and here is my robot code
package frc.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import com.revrobotics.ColorMatchResult;
import com.revrobotics.ColorSensorV3;
import com.revrobotics.ColorMatch;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
/**
-
The VM is configured to automatically run this class, and to call the
-
functions corresponding to each mode, as described in the TimedRobot
-
documentation. If you change the name of this class or the package after
-
creating this project, you must also update the build.gradle file in the
-
project.
*/
final public class Robot extends TimedRobot {
private static final String kDefaultAuto = “Default”;
private static final String kCustomAuto = “My Auto”;
private String m_autoSelected;
private final SendableChooser m_chooser = new SendableChooser<>();
private I2C.Port i2cPort = I2C.Port.kOnboard;
/**
-
This function is run when the robot is first started up and should be
-
used for any initialization code.
*/
@Override
final public void robotInit() {
m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
m_chooser.addOption("My Auto", kCustomAuto);
SmartDashboard.putData("Auto choices", m_chooser);
}
@Override
final public void teleopPeriodic() {
}
/**
-
This function is called every robot packet, no matter the mode. Use
-
this for items like diagnostics that you want ran during disabled,
-
autonomous, teleoperated and test.
-
This runs after the mode specific periodic functions, but before
-
LiveWindow and SmartDashboard integrated updating.
*/
private final ColorMatch m_colorMatcher = new ColorMatch();
private final ColorSensorV3 m_colorSensor = new ColorSensorV3(i2cPort);
Color detectedColor = m_colorSensor.getColor();
String colorString;
@Override
final public void robotPeriodic() {
ColorMatchResult match = m_colorMatcher.matchClosestColor(detectedColor);
SmartDashboard.putNumber("Red", detectedColor.red);
SmartDashboard.putNumber("Green", detectedColor.green);
SmartDashboard.putNumber("Blue", detectedColor.blue);
SmartDashboard.putNumber("Confidence", match.confidence);
SmartDashboard.putString("Detected Color", colorString);
}
/**
-
This autonomous (along with the chooser code above) shows how to select
-
between different autonomous modes using the dashboard. The sendable
-
chooser code works with the Java SmartDashboard. If you prefer the
-
LabVIEW Dashboard, remove all of the chooser code and uncomment the
-
getString line to get the auto name from the text box below the Gyro
-
You can add additional auto modes by adding additional comparisons to
-
the switch structure below with additional strings. If using the
-
SendableChooser make sure to add them to the chooser code above as well.
*/
@Override
public void autonomousInit() {
m_autoSelected = m_chooser.getSelected();
// m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
System.out.println("Auto selected: " + m_autoSelected);
}
/**
- This function is called periodically during autonomous.
/**
- This function is called periodically during operator control.
*/
}