Updated Spin

Author: LePlebClam

RobotCode2020/src/main/java/frc/robot/Robot.java

@@ -31,6 +31,7 @@ public class Robot extends TimedRobot {
   public static Drivetrain drivetrain = new Drivetrain();
   public static Arm armMotor = new Arm();
   public static Climb climbDevice = new Climb();
+  public static ColorSensor sensor = new ColorSensor();
 
   Command m_autonomousCommand;
   SendableChooser<Command> m_chooser = new SendableChooser<>();

RobotCode2020/src/main/java/frc/robot/commands/Spin.java

@@ -9,14 +9,15 @@
 
 import edu.wpi.first.wpilibj.command.Command;
 import frc.robot.Robot;
-import frc.robot.RobotMap;
+/*import frc.robot.RobotMap;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.can.VictorSPX;
 import com.revrobotics.ColorSensorV3;
 import edu.wpi.first.wpilibj.I2C;
 import com.revrobotics.ColorMatch;
 import com.revrobotics.ColorMatchResult;
+import frc.robot.OI;*/
 import edu.wpi.first.wpilibj.util.Color;
 
 public class Spin extends Command {
@@ -27,33 +28,51 @@ public Spin() {
   }
 
   public Color initialColor;
-  public Color testedColor;
-  public Color prevColor;
-  public int colorCount = 0;
+  public Color currentColor;
+  public Color previousColor;
+  public int halfSpins = 0;
+  public int fullSpins = 0;
 
   // Called just before this Command runs the first time
   @Override
   protected void initialize() {
-    initialColor = Robot.sensor.getColor();
-    Robot.sensor.turn(0.5);
+    initialColor = Robot.sensor.getColorMatch();
+    previousColor = Robot.sensor.getColorMatch();
+    halfSpins = 0;
+    fullSpins = 0;
   }
 
   // Called repeatedly when this Command is scheduled to run
   @Override
   protected void execute() {
-    testedColor = Robot.sensor.getColor();
-    if (testedColor.equals(initialColor) && !(testedColor.equals(prevColor))) {
-      colorCount++;
+    // System.out.println("running Spin execute");
+
+    currentColor = Robot.sensor.getColorMatch();
+
+    // if sensor sees initial color and the wheel has moved, then the wheel has turned halfway
+
+    if (currentColor.equals(initialColor) && !(currentColor.equals(previousColor))) {
+      halfSpins++;
+      // update number of full spins
+      fullSpins = (int) (halfSpins / 2);
     }
-    prevColor = Robot.sensor.getColor();
+
+    previousColor = Robot.sensor.getColorMatch();
+    Robot.sensor.turn(0.25);
+
+    Robot.sensor.commandHalfSpins = halfSpins;
+    Robot.sensor.commandFullSpins = fullSpins;
   }
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
   protected boolean isFinished() {
-    if (colorCount > 6) {
+    // stop spinning when done enough spins
+    if (fullSpins >= 3) {
+      Robot.sensor.turn(0);
       return true;
     }
+
     return false;
   }
 
@@ -66,5 +85,7 @@ protected void end() {
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
-  protected void interrupted() {}
+  protected void interrupted() {
+    Robot.sensor.turn(0);
+  }
 }

RobotCode2020/src/main/java/frc/robot/subsystems/ColorSensor.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 import frc.robot.RobotMap;
+import frc.robot.commands.Spin;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.can.VictorSPX;
@@ -18,6 +19,7 @@
 import com.revrobotics.ColorMatch;
 import com.revrobotics.ColorMatchResult;
 import edu.wpi.first.wpilibj.util.Color;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 /** Add your docs here. */
 public class ColorSensor extends Subsystem {
@@ -34,11 +36,17 @@ public class ColorSensor extends Subsystem {
   private final Color kRedTarget = ColorMatch.makeColor(0.561, 0.232, 0.114);
   private final Color kYellowTarget = ColorMatch.makeColor(0.361, 0.524, 0.113);
 
+  // variables from Spin
+  public int commandHalfSpins = 0;
+  public int commandFullSpins = 0;
+
   public ColorSensor() {
     m_colorMatcher.addColorMatch(kBlueTarget);
     m_colorMatcher.addColorMatch(kGreenTarget);
     m_colorMatcher.addColorMatch(kRedTarget);
     m_colorMatcher.addColorMatch(kYellowTarget);
+
+    sensorMotor.setNeutralMode(NeutralMode.Brake);
   }
 
   Color detectedColor = m_colorSensor.getColor();
@@ -66,23 +74,32 @@ public void reportColorToDashboard() {
     // putting the values onto Shuffleboard
     SmartDashboard.putNumber("Red", detectedColor.red);
     SmartDashboard.putNumber("Green", detectedColor.green);
-    SmartDashboard.putNumber("Confidence", match.confidence);
     SmartDashboard.putNumber("Blue", detectedColor.blue);
+    SmartDashboard.putNumber("Confidence", match.confidence);
+
     SmartDashboard.putString("DetectedColor", colorString);
   }
 
+  public void reportSpinsToDashboard() {
+    SmartDashboard.putNumber("Half Spins", commandHalfSpins);
+    SmartDashboard.putNumber("Full Spins", commandFullSpins);
+  }
+
   // Wrapper class
   public void turn(double voltagePercent) {
     sensorMotor.set(ControlMode.PercentOutput, voltagePercent);
   }
 
   @Override
   public void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
+    // setDefaultCommand(new Spin());
   }
 
   public Color getColor() {
     return m_colorSensor.getColor();
   }
+
+  public Color getColorMatch() {
+    return match.color;
+  }
 }

RobotCode2020/src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -25,7 +25,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-import frc.robot.commands.drivetrain.Drive;
+import frc.robot.commands.drivetrainDrive;
 
 /** Drivetrain class w/ limelight vision tracking */
 public class Drivetrain extends Subsystem {