diff --git a/src/main/java/com/team2502/robot2023/Constants.java b/src/main/java/com/team2502/robot2023/Constants.java index b8c066f..d061e6c 100644 --- a/src/main/java/com/team2502/robot2023/Constants.java +++ b/src/main/java/com/team2502/robot2023/Constants.java @@ -355,6 +355,10 @@ public static final class Drivetrain { public static final double MAX_ROT = 3; // driver rotation gain (rad/s) 9 - 4 public static final double RET_VEL = 1; // driver speed gain (m/s) public static final double RET_ROT = 0.25; // driver rotation gain (rad/s) + public static final double OI_DEADZONE_XY = 0.02; // Joystick deadzone + public static final double OI_DEADZONE_Z = 0.03; // Joystick twist deadzone + public static final double DEADZONE_XY = 0.05; // Transpose deadzone + public static final double DEADZONE_THETA = 0.05; // Rotation deadzone // constants for pose control public static final double DRIVETRAIN_MOVE_P = 2.7; // 0.6 diff --git a/src/main/java/com/team2502/robot2023/Utils.java b/src/main/java/com/team2502/robot2023/Utils.java index 26ea5f8..f171864 100644 --- a/src/main/java/com/team2502/robot2023/Utils.java +++ b/src/main/java/com/team2502/robot2023/Utils.java @@ -103,4 +103,44 @@ public static double findDist(double camHeight, double camElevation, double bask Math.tan(Math.toRadians(targetElevation+ camElevation)) ); } + + /** + * map x from one deadzone to another + * @param minInput start of input deadzone + * @param minOutput start of mapped deadzone + * @param x value to map + * @param scale gain + * @return transformed x + */ + public static double deadzone(double minInput, double minOutput, double x, double scale) { + boolean sign = x > 0.0; + x = sign ? x : -x; + double inputRange = 1.0 - minInput; + double outpuRange = 1.0 - minOutput; + + // below input deadzone return 0 + if (x < minInput) { + return 0; + } + + // this is some kind of affine transformation + double inputAdjusted = (x-minInput) * inputRange; + + inputAdjusted *= scale; + + double outputAjusted = inputAdjusted / outpuRange + minOutput; + + return sign ? outputAjusted : -outputAjusted; + } + + /** + * map x from one deadzone to another + * @param minInput start of input deadzone + * @param minOutput start of mapped deadzone + * @param x value to map + * @return transformed x + */ + public static double deadzone(double minInput, double minOutput, double x) { + return deadzone(minInput, minOutput, x, 1.0); + } } diff --git a/src/main/java/com/team2502/robot2023/autonomous/Autos.java b/src/main/java/com/team2502/robot2023/autonomous/Autos.java index 5b6be71..23d827a 100644 --- a/src/main/java/com/team2502/robot2023/autonomous/Autos.java +++ b/src/main/java/com/team2502/robot2023/autonomous/Autos.java @@ -36,6 +36,18 @@ public enum Autos { // first auto is default new FollowPathAbsoluteCommand(d, "testpath") )), + PLACE_CUBE("P", (d,i,e) -> Commands.sequence( + new InstantCommand(d::resetPitch), + new InstantCommand(d::resetHeading), + new InstantCommand(() -> d.setPose(new Pose2d(1.75, 4.45, Rotation2d.fromDegrees(0))), d), + new InstantCommand(() -> i.setSpeed(0.2)), + Commands.deadline(Commands.waitSeconds(2.25), new SetArmSimpleCommand(e, ElevatorPosition.CUBE_TOP, IntakePosition.CUBE_TOP)), + Commands.deadline(Commands.waitSeconds(0.125), new InstantCommand(() -> i.setSpeed(-0.25))), + Commands.deadline(new InstantCommand(() -> i.setSpeed(0))), + Commands.deadline(Commands.waitSeconds(1.5), new SetArmSimpleCommand(e, ElevatorPosition.BOTTOM, IntakePosition.CUBE_GROUND)), + Commands.deadline(new InstantCommand(() -> i.setSpeed(0.5))) + )), + PLACE_CUBE_GRAB_CUBE("PP left", (d,i,e) -> Commands.sequence( new InstantCommand(d::resetPitch), new InstantCommand(d::resetHeading), diff --git a/src/main/java/com/team2502/robot2023/commands/DriveCommand.java b/src/main/java/com/team2502/robot2023/commands/DriveCommand.java index 93bb5f8..1b66267 100644 --- a/src/main/java/com/team2502/robot2023/commands/DriveCommand.java +++ b/src/main/java/com/team2502/robot2023/commands/DriveCommand.java @@ -6,6 +6,7 @@ import com.team2502.robot2023.Constants.OI; import com.team2502.robot2023.Constants.Subsystems.Drivetrain; import com.team2502.robot2023.subsystems.DrivetrainSubsystem; +import com.team2502.robot2023.Utils; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -31,6 +32,8 @@ private enum Drivetype { FieldOriented, RobotOriented, FieldOrientedTwist, + FieldOrientedTwistRet, + FieldOrientedTwistRetDead, FieldOrientedTwistPow, RobotOrientedCenteredRot, VirtualTank, @@ -40,7 +43,7 @@ private enum Drivetype { private double dlxDrift; private double dlyDrift; - private boolean tgoggla = false; + private boolean slowmode = false; private boolean prevTog = false; private final SendableChooser typeEntry = new SendableChooser<>(); @@ -58,7 +61,9 @@ public DriveCommand(DrivetrainSubsystem drivetrain, Joystick joystickDriveLeft, typeEntry.addOption("nolan mode", Drivetype.RobotOrientedCenteredRot); typeEntry.addOption("Robot Oriented", Drivetype.RobotOriented); typeEntry.addOption("Field Pow", Drivetype.FieldOrientedTwistPow); - typeEntry.setDefaultOption("Field Twist", Drivetype.FieldOrientedTwist); + typeEntry.addOption("Field Twist", Drivetype.FieldOrientedTwist); + typeEntry.addOption("Super Loud Olympic Winning", Drivetype.FieldOrientedTwistRet); + typeEntry.setDefaultOption("Dead", Drivetype.FieldOrientedTwistRetDead); SmartDashboard.putData("Drive Type", typeEntry); controllerEntry.addOption("Joystick", DriveController.Joystick); @@ -76,11 +81,11 @@ public void execute() { boolean tog = leftJoystick.getRawButton(OI.RET_MODE); if (prevTog != tog && tog) { - tgoggla = !tgoggla; + slowmode = !slowmode; } prevTog = tog; - SmartDashboard.putBoolean("iosajioj", tgoggla); + SmartDashboard.putBoolean("iosajioj", slowmode); ChassisSpeeds speeds; Translation2d centerOfRotation; @@ -117,9 +122,27 @@ public void execute() { break; case FieldOrientedTwist: speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - leftJoystick.getY() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), - -leftJoystick.getX() * (tgoggla ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), - -rightJoystick.getZ() * (tgoggla ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT), + leftJoystick.getY() * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), + -leftJoystick.getX() * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), + -rightJoystick.getZ() * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT), + Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset)); + centerOfRotation = new Translation2d(0, 0); + drivetrain.setSpeeds(speeds, centerOfRotation); + break; + case FieldOrientedTwistRet: + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.08, leftJoystick.getY()) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL) * 0.65, + -Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.08, leftJoystick.getX()) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL) * 0.65, + -Utils.deadzone(Drivetrain.OI_DEADZONE_Z, 0.07, rightJoystick.getZ()) * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT) * 0.65, + Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset)); + centerOfRotation = new Translation2d(0, 0); + drivetrain.setSpeeds(speeds, centerOfRotation); + break; + case FieldOrientedTwistRetDead: + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getY(), 0.45 * 0.6) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), + -Utils.deadzone(Drivetrain.OI_DEADZONE_XY, 0.04, leftJoystick.getX(), 0.45 * 0.6) * (slowmode ? Drivetrain.RET_VEL : Drivetrain.MAX_VEL), + -Utils.deadzone(Drivetrain.OI_DEADZONE_Z, 0.04, rightJoystick.getZ(), 0.55 * 0.6) * (slowmode ? Drivetrain.RET_ROT : Drivetrain.MAX_ROT), Rotation2d.fromDegrees(drivetrain.getHeading()+drivetrain.fieldOrientedOffset)); centerOfRotation = new Translation2d(0, 0); drivetrain.setSpeeds(speeds, centerOfRotation);