Wojtaszek - Autonomous and other stuff

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

@@ -14,7 +14,55 @@
  */
 public final class Constants {
   public class DriveConstants {
+    // The maximum linear velocity of the robot in meters per second.
+    // This represents the highest speed at which the robot can move forward or backward.
     public static double maxLinearVelocityMetersPerSec = 1.0;
+
+    // The maximum angular velocity of the robot in radians per second.
+    // This represents the highest speed at which the robot can rotate in place.
     public static double maxAngularVelocityRadPerSec = 4 * 2 * Math.PI;
+
+    // The track width of the robot in meters.
+    // This is the distance between the left and right wheels of the robot.
+    public static double trackWidthMeters = 0.155;
+
+    // The diameter of the robot's wheels in meters (60 mm).
+    public static double wheelDiameterMeters = 0.060;
+
+    public class speedPIDConstants {
+      // The proportional gain for the PID controller.
+      // This value determines how strongly the controller reacts to the current error.
+      // A higher kP value will result in a stronger response to the error, but may cause overshooting.
+      public static double kP = .5;
+
+      // The integral gain for the PID controller.
+      // This value determines how strongly the controller reacts to the accumulation of past errors.
+      // A higher kI value will result in a stronger response to accumulated errors, but may cause instability.
+      public static double kI = 0.00;
+
+      // The derivative gain for the PID controller.
+      // This value determines how strongly the controller reacts to the rate of change of the error.
+      // A higher kD value will result in a stronger response to the rate of change, helping to dampen oscillations.
+      public static double kD = .5;
+    }
+
+    public class zRotationPIDConstants {
+      // The proportional gain for the PID controller.
+      // This value determines how strongly the controller reacts to the current error.
+      // A higher kP value will result in a stronger response to the error, but may cause overshooting.
+      public static double kP = 0.01;
+
+      // The integral gain for the PID controller.
+      // This value determines how strongly the controller reacts to the accumulation of past errors.
+      // A higher kI value will result in a stronger response to accumulated errors, but may cause instability.
+      public static double kI = 0.0;
+
+      // The derivative gain for the PID controller.
+      // This value determines how strongly the controller reacts to the rate of change of the error.
+      // A higher kD value will result in a stronger response to the rate of change, helping to dampen oscillations.
+      public static double kD = 0.01;
+    }
+
+
   }
 }

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

@@ -8,7 +8,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.ArcadeDrive;
+// This is neeeded if you are using TankDrive
+// import frc.robot.commands.TankDrive;
 import frc.robot.commands.ArmCommand;
+import frc.robot.commands.AutonomousCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.ShooterCommand;
 import frc.robot.subsystems.arm.ArmIOXrp;
@@ -19,6 +22,7 @@
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.shooter.ShooterIOXrp;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
+import org.littletonrobotics.junction.Logger;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -27,10 +31,11 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+
   // The robot's subsystems and commands are defined here...
   private final DriveSubsystemXrp drive = new DriveSubsystemXrp(new DriveDifferentialIOXrp());
-  private final ArmSubsystem arm = new ArmSubsystem(new ArmIOXrp(4));
-  private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOXrp(5));
+  private final ArmSubsystem arm = new ArmSubsystem(new ArmIOXrp(4), new ArmIOXrp(5));
+  //private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOXrp(5));
   private final ShooterSubsystem shooter = new ShooterSubsystem(new ShooterIOXrp(2));
 
   private final CommandXboxController mainController = new CommandXboxController(0);
@@ -48,11 +53,28 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+
+    // Configure the robot to be driven by the left stick for steering and for
+    // throttle
     drive.setDefaultCommand(
         new ArcadeDrive(drive, () -> -mainController.getLeftY(), () -> -mainController.getLeftX()));
 
+    // // Arcade drive but left stick for throttle and right stick controls steering
+    // drive.setDefaultCommand(
+    //     new ArcadeDrive(drive,
+    //         () -> -mainController.getLeftY(),
+    //         () -> -mainController.getRightX()));
+
+    // Both sticks control steering using TankDrive
+    // drive.setDefaultCommand(
+    //        new TankDrive(drive,
+    //           () -> -mainController.getLeftY(),
+    //           () -> -mainController.getRightY()));
+
     arm.setDefaultCommand(new ArmCommand(arm, () -> -mainController.getRightY()));
