Merge branch 'main' of https://github.com/redshiftrobotics/crescendo-…

…2024

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

@@ -45,6 +45,13 @@ public static class DriverConstants {
         public static final double[] maxSpeedOptionsRotation = {1, 2, 4};
     }
 
+    public static class ControllerConstants {
+        public static final double maxSpeed = 5;
+        public static final double CONTROLLER_PID_P = 0.5;
+        public static final double CONTROLLER_PID_I = 0;
+        public static final double CONTROLLER_PID_D = 0.5;
+    }
+
     public static class OperatorConstants {
         public static final int OPERATOR_JOYSTICK_PORT = 1;
     }

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

@@ -1,4 +1,6 @@
 package frc.robot.commands;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -23,7 +25,6 @@ public AutoDriveForward(SwerveDrivetrain drivetrain) {
     public void initialize() {
         ChassisSpeeds desiredState = new ChassisSpeeds(0.5,0,0);
         this.beganDriving = System.currentTimeMillis();
-
         drivetrain.setDesiredState(desiredState);
     }
 

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

@@ -0,0 +1,5 @@
+package frc.robot.commands;
+
+public class RotateTo {
+
+}

src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java

@@ -1,9 +1,12 @@
 package frc.robot.commands.SwerveRemoteOperation;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.Constants.ControllerConstants;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 
@@ -16,6 +19,8 @@
 public class SwerveDriveXboxControl extends Command {
     private final SwerveDrivetrain drivetrain;
     private final CommandXboxController controller;
+    private final PIDController xboxPID;    
+
 
     /**
 	 * Creates a new SwerveDriveXboxControl Command.
@@ -27,6 +32,14 @@ public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController
         this.drivetrain = drivetrain;
         this.controller = driverXboxController;
 
+        //not sure if this code in right place
+        xboxPID = new PIDController(
+            ControllerConstants.CONTROLLER_PID_P,
+            ControllerConstants.CONTROLLER_PID_I,
+            ControllerConstants.CONTROLLER_PID_D
+        );
+        xboxPID.enableContinuousInput(0, 2*Math.PI);
+
         // Create and configure buttons
         // OptionButton exampleToggleButton = new OptionButton(controller::a, ActivationMode.TOGGLE);
 
@@ -47,14 +60,29 @@ public void initialize() {
     /**
      * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
      */
+
+
     @Override
     public void execute() {
         double leftX = controller.getLeftX();
         double leftY = controller.getLeftY();
-
         double rightX = controller.getRightX();
         double rightY = controller.getRightY();
+        double angleGoal= Math.atan2(rightX,rightY);
 
+        final double currentAngle = drivetrain.getHeading().getRadians();
+        final double turnspeed = xboxPID.calculate(currentAngle,angleGoal);
+
+        final ChassisSpeeds speeds = new ChassisSpeeds(
+            leftX * ControllerConstants.maxSpeed,
+            leftY * ControllerConstants.maxSpeed,
+            turnspeed
+        );
+
+
+
+        drivetrain.setDesiredState(speeds);
+
     }
 
     /**