Origin/feature/kinematics

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

@@ -12,6 +12,9 @@
 import frc.robot.subsystems.Spinner;
 import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.SwerveModule;
+import frc.robot.util.Kinematics;
+import frc.robot.util.Kinematics.StartPosition;
+
 import java.util.function.Supplier;
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -29,6 +32,7 @@ public class RobotContainer {
 	private final Pneumatics pneumaticsSubsystem;
 	private final AHRS gyro;
 	private final Spinner spinnerSubsystem;
+	private final Kinematics kinematics;
 	private final DriveCommand driveCommand;
 	private final IntakeCommand intakeCommand;
 	private final SwerveModule flModule;
@@ -42,6 +46,7 @@ public class RobotContainer {
 	private XboxController driverController;
 	private XboxController commandController;
 	private final SendableChooser<Supplier<Command>> autonChooser;
+	private final SendableChooser<Supplier<StartPosition>> startPositionChooser;
 
 	public RobotContainer() {
 
@@ -122,9 +127,13 @@ public RobotContainer() {
 		pneumaticsSubsystem.setDefaultCommand(intakeCommand);
 
 		autonChooser = new SendableChooser<>();
+		startPositionChooser = new SendableChooser<>();
 
 		configAutonChooser();
+		configStartPositionChoice();
+
 
+		kinematics = new Kinematics(gyro, startPositionChooser.getSelected().get());
 	}
 
 	/**
@@ -150,6 +159,27 @@ public static void putCommand(
 			command.withName(name).ignoringDisable(canRunWhileDisabled)
 		);
 
+	}
+
+	private void configStartPositionChoice () {
+		startPositionChooser.addOption("Wireguard Side Position", () ->
+			StartPosition.WIREGUARD
+		);
+
+		startPositionChooser.addOption("Inside Position", () ->
+			StartPosition.INSIDE
+		);
+
+		startPositionChooser.addOption("Balance (Middle) Position", () ->
+			StartPosition.BALANCE
+		);
+
+		putSendable("Start Position Chooser", startPositionChooser);
+	}
+
+	private void configAutonPage() {
+
+
 	}
 
 	private void configAutonChooser() {
@@ -166,7 +196,9 @@ private void configAutonChooser() {
 			new PlaceAndBalanceAuton(
 				swerveSubsystem,
 				spinnerSubsystem,
-				pneumaticsSubsystem
+				pneumaticsSubsystem, 
+				kinematics,
+				gyro
 			)
 		);
 

src/main/java/frc/robot/commands/Auton/PlaceAndBalanceAuton.java

@@ -4,20 +4,23 @@
 
 package frc.robot.commands.Auton;
 
+import com.kauailabs.navx.frc.AHRS;
+
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.Auton.framework.BalanceAuton;
 import frc.robot.commands.Auton.framework.ShootAuton;
 import frc.robot.subsystems.Pneumatics;
 import frc.robot.subsystems.Spinner;
 import frc.robot.subsystems.Swerve;
+import frc.robot.util.Kinematics;
 
 public class PlaceAndBalanceAuton extends SequentialCommandGroup {
 
-  public PlaceAndBalanceAuton(Swerve swerveSubsystem, Spinner spinnerSubsystem, Pneumatics pneumaticsSubsystem) {
+  public PlaceAndBalanceAuton(Swerve swerveSubsystem, Spinner spinnerSubsystem, Pneumatics pneumaticsSubsystem, Kinematics kinematics, AHRS gyro) {
 
     super (
       new ShootAuton(spinnerSubsystem),
-      new BalanceAuton(swerveSubsystem, pneumaticsSubsystem)
+      new BalanceAuton(swerveSubsystem, pneumaticsSubsystem, kinematics, gyro)
     );
   }
 

src/main/java/frc/robot/commands/Auton/framework/AutonDrive.java

@@ -4,44 +4,71 @@
 
 package frc.robot.commands.Auton.framework;
 
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Swerve;
+import frc.robot.util.Kinematics;
 
 public class AutonDrive extends CommandBase {
 
   Swerve swerveSubsystem;
+  Kinematics kinematics;
+  AHRS gyro;
   Timer timer;
-  double timeInSeconds, xSpeedMPS, ySpeedMPS, thetaSpeedMPS;
+  double timeInSeconds, xSpeedMPS, ySpeedMPS, thetaSpeedMPS, desiredRotation;
+  Translation2d desiredPosition;
+  PIDController xPID, yPID, thetaPID;
 
-  public AutonDrive(Swerve swerveSubsystem, double timeInSeconds, double xSpeedMPS, double ySpeedMPS, double thetaSpeedMPS) {
+  public AutonDrive(Swerve swerveSubsystem, Kinematics kinematics, Translation2d desiredPosition, AHRS gyro, double desiredRotation, double timeInSeconds/**, double xSpeedMPS, double ySpeedMPS, double thetaSpeedMPS*/) {
     this.swerveSubsystem = swerveSubsystem;
+    this.kinematics = kinematics;
+    this.gyro = gyro;
+    this.desiredRotation = desiredRotation;
+    this.desiredPosition = desiredPosition;
     this.timer = new Timer();
     this.timeInSeconds = timeInSeconds;
+    xPID = new PIDController(0.05, 0, 0);
+    yPID = new PIDController(0.05, 0, 0);
+    thetaPID = new PIDController(0.05, 0, 0);
     addRequirements(swerveSubsystem);
   }
 
   @Override
   public void initialize() {
     /**Restarts the timer which is used to run the auton cycle. */
-    timer.restart();
+    // timer.restart();
     swerveSubsystem.stop();
   }
 
   @Override
   public void execute() {
+
 
+
+    swerveSubsystem.updateModules(new ChassisSpeeds(
+                  xPID.calculate(kinematics.getRobotDisplacementX(), desiredPosition.getX()), 
+                  yPID.calculate(kinematics.getRobotDisplacementY(), desiredPosition.getY()), 
+                  thetaPID.calculate(gyro.getAngle(), gyro.getAngle() + desiredRotation)), 
+                  1);
     /**Checks if the given time has passed since the timer has been 
      * reset. If not, run the updateModules() method to move the robot*/
-    if (!timer.hasElapsed(timeInSeconds)) swerveSubsystem.updateModules(new ChassisSpeeds(xSpeedMPS, ySpeedMPS, thetaSpeedMPS), 1);
-    else swerveSubsystem.stop();
+    // if (!timer.hasElapsed(timeInSeconds)) swerveSubsystem.updateModules(new ChassisSpeeds(xSpeedMPS, ySpeedMPS, thetaSpeedMPS), 1);
+    // else swerveSubsystem.stop();
+
+    if (timer.hasElapsed(timeInSeconds)) {
+      end(false);
+    }
   }
 
   @Override
   public void end(boolean interrupted) {
     swerveSubsystem.stop();
-    timer.stop();
+    // timer.stop();
   }
 
   @Override

src/main/java/frc/robot/commands/Auton/framework/BalanceAuton.java

@@ -4,23 +4,29 @@
 
 package frc.robot.commands.Auton.framework;
 
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Pneumatics;
 import frc.robot.subsystems.Swerve;
+import frc.robot.util.Kinematics;
 
 public class BalanceAuton extends CommandBase {
 
   Swerve swerveSubsystem;
+  AHRS gyro;
   AutonDrive autonDriveCommand;
   Timer timer;
   Pneumatics pneumaticsSubsystem;
 
-  public BalanceAuton(Swerve swerveSubsystem, Pneumatics pneumaticsSubsystem) {
+  public BalanceAuton(Swerve swerveSubsystem, Pneumatics pneumaticsSubsystem, Kinematics kinematics, AHRS gyro) {
     this.swerveSubsystem = swerveSubsystem;
     this.pneumaticsSubsystem = pneumaticsSubsystem;
-    autonDriveCommand = new AutonDrive(swerveSubsystem, 3, .25, 0, 0); //TODO: determine these values
+    this.gyro = gyro;
+    autonDriveCommand = new AutonDrive(swerveSubsystem, kinematics, new Translation2d(), gyro, 0, 10); //TODO: determine these values
     timer = new Timer();
     addRequirements(swerveSubsystem, pneumaticsSubsystem);
   }

src/main/java/frc/robot/commands/Auton/framework/TurnAroundAuton.java

@@ -4,19 +4,17 @@
 
 package frc.robot.commands.Auton.framework;
 
+import com.kauailabs.navx.frc.AHRS;
+
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.Swerve;
+import frc.robot.util.Kinematics;
 
 public class TurnAroundAuton extends SequentialCommandGroup {
-
-  Swerve swerveSubsystem;
-
-  AutonDrive autonDrive;
-
-  Timer timer;
 
-  public TurnAroundAuton(Swerve swerveSubsystem) {
-    super (new AutonDrive(swerveSubsystem, 1, 0, 0, 1));
+  public TurnAroundAuton(Swerve swerveSubsystem, Kinematics kinematics, AHRS gyro) {
+    super (new AutonDrive(swerveSubsystem, kinematics, new Translation2d(), gyro, 180, 3)); //TODO: Determine these values
   }
 }

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

@@ -99,22 +99,16 @@ public void execute() {
       absoluteNextYSpeed < DriveCommand.Y_DEADBAND &&
       absoluteNextThetaSpeed < DriveCommand.THETA_DEADBAND
     ) {
-
       nextXSpeed = 0;
       nextYSpeed = 0;
       nextThetaSpeed = 0;
-
     } else {
-
       // Enforce maximum change in acceleration.
       nextXSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentXSpeed, nextXSpeed, X_MAX_ACCELERATION);
       nextYSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentYSpeed, nextYSpeed, Y_MAX_ACCELERATION);
       nextThetaSpeed = ControlsUtilities.enforceMaximumPositiveDelta(currentThetaSpeed, nextThetaSpeed, THETA_MAX_ACCELERATION);
-
     }
 
-
-
     this.speedMultiplier = slowMode.getAsBoolean() ? 0.25 : 1;
 
     if (turnAround.getAsBoolean() && !wasOneEighty) {

src/main/java/frc/robot/subsystems/SwerveModule.java

@@ -15,7 +15,6 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.util.AccelerationCurve;
 
 public class SwerveModule extends SubsystemBase {
 
@@ -48,7 +47,7 @@ public class SwerveModule extends SubsystemBase {
 	 * wheel, to calculate an approximate voltage value when given a speed in
 	 * meters per second. 
 	 */
-	private double maxSpeed = (5500 / 60.) * .1 / 10;
+	private double maxSpeed = (5500 / 60.) * .1 / 2;
 
 	double finalAngle;
 

src/main/java/frc/robot/util/ControlsUtilities.java

@@ -24,6 +24,18 @@ public static double applyDeadband(double input, double deadband) {
 		return Math.abs(input) < deadband ? 0 : input;
 
 	}
+
+	public static double applyCircularDeadband (double xInput, double yInput, double deadband, boolean valueToRetrieve) {
+
+		double hypotenuse = Math.sqrt(Math.pow(xInput, 2) + Math.pow(yInput, 2));
+
+		if (Math.abs(hypotenuse) < deadband) {
+			if (valueToRetrieve) return xInput;
+			else return yInput;
+		}
+
+		else return 0;
+	}
 
 	/**
 	 * Returns the new value so long as its delta from the old value does not