diff --git a/src/main/java/frc/robot/commands/colorWheel_command.java b/src/main/java/frc/robot/commands/colorWheel_command.java index 1615e8b..cc16d55 100644 --- a/src/main/java/frc/robot/commands/colorWheel_command.java +++ b/src/main/java/frc/robot/commands/colorWheel_command.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; import frc.robot.RobotMap; +import frc.robot.subsystems.colorWheel_subsystem; + import java.util.ArrayList; // import frc.robot.subsystems.colorWheel_subsystem; import edu.wpi.first.wpilibj.DriverStation; @@ -55,7 +57,7 @@ protected void execute() { gameData = DriverStation.getInstance().getGameSpecificMessage(); - System.out.println("Control panel sensor target: " + gameData); // color the control panel sensor needs to see for 5 sec + System.out.println("Control panel sensor target: " + gameData); // color the *CONTROL PANEL* sensor needs to see (for 5 sec) if(gameData.length() > 0 && yPressed) { switch (gameData.charAt(0)) @@ -108,7 +110,7 @@ protected void execute() { stopTimer = 5.0; yPressed = false; } else { - --stopTimer; // subtract from over color stop timer + --stopTimer; // subtract from over color stop timer, this will happen until the color timer = 0, if it still sees the target color, then the motor can stop } } else { switch (initialColor) { @@ -193,11 +195,23 @@ protected void execute() { break; } } - } else { // if fms has not provided a color + } else if (yPressed) { // PRESUMABLY if y is pressed but no fms data is/was received + if (Robot.colorWheel_subsystem.numOfColorWheelRotations() >= 4) { // checking to see if the color wheel has completed 4 or more rotations + yPressed = false; // stopping the yPressed loop + rotateSpeed = 0; // cancelling motor movement + Robot.colorWheel_subsystem.resetColorWheelEncoder(); + } else { + rotateSpeed = 0.23; // spinning color wheel in this loop at a higher speed because it doesn't matter (I think), will continue PRESUMABLY until the color wheel has spun 4 or more times + } + } else { // if fms has not provided a color and Y is not pressed rotateSpeed = 0; } - System.out.println(stopTimer); + System.out.println(stopTimer); // TEMPORARY logging the timer that iterates when a different color is seen to make sure it counts down Robot.colorWheel_subsystem.rotateColorWheel(rotateSpeed); // set motor speed to rotate speed as defined above + + /* TESTING FOR NEXT TIME TO HOPEFULLY SEE THE NUMBER OF **PULSES??** IT TAKES TO COVER CERTAIN DISTANCE WITH ENCODER */ + // system out sc "syso" + System.out.println(Robot.colorWheel_subsystem.numOfColorWheelRotations()); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/subsystems/colorWheel_subsystem.java b/src/main/java/frc/robot/subsystems/colorWheel_subsystem.java index 3b0a4f6..abc61f7 100644 --- a/src/main/java/frc/robot/subsystems/colorWheel_subsystem.java +++ b/src/main/java/frc/robot/subsystems/colorWheel_subsystem.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.Encoder; /** * Add your docs here. @@ -27,6 +28,8 @@ public class colorWheel_subsystem extends Subsystem { // here. Call these from Commands. WPI_TalonSRX colorWheelWheel = null; + // Encoder colorWheelEncoder = null; + Encoder colorWheelEncoder = new Encoder(0, 1); // for use in getting the number of rotations the color wheel has had // FIX PORT NUMBERS NOT SURE WHAT THEY ARE /** * Change the I2C port below to match the connection of your color sensor @@ -75,11 +78,12 @@ public colorWheel_subsystem() { m_colorMatcher.setConfidenceThreshold(0.8); colorWheelWheel = new WPI_TalonSRX(RobotMap.MOTOR_COLOR_WHEEL); + // colorWheelEncoder = new Encoder(0, 1); // FIX PORT NUMBERS NOT SURE WHAT THEY ARE } // method called by command to get color from sensor /** * Get the current color (as defined) that the color sensor sees - * @return + * @return color result */ public String getColor() { String result; @@ -132,4 +136,12 @@ public String getColor() { public void rotateColorWheel(double speed) { colorWheelWheel.set(speed); } + + public double numOfColorWheelRotations() { + return colorWheelEncoder.getDistance(); + } + + public void resetColorWheelEncoder() { + colorWheelEncoder.reset(); + } }