-    intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX()));
+
+    //intake.setDefaultCommand(new IntakeCommand(intake, () -> -mainController.getRightX()));
+
     shooter.setDefaultCommand(
         new ShooterCommand(
             shooter,
@@ -66,7 +88,9 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
-    return null;
+
+    Command myAuto = new AutonomousCommand(drive);
+
+    return myAuto;
   }
 }

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

@@ -0,0 +1,36 @@
+// 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 frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.subsystems.drive.DriveSubsystemXrp;
+
+public class AutonomousCommand extends SequentialCommandGroup {
+  /**
+   * Creates a new Autonomous Drive based on distance. This will drive out for a specified distance,
+   * turn around and drive back.
+   *
+   * @param drivetrain The drivetrain subsystem on which this command will run
+   */
+  public AutonomousCommand(DriveSubsystemXrp drivetrain) {
+
+    // Creates a PID Controller for the wheels going in a straight line
+    PIDController wheelPIDController =
+        new PIDController(DriveConstants.speedPIDConstants.kP, 
+                          DriveConstants.speedPIDConstants.kI,
+                          DriveConstants.speedPIDConstants.kD);
+
+    PIDController zAxisPidController =
+        new PIDController(DriveConstants.zRotationPIDConstants.kP, 
+                          DriveConstants.zRotationPIDConstants.kI,
+                          DriveConstants.zRotationPIDConstants.kD);
+
+    addCommands(
+        new DriveDistance(1, 1, drivetrain, wheelPIDController, zAxisPidController), // Drive forward
+        new DriveDistance(-1, 1, drivetrain, wheelPIDController, zAxisPidController)); // Drive backward
+  }
+}

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

@@ -0,0 +1,129 @@
+// 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 frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.subsystems.drive.DriveSubsystemXrp;
+import org.littletonrobotics.junction.Logger;
+
+public class DriveDistance extends Command {
+  private DriveSubsystemXrp drive;
+  private double distance;
+  private double speed;
+  private PIDController distancePIDController;
+  private PIDController zRotationPIDController;
+  private DifferentialDriveKinematics kinematics;
+  private double rightWheelStartPosition;
+  private double leftWheelStartPosition;
+  private double zRotationStartingPosition;
+
+  /**
+   * Creates a new DriveDistance. This command will drive your your robot for a desired distance at
+   * a desired speed.
+   *
+   * @param speed The speed at which the robot will drive
+   * @param meters The number of meters the robot will drive
+   * @param drive The drivetrain subsystem on which this command will run
+   */
+
+  public DriveDistance(
+      double driveSpeed,
+      double meters,
+      DriveSubsystemXrp driveSubsystem,
+      PIDController dPIDController,
+      PIDController zPIDController
+      )
+       {
+
+    distance = meters;
+    speed = driveSpeed;
+    drive = driveSubsystem;
+    distancePIDController = dPIDController;
+    zRotationPIDController = zPIDController; // Initialize the PID controller for Z rotation
+
+    // Create a DifferentialDriveKinematics object with the track width
+    kinematics = new DifferentialDriveKinematics(DriveConstants.trackWidthMeters);
+
+
+
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    drive.resetEncoders();
+    drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0));
+    distancePIDController.reset();
+    rightWheelStartPosition = drive.getRightPositionMeters();
+    leftWheelStartPosition = drive.getLeftPositionMeters();
+    zRotationStartingPosition = drive.getAngleZ();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    double leftDistance = leftWheelStartPosition - drive.getLeftPositionMeters();
+    double rightDistance = rightWheelStartPosition - drive.getRightPositionMeters();
+
+    double distanceError = leftDistance - rightDistance;
+
+    double distanceCorrection = distancePIDController.calculate(distanceError);
+
+    double zRotationDrift = zRotationStartingPosition - drive.getAngleZ();
+
+    // Calculate the Z rotation correction using the PID controller
+    double headingCorrection = zRotationPIDController.calculate(zRotationDrift);
+
+    double leftSpeed = speed -  distanceCorrection;
+    double rightSpeed = speed + distanceCorrection;
+
+    // Create a DifferentialDriveWheelSpeeds object with the wheel speeds
+    DifferentialDriveWheelSpeeds wheelSpeeds =
+            new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
+
+    // // Convert the wheel speeds to chassis speeds
+    ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds);
+    // Log the new value
+    Logger.recordOutput("leftWheelStartPosition", leftWheelStartPosition);
+    Logger.recordOutput("rightWheelStartPosition", rightWheelStartPosition);
+    Logger.recordOutput("LeftPositionMeters", drive.getLeftPositionMeters());
+    Logger.recordOutput("RightPositionMeters", drive.getLeftPositionMeters());
+    Logger.recordOutput("leftDistance", leftDistance);
+    Logger.recordOutput("rightDistance", leftDistance);
+    Logger.recordOutput("distanceError", distanceError);
+    Logger.recordOutput("distanceCorrection", distanceCorrection);
+    Logger.recordOutput("zRotationDrift", zRotationDrift);
+    Logger.recordOutput("headingCorrection", headingCorrection);
+    Logger.recordOutput("leftSpeed", leftSpeed);
+    Logger.recordOutput("rightSpeed", rightSpeed);
+    Logger.recordOutput("chassisSpeeds", chassisSpeeds);
+    Logger.recordOutput("zRotationStartingPosition", zRotationStartingPosition);
+
+    drive.setChassisSpeeds(chassisSpeeds);
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0));
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    double leftDistanceTraveled = leftWheelStartPosition - drive.getLeftPositionMeters();
+    double rightDistanceTraveled = rightWheelStartPosition - drive.getRightPositionMeters();
+
+    double distanceTraveled = (leftDistanceTraveled + rightDistanceTraveled) / 2.0;
+    return (Math.abs(distanceTraveled) >= distance);
+  }
+}

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

