From b537d04c52ffbb061ea0cba9dbcaa56ebd849a59 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Fri, 8 Dec 2023 20:53:44 -0600 Subject: [PATCH] Improvements to Red Crab Pilot --- .../minibots/red_crab/RedCrabMinibot.java | 10 ++++--- .../minibots/red_crab/states/Pilot.java | 26 +++++++++++++------ 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 060f845..9ba3884 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -15,9 +15,11 @@ import dev.cyberarm.engine.V2.Utilities; public class RedCrabMinibot { /// CLAW ARM /// + public static final int ClawArm_INITIAL = -1; // NO OP match starting position public static final int ClawArm_STOW = 0; public static final int ClawArm_DEPOSIT = 1; - public static final int ClawArm_COLLECT = 2; + public static final int ClawArm_COLLECT_FLOAT = 2; + public static final int ClawArm_COLLECT = 3; /// TUNING CONSTANTS /// public static final double DRIVETRAIN_MAX_SPEED = 0.5; @@ -25,12 +27,14 @@ public class RedCrabMinibot { public static final double CLAW_ARM_kP = 0.025; public static final double CLAW_ARM_POSITION_TOLERANCE = 3.3; public static final double WINCH_MAX_SPEED = 0.5; - public static final double CLAW_ARM_STOW_ANGLE = 45.0; - public static final double CLAW_ARM_DEPOSIT_ANGLE = 110.0; // 130.0 + public static final double CLAW_ARM_STOW_ANGLE = 45.0; // 45.0 + public static final double CLAW_ARM_DEPOSIT_ANGLE = 130.0; // 110.0 + public static final double CLAW_ARM_COLLECT_FLOAT_ANGLE = 180.0; public static final double CLAW_ARM_COLLECT_ANGLE = 200.0; public static final double CLAW_WRIST_STOW_POSITION = 0.5; public static final double CLAW_WRIST_DEPOSIT_POSITION = 0.64; + public static final double CLAW_WRIST_COLLECT_FLOAT_POSITION = 0.64; public static final double CLAW_WRIST_COLLECT_POSITION = 0.64; public static final double CLAW_LEFT_CLOSED_POSITION = 0.2; diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java index 8c0cea2..4db680d 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Pilot.java @@ -13,7 +13,7 @@ public class Pilot extends CyberarmState { private boolean leftClawOpen = false; private boolean rightClawOpen = false; - private int clawArmPosition = RedCrabMinibot.ClawArm_STOW; + private int clawArmPosition = RedCrabMinibot.ClawArm_INITIAL; private boolean hookArmUp = false; private boolean droneLaunchAuthorized = false; private boolean droneLaunchRequested = false; @@ -61,11 +61,13 @@ public class Pilot extends CyberarmState { clawArmPosition = RedCrabMinibot.ClawArm_COLLECT; break; case "dpad_down": - clawArmPosition = RedCrabMinibot.ClawArm_DEPOSIT; + clawArmPosition = RedCrabMinibot.ClawArm_STOW; break; case "dpad_left": + clawArmPosition = RedCrabMinibot.ClawArm_DEPOSIT; + break; case "dpad_right": - clawArmPosition = RedCrabMinibot.ClawArm_STOW; + clawArmPosition = RedCrabMinibot.ClawArm_COLLECT_FLOAT; break; } } @@ -123,11 +125,11 @@ public class Pilot extends CyberarmState { private void clawArmAndWristController() { switch (clawArmPosition) { case RedCrabMinibot.ClawArm_STOW: -// robot.clawArm.setTargetPosition(Utilities.motorAngle( -// RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, -// RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, -// RedCrabMinibot.CLAW_ARM_STOW_ANGLE)); -// + robot.clawArm.setTargetPosition(Utilities.motorAngle( + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, + RedCrabMinibot.CLAW_ARM_STOW_ANGLE)); + robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_STOW_POSITION); break; case RedCrabMinibot.ClawArm_DEPOSIT: @@ -138,6 +140,14 @@ public class Pilot extends CyberarmState { robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_DEPOSIT_POSITION); break; + case RedCrabMinibot.ClawArm_COLLECT_FLOAT: + robot.clawArm.setTargetPosition(Utilities.motorAngle( + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, + RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE)); + + robot.clawWrist.setPosition(RedCrabMinibot.CLAW_WRIST_COLLECT_FLOAT_POSITION); + break; case RedCrabMinibot.ClawArm_COLLECT: robot.clawArm.setTargetPosition(Utilities.motorAngle( RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION,