diff --git a/SwerveCode/gradlew b/SwerveCode/gradlew old mode 100644 new mode 100755 diff --git a/SwerveCode/src/main/java/frc/robot/ColorSensor.java b/SwerveCode/src/main/java/frc/robot/ColorSensor.java index d390cc2..13f4333 100644 --- a/SwerveCode/src/main/java/frc/robot/ColorSensor.java +++ b/SwerveCode/src/main/java/frc/robot/ColorSensor.java @@ -12,44 +12,42 @@ public class ColorSensor{ private final I2C.Port i2cPort = I2C.Port.kOnboard; private final ColorSensorV3 m_colorSensor = new ColorSensorV3(i2cPort); - private ALLIANCE_COLOR alliance; private ALLIANCE_COLOR currentBallColor; public ColorSensor(){ - alliance = updateCurrentBallColor(); + alliance = updateCurrentBallColor(); currentBallColor = alliance; } - - public void getColors(){ + public ALLIANCE_COLOR updateCurrentBallColor(){ Color detectedColor = m_colorSensor.getColor(); - int proximity = m_colorSensor.getProximity(); - SmartDashboard.putNumber("Red", detectedColor.red); //output the seen overall red value to the smartdashboard SmartDashboard.putNumber("Blue", detectedColor.blue); //output the seen overall blue value to the smartdashboard - SmartDashboard.putNumber("Proximity", proximity); //distance from colorsensor to the nearest object (2047 to 0) output to the smartdashboard + SmartDashboard.putNumber("Proximity", m_colorSensor.getProximity()); //distance from colorsensor to the nearest object (2047 to 0) output to the smartdashboard SmartDashboard.putString("alliance", this.toString()); //current alliance value output the the smartdashboard - //System.out.println(detectedColor.red); - } - - public ALLIANCE_COLOR updateCurrentBallColor(){ - Color detectedColor = m_colorSensor.getColor(); + if(m_colorSensor.getProximity() >= 1500){ if(detectedColor.blue > detectedColor.red){ return ALLIANCE_COLOR.BLUE; }else if(detectedColor.red > detectedColor.blue){ return ALLIANCE_COLOR.RED; + } + else{ + return ALLIANCE_COLOR.NONE; + } }else{ - return ALLIANCE_COLOR.NONE;} + return ALLIANCE_COLOR.NONE; + } } public boolean compareBallToAlliance(){ - if(currentBallColor == alliance){ - return true; - }else if(currentBallColor != alliance){ - return false; - }else{return false;} + boolean aColor = true; + if (this.alliance != ALLIANCE_COLOR.NONE){ + ALLIANCE_COLOR color = updateCurrentBallColor(); + aColor = (color != ALLIANCE_COLOR.NONE && color != this.alliance); + } + return aColor; } public int getProximity(){ @@ -78,4 +76,4 @@ public String toString(){ } return s; } -} \ No newline at end of file +} diff --git a/SwerveCode/src/main/java/frc/robot/Robot.java b/SwerveCode/src/main/java/frc/robot/Robot.java index 66f8d47..c2fd9b1 100644 --- a/SwerveCode/src/main/java/frc/robot/Robot.java +++ b/SwerveCode/src/main/java/frc/robot/Robot.java @@ -40,7 +40,7 @@ public class Robot extends TimedRobot { public Locality locality; public Shooter shooter; public Collector collector; - public ColorSensor color; + public ColorSensor cSensor; /** @@ -59,7 +59,7 @@ public void robotInit() { visionBall = new Vision(Constants.LIMELIGHT_BALL, Constants.BALL_LOCK_ERROR, Constants.BALL_CAMERA_HEIGHT, Constants.BALL_CAMERA_ANGLE, Constants.BALL_TARGET_HEIGHT, Constants.BALL_ROTATE_KP); locality = new Locality(0, 0); shooter = new Shooter(); - color = new ColorSensor(); + cSensor = new ColorSensor(); //ball.setLimelightAllianceColor(ALLIANCE_COLOR.RED); NetworkTableInstance.getDefault().getTable("limelight-ball").getEntry("pipeline").setNumber(1); } @@ -96,7 +96,7 @@ public void robotPeriodic() { */ @Override public void autonomousInit() { - + cSensor = new ColorSensor(); m_autoSelected = m_chooser.getSelected(); // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto); System.out.println("Auto selected: " + m_autoSelected); @@ -108,6 +108,9 @@ public void autonomousInit() { /** This function is called once when teleop is enabled. */ @Override public void teleopInit() { + if (cSensor == null){ + cSensor = new ColorSensor(); + } drive.zeroGyroscope(); //Create the primary controller object } @@ -160,12 +163,11 @@ else if(driverController.getLeftBumper() == true) { } SmartDashboard.putNumber("Rotate", rotate); - color.getColors(); - if(color.getProximity() > 200){ - color.updateCurrentBallColor(); + cSensor.updateCurrentBallColor(); + if(cSensor.getProximity() > 200){ + cSensor.updateCurrentBallColor(); } - } /** This function is called once when the robot is disabled. */ @Override diff --git a/SwerveCode/src/main/java/frc/robot/Shooter.java b/SwerveCode/src/main/java/frc/robot/Shooter.java index 6d25bec..d675bae 100644 --- a/SwerveCode/src/main/java/frc/robot/Shooter.java +++ b/SwerveCode/src/main/java/frc/robot/Shooter.java @@ -25,7 +25,7 @@ public class Shooter{ public WPI_TalonFX flyWheelLeft; public WPI_TalonFX flyWheelRight; public WPI_TalonFX staging; - + public ColorSensor cSensor; //motor groups MotorControllerGroup flyWheel; @@ -61,7 +61,8 @@ public void testshooter(XboxController shooterController) { staging.set(shooterController.getLeftTriggerAxis()); flyWheel.set(flyWheelSpeed); - } } + +}