Updated PID Controls

src/main/java/frc/robot/Constants.java

@@ -4,6 +4,14 @@
 
 package frc.robot;
 
+import org.photonvision.SimVisionTarget;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.util.Units;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -12,4 +20,40 @@
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
  */
-public final class Constants {}
+public final class Constants {
+    public static final class Vision{
+        // public static final double targetWidth =
+
+        //     Units.inchesToMeters(91.0) - Units.inchesToMeters(6.70); // meters
+
+        //     // See
+
+        //     // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf
+
+        //     // page 197
+
+            public static final double targetHeight =
+
+            Units.inchesToMeters(91.0); // meters
+
+            public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters
+
+            // See
+            // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf
+
+            // pages 4 and 5
+
+            public static final double kFarTgtXPos = 1.0;
+
+            public static final String kCamName = "targetCam";
+            public static final Translation2d translation = new Translation2d(.5, 0);
+            public static final Rotation2d rotation = new Rotation2d(0.0);
+            public static final Transform2d kCameraToRobot = new Transform2d(translation, rotation);
+    }
+
+    public static final double kTurnRateToleranceDegPerS = 10;
+    public static final double kTurnToleranceDeg = 10;
+    public static final double kTurnP = 0.01;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0.0001;
+}

src/main/java/frc/robot/RobotContainer.java

@@ -4,9 +4,19 @@
 
 package frc.robot;
 
+import javax.tools.Diagnostic;
+
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.AimToBall;
+import frc.robot.commands.AimToBallManualDist;
+import frc.robot.commands.AimToTarget;
+import frc.robot.commands.AutoCommand;
+import frc.robot.commands.TurnToAngle;
 import frc.robot.subsystems.DriveSystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -22,15 +32,18 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final DriveSystem robotDrive = new DriveSystem();
   private JoystickButton left_bumper;
-  private GenericHID driveController =  new XboxController(0) ;
+  private JoystickButton right_bumper;
+  private JoystickButton a_button;
 
