Need help with I2C color sensor code

Hello, my team has been working on the code for the color sensor. To make the color wheel spin 3-5 times I am making a counter, in the matchColor method, that adds one every time it sees the yellow color and stop at 8. I am pretty sure the sensor is reading the colors right since it is sometimes printing “color: yellow” but the counter is not adding up. What might I be doing wrong?


package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFX;
import com.revrobotics.ColorMatch;
import com.revrobotics.ColorMatchResult;
import com.revrobotics.ColorSensorV3;

import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.commands.GetColorIntendity;



public class ControlPanel extends SubsystemBase {
  /**
   * Creates a new ControlPanel.
   */

  TalonFX talonMove = new TalonFX(Constants.moveControlPanel);

  private final I2C.Port i2cPort = I2C.Port.kOnboard;

  private final ColorSensorV3 colorSensor = new ColorSensorV3(i2cPort); 

  public final ColorMatch colorMatcher = new ColorMatch(); 

  public final JoystickButton blueButton = new JoystickButton(RobotContainer.stick, Constants.blueColor);
  public final JoystickButton redButton = new JoystickButton(RobotContainer.stick, Constants.redColor);
  public final JoystickButton yellowButton = new JoystickButton(RobotContainer.stick, Constants.yellowColor);
  public final JoystickButton greenButton = new JoystickButton(RobotContainer.stick, Constants.greenColor);
  public final JoystickButton vueltas = new JoystickButton(RobotContainer.xBox, Constants.Vueltas);


  public final Color kBlueTarget = ColorMatch.makeColor(0.143, 0.427, 0.429);
  public final Color kGreenTarget = ColorMatch.makeColor(0.197, 0.561, 0.240);
  public final Color kRedTarget = ColorMatch.makeColor(0.561, 0.232, 0.114);
  public final Color kYellowTarget = ColorMatch.makeColor(0.361, 0.524, 0.113);

  public ControlPanel() {

    double IR = colorSensor.getIR();
   
  
  }

  public void getColorIntensity(){
    // colorSensor.

    Color detectedColor = colorSensor.getColor();

    

    // System.out.println("Red" + detectedColor.red);
    // System.out.println("Green" + detectedColor.green);
    // System.out.println("Blue" + detectedColor.blue);

  }

  String colorString;

  public void matchColor(){
    /**
     * 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 = colorSensor.getColor();

    /**
     * Run the color match algorithm on our detected color
     */
  
    ColorMatchResult match = 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";
    }
    System.out.println("Color: " + colorString);
    /**
     * Open Smart Dashboard or Shuffleboard to see the color detected by the 
     * sensor.
     */

    
    if(RobotContainer.xBox.getYButton()){
      int counter = 0;
      while(counter < 8){
        
        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";
        }

        talonMove.set(ControlMode.PercentOutput, 0.3); 

        if(colorString.equals("Yellow")){
          counter++; 
        } 

        System.out.println("Counter: " + counter + " Color: " + colorString);
      }
      
    }
    
  }

  // public void spinMotorColor(){
    
  //   int counter = 1;
  //   if(RobotContainer.xBox.getYButton()){
      
  //     while(counter <= 8){

  //       talonMove.set(ControlMode.PercentOutput, 0.3); 

  //       if(colorString.equals("Yellow")){
  //         counter++; 
  //       } 
            
  //     System.out.println("Counter: " + counter);
  //     }
      
  //   }
  
  // } 


  public void matchBlueColor(){
    if(colorString.equals("Blue")){
      talonMove.set(ControlMode.Velocity, 0.0);
    }else{
      talonMove.set(ControlMode.Velocity, 0.3);
    }
  }

  public void matchRedColor(){
    if(colorString.equals("Red")){
      talonMove.set(ControlMode.Velocity, 0.0);
    }else{
      talonMove.set(ControlMode.Velocity, 0.3);
    }
  }

  public void matchYellowColor(){
    if(colorString.equals("Yellow")){
      talonMove.set(ControlMode.Velocity, 0.0);
    }else{
      talonMove.set(ControlMode.Velocity, 0.3);
    }
  }

  public void matchGreenColor(){
    if(colorString.equals("Green")){
      talonMove.set(ControlMode.Velocity, 0.0);
    }else{
      talonMove.set(ControlMode.Velocity, 0.3);
    }
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }
}

It’s hard to tell because the code posted is not formatted very well (it’s not cleanly wrapped in a single Chief Delphi message code block). Can you fix the surrounding block quoting and/or post a link to a github repo?

I have a guess of what might be wrong. You have your check to see if the color is yellow wrapped in
if(RobotContainer.xBox.getYButton()){
so unless the y button is being held down, it’s not going to count anything.

Don’t use loops in your code. If you don’t start on yellow, your code will get stuck in this loop forever because you don’t give it the opportunity to actually run anything outside of it (like actually setting motor outputs). If you DO start on yellow, it’ll count to 8 very quickly then exit without actually driving the motor anywhere (during that time at least.

Change your while to and if and I think it’ll work. (though this is based on a 30 second review of your code, so I likely missed something else as well.

Edit: Also @randomstring’s comment about the button. I would recommend re-thinking the logic here. You generally should never need to reference joystick inputs in your subsystem directly.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.