Ball detection and rotation capabilities

src/main/java/org/team639/Robot.java

@@ -5,6 +5,7 @@
 package org.team639;
 
 import org.team639.lib.Constants;
+import org.team639.lib.states.AllianceColor;
 import org.team639.subsystems.JeVoisInterface;
 
 import edu.wpi.first.networktables.NetworkTableEntry;
@@ -25,6 +26,7 @@ public class Robot extends TimedRobot {
   Compressor phCompressor = new Compressor(Constants.Ports.PneumaticsModuleType.phCompressorID, PneumaticsModuleType.REVPH);
 
   private RobotContainer m_robotContainer;
+
   public static NetworkTableEntry llpython;
 
   public static double runningHorizontalAngle;
@@ -65,14 +67,16 @@ public void robotInit() {
   public void robotPeriodic() {
     llpython = NetworkTableInstance.getDefault().getTable("limelight").getEntry("llpython");
 
-    // blueBallAngle = NetworkTableInstance.getDefault().getTable("visions").getEntry("blueAngle");
-    // redBallAngle = NetworkTableInstance.getDefault().getTable("visions").getEntry("blueAngle");
-
+    blueBallAngle = NetworkTableInstance.getDefault().getTable("visions").getEntry("blueAngle").getDouble(361.0);
+    redBallAngle = NetworkTableInstance.getDefault().getTable("visions").getEntry("redAngle").getDouble(361.0);
 
 
     SmartDashboard.putNumber("Angle to target", getAngleToTarget());
     SmartDashboard.putNumber("Distance to target", getDistanceToTarget());
 
+    SmartDashboard.putNumber("Angle to Closest Ball B", getAngleToBallBlue());
+    SmartDashboard.putNumber("Angle to Closest Ball R", getAngleToBallRed());
+
     NetworkTableInstance.getDefault().flush();
     CommandScheduler.getInstance().run();
   }
@@ -163,10 +167,15 @@ public static double getDistanceToTarget()
     }
   }
 
-  // public static double getAngleToBall()
-  // {
-  //   return NetW
-  // }
+  public static double getAngleToBallBlue()
+  {
+    return blueBallAngle;
+  }
+
+  public static double getAngleToBallRed()
+  {
+    return redBallAngle;
+  }
 
 
   /**

src/main/java/org/team639/commands/Drive/TurnToBall.java

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.team639.commands.Drive;
+
+import org.team639.Robot;
+import org.team639.RobotContainer;
+import org.team639.lib.Constants;
+import org.team639.lib.states.AllianceColor;
+import org.team639.lib.states.GearMode;
+import org.team639.subsystems.DriveTrain;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class TurnToBall extends CommandBase {
+  private DriveTrain driveTrain;
+  private double setpoint;
+
+  private double angle;
+  private boolean clockwise;
+
+  private GearMode lastGear;
+  private PIDController controller = new PIDController(Constants.AutoConstants.autoRotateP,
+      Constants.AutoConstants.autoRotateI, Constants.AutoConstants.autoRotateD);
+
+  /**
+   * Creates relative turn to angle that uses vision tracking
+   * @param driveTrain DriveTrain to use
+   */
+  public TurnToBall(DriveTrain driveTrain) {
+    this.driveTrain = driveTrain;
+    addRequirements(driveTrain);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    lastGear = driveTrain.getGearMode();
+    angle = RobotContainer.getAllianceColor().equals(AllianceColor.blue) ? Robot.getAngleToBallBlue() % 360.0 : Robot.getAngleToBallRed();
+    clockwise = Math.signum(angle) > 0 ? true : false;
+    //setpoint = clockwise ? Math.abs(angle) + driveTrain.getHeading() : driveTrain.getHeading() - Math.abs(angle);
+    setpoint = driveTrain.getHeading() + angle;
+
+    controller.setTolerance(Constants.AutoConstants.autoRotateThreshHold,
+        Constants.AutoConstants.autoRotateThreshHoldVelo);
+    controller.setSetpoint(setpoint);
+
+    driveTrain.setSpeedsPercent(0, 0);
+    if (lastGear.equals(GearMode.high))
+      driveTrain.toggleGearLow();
+    System.out.println("Auto rotate initializing");
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (clockwise)
+      driveTrain.turnCommandR(controller.calculate(driveTrain.getHeading(), setpoint));
+    else
+      driveTrain.turnCommandL(-controller.calculate(driveTrain.getHeading(), setpoint));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    driveTrain.setSpeedsPercent(0, 0);
+    if (lastGear.equals(GearMode.high))
+      driveTrain.toggleGearHigh();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (controller.atSetpoint())
+      return true;
+    return false;
+  }
+}