+  private GenericHID driveController =  new XboxController(0) ;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     configureButtonBindings();
+    robotDrive.resetGyro();
 
     robotDrive.setDefaultCommand( 
-      
+
       (new RunCommand(
         () -> 
              robotDrive.drive(
@@ -51,10 +64,14 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    left_bumper = new JoystickButton(driveController, XboxController.Button.kY.value);
+    left_bumper = new JoystickButton(driveController, XboxController.Button.kLeftBumper.value);
+    right_bumper = new JoystickButton(driveController, XboxController.Button.kRightBumper.value);
+    a_button = new JoystickButton(driveController, XboxController.Button.kA.value);
 
-    left_bumper.whileHeld(new AimToBall(robotDrive));
 
+    left_bumper.whileHeld(new AimToTarget(robotDrive));
+    right_bumper.whileHeld(new AimToBall(robotDrive));
+    a_button.whenPressed(new AutoCommand(robotDrive));
   }
 
   /**

src/main/java/frc/robot/commands/AimToBall.java

@@ -11,26 +11,33 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DriveSystem;
 
 public class AimToBall extends CommandBase {
     private DriveSystem m_subsystem;
     private boolean m_finished = false;
 
-    final double LINEAR_P = 0.1;
+    final double LINEAR_P = 0.25;
 
     final double LINEAR_D = 0.0;
 
     PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
 
-    final double ANGULAR_P = 0.1;
+    final double ANGULAR_P = 0.8;// SmartDashboard.getNumber("Angular Pos", 0.1);
 
-    final double ANGULAR_D = -.1;
+    final double ANGULAR_D = 0.01;
 
     PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
     private NetworkTable table;
 
+    private boolean isBlue;
+
     /**
      * Creates a new ExampleCommand.
      *
@@ -39,36 +46,55 @@ public class AimToBall extends CommandBase {
     public AimToBall(DriveSystem subsystem) {
         m_subsystem = subsystem;
         m_finished = true;
+
         // Use addRequirements() here to declare subsystem dependencies.
         addRequirements(subsystem);
     }
 
     @Override
     public void initialize() {
+        System.out.println("Aiming to Ball");
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
         table = inst.getTable("ML");
     }
 
+    @Override
+    public boolean isFinished(){
+        return m_finished;
+    }
+
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
+        isBlue = DriverStation.getAlliance() == DriverStation.Alliance.Blue;
         m_finished = false;
-        NetworkTableEntry xEntry = table.getEntry("best_blue_x");
+        NetworkTableEntry xEntry = table.getEntry(isBlue ? "best_blue_x" : "best_red_x");
+        NetworkTableEntry sizeEntry = table.getEntry(isBlue ? "best_blue_size" : "best_red_size");
         System.out.println(xEntry);
         NetworkTableEntry widthEntry = table.getEntry("width");
+        double turnSpeed = 0.0;
+        double linearSpeed = 0.0;
         if ((Double) xEntry.getNumber(Double.valueOf(0.0)) > Double.valueOf(1.0)) {
             Double width = (Double) widthEntry.getNumber(Double.valueOf(0));
             Double best_target_x = (Double) xEntry.getNumber(0);
             System.out.println(best_target_x);
-            double error = best_target_x - width / 2.0;
-            if (Math.abs(error) > 50) {
-                System.out.println(error);
+            double errorAng = best_target_x - width / 2.0;
+            m_finished = errorAng < 10;
+            if (Math.abs(errorAng) > 5) {
+                // System.out.println(errorAng);
                 // error = error / 160.0;
-                double turnSpeed = turnController.calculate(error, 0);
-                System.out.println(turnSpeed);
-                m_subsystem.drive(0.0, turnSpeed * .04);
+                turnSpeed = turnController.calculate(errorAng, 0);
+                turnSpeed = turnSpeed * (1.0 / 160.0) * 0.5;
+                SmartDashboard.putNumber("turnSpeed", turnSpeed);
+            }
+        }
+        if ((Double) sizeEntry.getNumber(Double.valueOf(0.0)) > Double.valueOf(1.0)) {
+            Double size = (Double) sizeEntry.getNumber(Double.valueOf(0));
+            if(Math.abs(size-28000) > 1500){
+                double errorLin = size - 28000;
+                linearSpeed = forwardController.calculate(errorLin/4000.0, 0.0);
             }
-
         }
+            m_subsystem.drive(linearSpeed, turnSpeed);
     }
 }

src/main/java/frc/robot/commands/AimToBallManualDist.java

@@ -0,0 +1,120 @@
+package frc.robot.commands;
+
+import java.util.Iterator;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.node.ArrayNode;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.DriveSystem;
+
+public class AimToBallManualDist extends CommandBase {
+    private DriveSystem m_subsystem;
+    private boolean m_finished = false;
+
+    final double LINEAR_P = 0.1;
+
+    final double LINEAR_D = 0.0;
+
+    PIDController forwardController = new PIDController(LINEAR_P, 0, LINEAR_D);
+
+    final double ANGULAR_P = 0.8;// SmartDashboard.getNumber("Angular Pos", 0.1);
+
+    final double ANGULAR_D = 0.01;
+
+    PIDController turnController = new PIDController(ANGULAR_P, 0, ANGULAR_D);
+    private NetworkTable table;
+
+    GenericHID controller;
+
+    private Boolean isBlue;
+
+    public AimToBallManualDist(DriveSystem subsystem, GenericHID driveController) {
+        m_subsystem = subsystem;
+        m_finished = true;
+        controller = driveController;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
+
+    public AimToBallManualDist(DriveSystem subsystem) {
+        m_subsystem = subsystem;
+        m_finished = true;
+        controller = null;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
+
+    public AimToBallManualDist(DriveSystem subsystem, GenericHID driveController, Boolean isTeamBlue) {
+        m_subsystem = subsystem;
+        m_finished = true;
+        controller = driveController;
+        isBlue = isTeamBlue;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
+
+    public AimToBallManualDist(DriveSystem subsystem, Boolean isTeamBlue) {
+        m_subsystem = subsystem;
+        m_finished = true;
+        controller = null;
+        isBlue = isTeamBlue;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
+
+
+    @Override
+    public void initialize() {
+        System.out.println("Aiming to Ball");
+        NetworkTableInstance inst = NetworkTableInstance.getDefault();
+        table = inst.getTable("ML");
+    }
+
+    @Override
+    public boolean isFinished(){
+        return m_finished;
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_finished = false;
+        NetworkTableEntry xEntry = table.getEntry(isBlue ? "best_blue_x": "best_red_x");
+        System.out.println(xEntry);
+        NetworkTableEntry widthEntry = table.getEntry("width");
+        if ((Double) xEntry.getNumber(Double.valueOf(0.0)) > Double.valueOf(1.0)) {
+            Double width = (Double) widthEntry.getNumber(Double.valueOf(0));
+            Double best_target_x = (Double) xEntry.getNumber(0);
+            System.out.println(best_target_x);
+            double error = best_target_x - width / 2.0;
+            m_finished = error < 10;
+            if (Math.abs(error) > 5) {
+                System.out.println(error);
+                // error = error / 160.0;
+                double turnSpeed = turnController.calculate(error, 0);
+                turnSpeed = turnSpeed * (1.0 / 160.0) * 0.5;
+                SmartDashboard.putNumber("turnSpeed", turnSpeed);
+                if (controller != null) {
+                    m_subsystem.drive(controller.getRawAxis(1) > 0.1 || controller.getRawAxis(1) < -0.1
+                            ? -controller.getRawAxis(1)
+                            : 0.0, turnSpeed);
+
+                } else {
+                    m_subsystem.drive(0.0, turnSpeed);
+                }
+            }
+
+        }
+    }
+}