Romy - uncommited code..

GreenBlitz · Jun 17, 2024 · 3c165f8 · 3c165f8
1 parent 96174b2
commit 3c165f8
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 3 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bindings.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bindings.java
@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.arcrobotics.ftclib.geometry.Rotation2d;
 import com.qualcomm.robotcore.hardware.Gamepad;
 
 import org.firstinspires.ftc.teamcode.gamepads.GamepadFunctions;
@@ -78,13 +79,15 @@ public static void razChassisTest(Gamepad gamepad) {
 
         Robot.getInstance().getChassis().setDefaultCommand(
                 ChassisCommands.fieldCentricDrive(
-                        () -> -mainGamepad.getLeftX(),
-                        () -> -mainGamepad.getLeftY(),
-                        () -> -mainGamepad.getRightX()
+                        () -> -GamepadFunctions.getDeadZonedSensitiveValue(mainGamepad.getLeftX()),
+                        () -> -GamepadFunctions.getDeadZonedSensitiveValue(mainGamepad.getLeftY()),
+                        () -> -GamepadFunctions.getDeadZonedSensitiveValue(mainGamepad.getRightX())
                 )
         );
         mainGamepad.getGamepadButton(GamepadKeys.Button.Y).whenPressed(ChassisCommands.resetHeading());
         mainGamepad.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(ChassisCommands.stop());
+        mainGamepad.getGamepadButton(GamepadKeys.Button.A).whileHeld(ChassisCommands.rotateToAngle(Rotation2d.fromDegrees(90)));
+        mainGamepad.getGamepadButton(GamepadKeys.Button.B).whileHeld(ChassisCommands.rotateToAngle(Rotation2d.fromDegrees(-90)));
     }
 
 }
diff --git a/...Code/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chassis/ChassisCommands.java b/...Code/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chassis/ChassisCommands.java
@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.subsystems.chassis;
 
 import com.arcrobotics.ftclib.command.Command;
+import com.arcrobotics.ftclib.command.FunctionalCommand;
 import com.arcrobotics.ftclib.command.InstantCommand;
 import com.arcrobotics.ftclib.command.RunCommand;
 import com.arcrobotics.ftclib.geometry.Rotation2d;
@@ -23,6 +24,16 @@ public static Command fieldCentricDrive(Supplier<Double> strafeSpeedSupplier, Su
         );
     }
 
+    public static Command rotateToAngle(Rotation2d angle){
+        return new FunctionalCommand(
+                () -> Robot.getInstance().getChassis().setRotateSetPoint(angle),
+                () -> Robot.getInstance().getChassis().rotateToAngle(),
+                i -> {},
+                () -> Robot.getInstance().getChassis().isAtAngle(angle),
+                Robot.getInstance().getChassis()
+        );
+    }
+
     public static Command stop() {
         return new InstantCommand(() -> Robot.getInstance().getChassis().stop(), Robot.getInstance().getChassis());
     }

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chassis/MecanumChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chassis/MecanumChassis.java
@@ -1,9 +1,12 @@
 package org.firstinspires.ftc.teamcode.subsystems.chassis;
 
 import com.arcrobotics.ftclib.command.SubsystemBase;
+import com.arcrobotics.ftclib.controller.PIDController;
 import com.arcrobotics.ftclib.drivebase.MecanumDrive;
+import com.arcrobotics.ftclib.geometry.Rotation2d;
 import com.arcrobotics.ftclib.hardware.RevIMU;
 import com.arcrobotics.ftclib.hardware.motors.Motor;
+import com.arcrobotics.ftclib.kinematics.wpilibkinematics.ChassisSpeeds;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -14,14 +17,20 @@ public class MecanumChassis extends SubsystemBase {
 
     private final RevIMU imu;
 
+    private final PIDController pidController;
+
+    private ChassisSpeeds a;
+
     public MecanumChassis(HardwareMap hardwareMap) {
+        a = new ChassisSpeeds();
         Motor frontLeft = getConfiguredChassisMotor(hardwareMap, ChassisConstants.FRONT_LEFT_ID);
         Motor frontRight = getConfiguredChassisMotor(hardwareMap, ChassisConstants.FRONT_RIGHT_ID);
         Motor backLeft = getConfiguredChassisMotor(hardwareMap, ChassisConstants.BACK_LEFT_ID);
         Motor backRight = getConfiguredChassisMotor(hardwareMap, ChassisConstants.BACK_RIGHT_ID);
 
         this.mecanumDrive = new MecanumDrive(frontLeft, frontRight, backLeft, backRight);
         this.imu = new RevIMU(hardwareMap);
+        this.pidController = new PIDController(0.01, 0, 0);
         imu.init();
     }
 
@@ -31,7 +40,21 @@ private Motor getConfiguredChassisMotor(HardwareMap hardwareMap, String id) {
         return chassisMotor;
     }
 
+    protected void setRotateSetPoint(Rotation2d angle) {
+        pidController.reset();
+        pidController.setSetPoint(angle.getDegrees());
+    }
+
+    protected void rotateToAngle() {
+        fieldCentricDrive(0, 0, pidController.calculate(imu.getRotation2d().getDegrees()));
+    }
+
+    protected boolean isAtAngle(Rotation2d angle){
+        return Math.abs(imu.getRotation2d().getDegrees() - angle.getDegrees()) <= 10;
+    }
+
     protected void fieldCentricDrive(double strafeSpeed, double forwardSpeed, double turnSpeed) {
+        a = new ChassisSpeeds(strafeSpeed, forwardSpeed, turnSpeed);
         mecanumDrive.driveFieldCentric(strafeSpeed, forwardSpeed, turnSpeed, imu.getRotation2d().getDegrees());
     }
 
@@ -49,6 +72,7 @@ protected void resetHeading() {
 
     public void telemetry(Telemetry telemetry) {
         telemetry.addData("Robot/Chassis angle degrees: ", imu.getRotation2d().getDegrees());
+        telemetry.addData("Robot/Chassis angle vel: ", a.omegaRadiansPerSecond);
     }
 
 }