Merge pull request #19 from GreenBlitz/auto-aling

Yoav - Auto align

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Bindings.java

@@ -3,6 +3,7 @@
 import com.arcrobotics.ftclib.gamepad.GamepadKeys;
 import com.qualcomm.robotcore.hardware.Gamepad;
 
+import org.firstinspires.ftc.teamcode.gamelayout.FieldConstants;
 import org.firstinspires.ftc.teamcode.gamepads.GamepadFunctions;
 import org.firstinspires.ftc.teamcode.gamepads.GamepadWrapper;
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmCommands;
@@ -90,13 +91,14 @@ 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(FieldConstants.getBoardAngle()));
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamelayout/FieldConstants.java

@@ -0,0 +1,15 @@
+package org.firstinspires.ftc.teamcode.gamelayout;
+
+import com.arcrobotics.ftclib.geometry.Rotation2d;
+import org.firstinspires.ftc.teamcode.Alliance;
+import org.firstinspires.ftc.teamcode.Robot;
+
+public class FieldConstants {
+
+    private static final Rotation2d BOARD_ANGLE = Rotation2d.fromDegrees(90);
+
+    public static Rotation2d getBoardAngle() {
+        return Robot.getInstance().getAlliance() == Alliance.RED ? BOARD_ANGLE.unaryMinus() : BOARD_ANGLE;
+    }
+
+}

TeamCode/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,11 +24,22 @@ 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(),
+                interrupt -> {},
+                () -> Robot.getInstance().getChassis().isAtAngle(angle),
+                Robot.getInstance().getChassis()
+        );
+    }
+
     public static Command stop() {
         return new InstantCommand(() -> Robot.getInstance().getChassis().stop(), Robot.getInstance().getChassis());
     }
 
     public static Command resetHeading() {
         return new InstantCommand(() -> Robot.getInstance().getChassis().resetHeading(), Robot.getInstance().getChassis());
     }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/chassis/ChassisConstants.java

@@ -1,10 +1,15 @@
 package org.firstinspires.ftc.teamcode.subsystems.chassis;
 
+import com.arcrobotics.ftclib.controller.PIDController;
+
 public class ChassisConstants {
 
     public static final String FRONT_LEFT_ID = "fl";
     public static final String FRONT_RIGHT_ID = "fr";
     public static final String BACK_LEFT_ID = "bl";
     public static final String BACK_RIGHT_ID = "br";
 
+    protected static final double ANGLE_TOLERANCE = 5;
+    protected static final PIDController PID_CONTROLLER = new PIDController(0.01, 0, 0);
+
 }

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,6 +17,10 @@ public class MecanumChassis extends SubsystemBase {
 
     private final RevIMU imu;
 
+    private final PIDController pidController;
+
+    private ChassisSpeeds currentSpeeds;
+
     public MecanumChassis(HardwareMap hardwareMap) {
         Motor frontLeft = getConfiguredChassisMotor(hardwareMap, ChassisConstants.FRONT_LEFT_ID);
         Motor frontRight = getConfiguredChassisMotor(hardwareMap, ChassisConstants.FRONT_RIGHT_ID);
@@ -22,6 +29,8 @@ public MecanumChassis(HardwareMap hardwareMap) {
 
         this.mecanumDrive = new MecanumDrive(frontLeft, frontRight, backLeft, backRight);
         this.imu = new RevIMU(hardwareMap);
+        this.pidController = ChassisConstants.PID_CONTROLLER;
+        this.currentSpeeds = new ChassisSpeeds();
         imu.init();
     }
 
@@ -31,12 +40,30 @@ private Motor getConfiguredChassisMotor(HardwareMap hardwareMap, String id) {
         return chassisMotor;
     }
 
+    protected void setRotateSetPoint(Rotation2d angle) {
+        pidController.reset();
+        pidController.setSetPoint(angle.getDegrees());
+    }
+
+    protected boolean isAtAngle(Rotation2d angle) {
+        return Math.abs(imu.getRotation2d().getDegrees() - angle.getDegrees()) <= ChassisConstants.ANGLE_TOLERANCE;
+    }
+
+    private void angleRelativeDrive(double strafeSpeed, double forwardSpeed, double turnSpeed, Rotation2d angle) {
+        currentSpeeds = new ChassisSpeeds(strafeSpeed, forwardSpeed, turnSpeed);
+        mecanumDrive.driveFieldCentric(strafeSpeed, forwardSpeed, turnSpeed, angle.getDegrees());
+    }
+
+    protected void rotateToAngle() {
+        fieldCentricDrive(0, 0, pidController.calculate(imu.getRotation2d().getDegrees()));
+    }
+
     protected void fieldCentricDrive(double strafeSpeed, double forwardSpeed, double turnSpeed) {
-        mecanumDrive.driveFieldCentric(strafeSpeed, forwardSpeed, turnSpeed, imu.getRotation2d().getDegrees());
+        angleRelativeDrive(strafeSpeed, forwardSpeed, turnSpeed, imu.getRotation2d());
     }
 
     protected void robotCentricDrive(double strafeSpeed, double forwardSpeed, double turnSpeed) {
-        mecanumDrive.driveRobotCentric(strafeSpeed, forwardSpeed, turnSpeed);
+        angleRelativeDrive(strafeSpeed, forwardSpeed, turnSpeed, new Rotation2d());
     }
 
     protected void stop() {
@@ -49,6 +76,9 @@ protected void resetHeading() {
 
     public void telemetry(Telemetry telemetry) {
         telemetry.addData("Robot/Chassis angle degrees: ", imu.getRotation2d().getDegrees());
+        telemetry.addData("Robot/Chassis x velocity: ", currentSpeeds.vxMetersPerSecond);
+        telemetry.addData("Robot/Chassis y velocity: ", currentSpeeds.vyMetersPerSecond);
+        telemetry.addData("Robot/Chassis omega: ", currentSpeeds.omegaRadiansPerSecond);
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/elevator/Elevator.java

@@ -7,7 +7,7 @@
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
-import org.firstinspires.ftc.teamcode.ControlMode;
+import org.firstinspires.ftc.teamcode.systemcontrol.ControlMode;
 
 public class Elevator extends SubsystemBase {
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ControlMode.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/systemcontrol/ControlMode.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode;
+package org.firstinspires.ftc.teamcode.systemcontrol;
 
 public enum ControlMode {