package frc.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.I2C.Port;
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 Port i2cPort;
/**
-
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() {
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 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() {
}
/**
-
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.
*/
}