@@ -0,0 +1,76 @@
+// 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 frc.robot.commands;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants.DriveConstants;
+import frc.robot.subsystems.drive.Drive;
+import java.util.function.Supplier;
+
+public class TankDrive extends Command {
+  private final Drive drive;
+  private final Supplier<Double> rightWheelSpeedSupplier;
+  private final Supplier<Double> leftWheelSpeedSupplier;
+
+  // Create a DifferentialDriveKinematics object with the track width
+  private DifferentialDriveKinematics kinematics =
+      new DifferentialDriveKinematics(DriveConstants.trackWidthMeters);
+
+  /**
+   * Creates a new TankDrive. This command will drive your robot according to the speed supplier
+   * lambdas. This command does not terminate.
+   *
+   * @param drive The drivetrain subsystem on which this command will run
+   * @param leftWheelSpeedSupplier Lambda supplier of forward/backward speed
+   * @param rightWheelSpeedSupplier Lambda supplier of rotational speed
+   */
+  public TankDrive(
+      Drive drive,
+      Supplier<Double> leftWheelSpeedSupplier,
+      Supplier<Double> rightWheelSpeedSupplier) {
+    this.drive = drive;
+    this.rightWheelSpeedSupplier = leftWheelSpeedSupplier;
+    this.leftWheelSpeedSupplier = rightWheelSpeedSupplier;
+    addRequirements((Subsystem) drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    double rightWheelSpeed =
+        rightWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec;
+    double leftWheelSpeed =
+        leftWheelSpeedSupplier.get() * DriveConstants.maxLinearVelocityMetersPerSec;
+
+    // Create a DifferentialDriveWheelSpeeds object with the wheel speeds
+    DifferentialDriveWheelSpeeds wheelSpeeds =
+        new DifferentialDriveWheelSpeeds(leftWheelSpeed, rightWheelSpeed);
+
+    // Convert the wheel speeds to chassis speeds
+    ChassisSpeeds chassisSpeeds = kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Pass the wheel speeds to the drive subsystem
+    drive.setChassisSpeeds(chassisSpeeds);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}