Hello everyone. Team 8051 is trying to detect the amount of spins that the control panel has by using the color sensor to detect color. Basically, we created variables fro the amounts of blues, greens, reds and yellows that it it finds. We then basically used an if statement where if the total of all of the colors counted is 9, then it will count one spin because there are a total of 8 colors plus the one color that would bring the wheel back to the first color. According to the smart board panel, this code works in counting spins when we get rid of the if statement that requires “getRawButton(1)” in the robot Periodic method. The point of the if statement though is to halt the process of counting the colors and spins everything until after the button is pressed, this way we can wait until the robot is actually at the control panel until it starts counting. Do you guys have any suggestions on how we could make the robot count accurately while still having it only call the method when raw button one is pressed?(raw button one is A by the way). Also, do you think the control panel could accurately detect/count color even when the wheel is spinning at high speeds? We’ve only tested it with low speeds so far so admittedly I’m a little concerned. I appreciate any and all advice that you all have.
Robot.java:
/----------------------------------------------------------------------------/
/* Copyright © 2018 FIRST. All Rights Reserved. /
/ Open Source Software - may be modified and shared by FRC teams. The code /
/ must be accompanied by the FIRST BSD license file in the root directory of /
/ the project. /
/----------------------------------------------------------------------------*/
package frc.team8051;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import frc.team8051.commands.drivebase.TestDrive;
import frc.team8051.sensors.DrivebaseEncoder;
import frc.team8051.sensors.Gyro;
import frc.team8051.services.OI;
import frc.team8051.subsystems.ColorSensor;
import frc.team8051.subsystems.DifferentialDriveBase;
import frc.team8051.subsystems.Drivebase;
public class Robot extends TimedRobot {
private static Robot robot;
private Drivebase drivebase;
private OI oi;
private TestDrive testDrive;
private DifferentialDriveBase differentialDriveBase;
private DrivebaseEncoder drivebaseEncoder;
private Gyro gyro;
private ColorSensor colorSensor;
// private final I2C.Port i2cPort = I2C.Port.kOnboard;
//private final ColorSensorV3 m_colorSensor = new ColorSensorV3(i2cPort);
//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);
Robot() {
robot = this;
oi = new OI();
differentialDriveBase = new DifferentialDriveBase();
drivebaseEncoder = new DrivebaseEncoder();
gyro = new Gyro();
gyro.calibrate();
colorSensor = new ColorSensor();
}
@Override
public void robotInit() {
oi.initializeBind();
testDrive = new TestDrive();
colorSensor.initializeColor();
colorSensor.setToZero();
}
@Override
public void robotPeriodic() {
Scheduler.getInstance().run();
if(oi.joystick.getRawButton(1)){
colorSensor.SenseColor();
}
}
@Override
public void autonomousInit() {
}
@Override
public void autonomousPeriodic() {
}
@Override
public void teleopInit() {
System.out.println("Running teleopInit()");
Scheduler.getInstance().add(testDrive);
}
@Override
public void teleopPeriodic () {
// System.out.println("encoder left " + drivebaseEncoder.getLeftSensorReading() +
// " encoder right " + drivebaseEncoder.getRightSensorReading());
Scheduler.getInstance().run();
System.out.println(“remove this”);
}
@Override
public void testPeriodic () {
}
public static Robot getInstance () {
if (robot == null)
robot = new Robot();
return robot;
}
public OI getOI () {
return oi;
}
public Drivebase getDrivebase () {
return drivebase;
}
public DifferentialDrive getDifferentialDrive () {
return differentialDriveBase.getDifferentialDrive();
}
public DrivebaseEncoder getDrivebaseEncoder () {
return drivebaseEncoder;
}
public Gyro getGyro () {
return gyro;
}
}
ColorSennsor.java
package frc.team8051.subsystems;
import com.revrobotics.ColorMatch;
import com.revrobotics.ColorMatchResult;
import com.revrobotics.ColorSensorV3;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
public class ColorSensor extends Subsystem {
private static ColorMatch m_colorMatcher = new ColorMatch();
private static Color kBlueTarget= ColorMatch.makeColor(0.143, 0.427, 0.429);
private static Color kGreenTarget= ColorMatch.makeColor(0.197, 0.561, 0.240);
private static Color kRedTarget= ColorMatch.makeColor(0.561, 0.232, 0.114);
private static Color kYellowTarget = ColorMatch.makeColor(0.361, 0.524, 0.113);
private final I2C.Port i2cPort = I2C.Port.kOnboard;
private final ColorSensorV3 m_colorSensor = new ColorSensorV3(i2cPort);
private int numberOfTimesSpun = 0;
private int numberOfBlues = 0;
private int numberOfReds = 0;
private int numberOfYellows = 0;
private int numberOfGreens = 0;
private String PreviousColor = “”;
private int ColorCounter = -1;
private String currentColor = “”;
@Override
protected void initDefaultCommand() {
}
public static void initializeColor(){
m_colorMatcher.addColorMatch(kBlueTarget);
m_colorMatcher.addColorMatch(kGreenTarget);
m_colorMatcher.addColorMatch(kRedTarget);
m_colorMatcher.addColorMatch(kYellowTarget);
}
public void setToZero(){
numberOfTimesSpun = 0;
numberOfBlues = 0;
numberOfReds = 0;
numberOfYellows = 0;
numberOfGreens = 0;
}
public void SenseColor() {
Color detectedColor = m_colorSensor.getColor();
String colorString;
ColorMatchResult match = m_colorMatcher.matchClosestColor(detectedColor);
if (match.color == kBlueTarget) {
colorString = "Blue";
if(ColorCounter != -1){
PreviousColor = currentColor;
}
if(ColorCounter == -1){
ColorCounter +=1;
}
currentColor = "Blue";
if(currentColor != PreviousColor){
currentColor = "Blue";
numberOfBlues +=1;
}
}
else if (match.color == kRedTarget) {
colorString = "Red";
if(ColorCounter != -1){
PreviousColor = currentColor;
}
if(ColorCounter == -1){
ColorCounter +=1;
}
currentColor = "Red";
if(currentColor != PreviousColor){
currentColor = "Red";
numberOfReds +=1;
}
}
else if (match.color == kGreenTarget) {
colorString = "Green";
if(ColorCounter != -1){
PreviousColor = currentColor;
}
if(ColorCounter == -1){
ColorCounter +=1;
}
currentColor = "Green";
if(currentColor != PreviousColor){
currentColor = "Green";
numberOfGreens +=1;
}
} else if (match.color == kYellowTarget) {
colorString = "Yellow";
if(ColorCounter != -1){
PreviousColor = currentColor;
}
if(ColorCounter == -1){
ColorCounter +=1;
}
currentColor = "Yellow";
if(currentColor != PreviousColor){
currentColor = "Yellow";
numberOfYellows +=1;
}
} else {
colorString = "Unknown";
}
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);
SmartDashboard.putString("Current Color", currentColor);
SmartDashboard.putString("Previous Color", PreviousColor);
SmartDashboard.putNumber("Blues", numberOfBlues);
SmartDashboard.putNumber("Reds", numberOfReds);
SmartDashboard.putNumber("Greens", numberOfGreens);
SmartDashboard.putNumber("Yellows", numberOfYellows);
SmartDashboard.putNumber("Spins", numberOfTimesSpun);
if (numberOfBlues + numberOfGreens + numberOfReds + numberOfYellows == 9) {
numberOfTimesSpun += 1;
numberOfYellows = 0;
numberOfReds = 0;
numberOfBlues = 0;
numberOfGreens = 0;
System.out.println("Spins: " + numberOfTimesSpun);
}
}
}
.
