From ad68fbb7c07eb25a8edb4de12e4666dd3239408c Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 30 Dec 2023 13:21:49 -0600 Subject: [PATCH 1/2] Removed rotation from openCV frame processor, got claw arm working as intended, winch now locks position when trigger is released, wip: toggling between claw arm and winch manual control (disabled for now), fixed claw arm deposit angle not correctly set from config (was using wrong key), updated hook arm hooking position. --- .../minibots/red_crab/RedCrabMinibot.java | 18 +++++- .../SpikeMarkDetectorVisionProcessor.java | 4 +- .../red_crab/TeamPropVisionProcessor.java | 4 +- .../minibots/red_crab/cyberarm_RedCrab.json | 2 +- .../RedCrabTeleOpDebuggingOnlyEngine.java | 47 ++++++++++++++++ .../minibots/red_crab/states/Pilot.java | 56 +++++++++++++++++-- 6 files changed, 120 insertions(+), 11 deletions(-) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java 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 36fe364..78e139e 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 @@ -45,6 +45,7 @@ public class RedCrabMinibot { public static double CLAW_ARM_MAX_SPEED = 0.5; public static double CLAW_ARM_MAX_VELOCITY_DEGREES = 10; + private static double CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = 1588.0; public static double CLAW_ARM_kP = 0.0; public static double CLAW_ARM_kI = 0.0; public static double CLAW_ARM_kD = 0.0; @@ -181,6 +182,7 @@ public class RedCrabMinibot { winch.setDirection(DcMotorSimple.Direction.FORWARD); /// --- RUN MODE + winch.setTargetPosition(winch.getCurrentPosition()); winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); /// CLAW and Co. /// @@ -249,12 +251,13 @@ public class RedCrabMinibot { RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES = config.variable("Robot", "ClawArm_Tuning", "max_velocityDEGREES").value(); RedCrabMinibot.CLAW_ARM_POSITION_TOLERANCE = config.variable("Robot", "ClawArm_Tuning", "tolerance").value(); RedCrabMinibot.CLAW_ARM_STOW_ANGLE = config.variable("Robot", "ClawArm_Tuning", "stow_angle").value(); - RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_float_angle").value(); + RedCrabMinibot.CLAW_ARM_DEPOSIT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "deposit_angle").value(); RedCrabMinibot.CLAW_ARM_COLLECT_FLOAT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value(); RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE = config.variable("Robot", "ClawArm_Tuning", "collect_angle").value(); RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO = config.variable("Robot", "ClawArm_Tuning", "gear_ratio").value(); RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION = config.variable("Robot", "ClawArm_Tuning", "motor_ticks").value(); + RedCrabMinibot.CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS = config.variable("Robot", "ClawArm_Tuning", "max_current_milliamps").value(); /// WINCH @@ -362,6 +365,7 @@ public class RedCrabMinibot { PIDFCoefficients clawArmPIDFPosition = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION); PIDFCoefficients clawArmPIDFEncoder = clawArm.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER); + int clawArmError = clawArm.getCurrentPosition() - clawArm.getTargetPosition(); engine.telemetry.addData( "Claw Arm", "Power: %.2f, Current: %.2f mAmp, Position: %d, Angle: %.2f, Velocity: %.2f (%.2f degrees/s)", @@ -371,6 +375,16 @@ public class RedCrabMinibot { Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getCurrentPosition()), clawArm.getVelocity(), Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, (int)clawArm.getVelocity())); + engine.telemetry.addData( + "Claw Arm", + "TPos: %d, TAngle: %.2f, ErrPos: %d ErrAngle: %.2f", + clawArm.getTargetPosition(), + Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArm.getTargetPosition()), + clawArmError, + Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, clawArmError), + clawArm.getVelocity(), + Utilities.motorTicksToAngle(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, (int)clawArm.getVelocity())); + engine.telemetry.addData("Current Alarm?", clawArm.isOverCurrent()); engine.telemetry.addData( " PIDF", "P: %.4f, I: %.4f, D: %.4f, F: %.4f, PosP: %.4f", clawArmPIDFEncoder.p, @@ -432,6 +446,8 @@ public class RedCrabMinibot { clawArm.setVelocityPIDFCoefficients(CLAW_ARM_kP, CLAW_ARM_kI, CLAW_ARM_kD, CLAW_ARM_kF); clawArm.setPositionPIDFCoefficients(CLAW_ARM_kPosP); + clawArm.setCurrentAlert(CLAW_ARM_MOTOR_MAX_CURRENT_MILLIAMPS, CurrentUnit.MILLIAMPS); + double velocity = Utilities.motorAngleToTicks(CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, CLAW_ARM_MOTOR_GEAR_RATIO, CLAW_ARM_MAX_VELOCITY_DEGREES); clawArm.setVelocity(velocity); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java index 6cfd3fd..49d77cc 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/SpikeMarkDetectorVisionProcessor.java @@ -41,8 +41,8 @@ public class SpikeMarkDetectorVisionProcessor implements VisionProcessor { @Override public Object processFrame(Mat frame, long captureTimeNanos) { - Core.rotate(frame, rotatedMat,Core.ROTATE_180); - Imgproc.cvtColor(rotatedMat, hsvMat, Imgproc.COLOR_RGB2HSV); +// Core.rotate(frame, rotatedMat,Core.ROTATE_180); + Imgproc.cvtColor(frame, hsvMat, Imgproc.COLOR_RGB2HSV); saturation = averageSaturation(hsvMat, rect); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java index bb1fbdf..76076c8 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/TeamPropVisionProcessor.java @@ -49,8 +49,8 @@ public class TeamPropVisionProcessor implements VisionProcessor { @Override public Object processFrame(Mat frame, long captureTimeNanos) { - Core.rotate(frame, rotatedMat,Core.ROTATE_180); - Imgproc.cvtColor(rotatedMat, hsvMat, Imgproc.COLOR_RGB2HSV); +// Core.rotate(frame, rotatedMat,Core.ROTATE_180); + Imgproc.cvtColor(frame, hsvMat, Imgproc.COLOR_RGB2HSV); saturationLeft = averageSaturation(hsvMat, rectLeft); saturationCenter = averageSaturation(hsvMat, rectCenter); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json index d2d10a2..ef4f270 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/cyberarm_RedCrab.json @@ -1 +1 @@ -{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-20 19:51:07 -0600","spec_version":2,"revision":566},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"00-01","comment":"@ServoMove - Move wrist to position camera","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]},{"name":"02-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx415"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"03-00","comment":"@Rotate - Rotate robot to 0d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"04-00","comment":"@PathEnactor - Enact path; paths must return to \"origin\" when done","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"06-00","comment":"@Move - Move up to backdrop","enabled":true,"variables":[{"name":"distanceMM","value":"Dx800"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"07-00","comment":"@Rotate - Rotate robot to face backdrop","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"20-00","comment":"@Move - park in corner","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxtrue"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]}]},{"name":"Robot","actions":[{"name":"Claw_Tuning","comment":"Claws","enabled":true,"variables":[{"name":"claw_left_closed_position","value":"Dx0.2"},{"name":"claw_left_open_position","value":"Dx0.5"},{"name":"claw_right_closed_position","value":"Dx0.78"},{"name":"claw_right_open_position","value":"Dx0.5"}]},{"name":"ClawArm_Tuning","comment":"Claw Arm","enabled":true,"variables":[{"name":"collect_angle","value":"Dx200.0"},{"name":"collect_float_angle","value":"Dx180"},{"name":"deposit_angle","value":"Dx130.0"},{"name":"gear_ratio","value":"Dx72.0"},{"name":"kD","value":"Dx0.0"},{"name":"kF","value":"Dx0.0"},{"name":"kI","value":"Dx6.0"},{"name":"kP","value":"Dx25"},{"name":"KPosP","value":"Dx25"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx60"},{"name":"motor_ticks","value":"Ix4"},{"name":"stow_angle","value":"Dx45.0"},{"name":"tolerance","value":"Ix6"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.64"},{"name":"collect_position","value":"Dx0.64"},{"name":"deposit_position","value":"Dx0.64"},{"name":"stow_position","value":"Dx0.5"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx13.0321"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Ix28"},{"name":"velocity_max_in_mm","value":"Dx610"},{"name":"velocity_slow_in_mm","value":"Dx250"},{"name":"wheel_diameter_mm","value":"Dx90"}]},{"name":"DroneLauncher_Tuning","comment":"Drone Launcher","enabled":true,"variables":[{"name":"initial_position","value":"Dx0.5"},{"name":"launch_confirmation_time_ms","value":"Ix1000"},{"name":"launch_position","value":"Dx0"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.4"}]},{"name":"Winch_Tuning","comment":"Winch","enabled":true,"variables":[{"name":"max_speed","value":"Dx1.0"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","actions":[]},{"name":"zAutonomous_RED_Audience","actions":[]}],"presets":{"groups":[],"actions":[{"name":"ClawArmMove","comment":"@ClawArmMove - Move claw arm","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"Move","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"PathEnactor","comment":"@PathEnactor - Enact path","enabled":true,"variables":[]},{"name":"PathSelector","comment":"@PathSelector - Pick path","enabled":true,"variables":[{"name":"fallbackPath","value":"Ix0"},{"name":"minConfidence","value":"Dx0.7"},{"name":"timeoutMS","value":"Ix2000"}]},{"name":"Rotate","comment":"@Rotate - Rotate robot to face degree heading","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"ServoMove","comment":"@ServoMove - Move servo to position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix2000"}]}]}}} \ No newline at end of file +{"config":{"created_at":"2023-12-11 09:31:55 -0600","updated_at":"2023-12-30 12:42:26 -0600","spec_version":2,"revision":629},"data":{"groups":[{"name":"Autonomous_RED_Backstage","actions":[{"name":"00-00","comment":"@ClawArmMove - Move claw arm up high","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"00-01","comment":"@ServoMove - Move wrist to position camera","enabled":true,"variables":[{"name":"lerp","value":"Bxfalse"},{"name":"lerpMS","value":"Dx1"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix100"}]},{"name":"01-00","comment":"@PathSelector - Pick path; default Center","enabled":true,"variables":[{"name":"minConfidence","value":"Dx0.7"},{"name":"path","value":"Ix1"},{"name":"timeoutMS","value":"Ix200000"}]},{"name":"02-00","comment":"@Move - Move forward","enabled":true,"variables":[{"name":"distanceMM","value":"Dx415"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"03-00","comment":"@Rotate - Rotate robot to 0d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"04-00","comment":"@PathEnactor - Enact path; paths must return to \"origin\" when done","enabled":false,"variables":[]},{"name":"05-00","comment":"@Rotate - Rotate robot to 90d","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"06-00","comment":"@Move - Move up to backdrop","enabled":true,"variables":[{"name":"distanceMM","value":"Dx800"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"07-00","comment":"@Rotate - Rotate robot to face backdrop","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"20-00","comment":"@Move - park in corner","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxtrue"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]}]},{"name":"Robot","actions":[{"name":"Claw_Tuning","comment":"Claws","enabled":true,"variables":[{"name":"claw_left_closed_position","value":"Dx0.2"},{"name":"claw_left_open_position","value":"Dx0.5"},{"name":"claw_right_closed_position","value":"Dx0.78"},{"name":"claw_right_open_position","value":"Dx0.5"}]},{"name":"ClawArm_Tuning","comment":"Claw Arm","enabled":true,"variables":[{"name":"collect_angle","value":"Dx240"},{"name":"collect_float_angle","value":"Dx220"},{"name":"deposit_angle","value":"Dx165.0"},{"name":"gear_ratio","value":"Dx1"},{"name":"kD","value":"Dx1"},{"name":"kF","value":"Dx0.0"},{"name":"kI","value":"Dx3"},{"name":"kP","value":"Dx6"},{"name":"KPosP","value":"Dx1"},{"name":"max_current_milliamps","value":"Dx1588.0"},{"name":"max_speed","value":"Dx0.5"},{"name":"max_velocityDEGREES","value":"Dx60"},{"name":"motor_ticks","value":"Ix8192"},{"name":"stow_angle","value":"Dx18"},{"name":"tolerance","value":"Ix288"}]},{"name":"ClawWrist_Tuning","comment":"Claw Wrist","enabled":true,"variables":[{"name":"collect_float_position","value":"Dx0.51"},{"name":"collect_position","value":"Dx0.51"},{"name":"deposit_position","value":"Dx0.508"},{"name":"stow_position","value":"Dx0.74"}]},{"name":"Drivetrain_Tuning","comment":"Drivetrain","enabled":true,"variables":[{"name":"gear_ratio","value":"Dx13.0321"},{"name":"max_speed","value":"Dx0.5"},{"name":"motor_ticks","value":"Ix28"},{"name":"velocity_max_in_mm","value":"Dx610"},{"name":"velocity_slow_in_mm","value":"Dx250"},{"name":"wheel_diameter_mm","value":"Dx90"}]},{"name":"DroneLauncher_Tuning","comment":"Drone Launcher","enabled":true,"variables":[{"name":"initial_position","value":"Dx0.5"},{"name":"launch_confirmation_time_ms","value":"Ix1000"},{"name":"launch_position","value":"Dx0"}]},{"name":"HookArm_Tuning","comment":"Hook Arm","enabled":true,"variables":[{"name":"stow_position","value":"Dx0.8"},{"name":"up_position","value":"Dx0.35"}]},{"name":"Winch_Tuning","comment":"Winch","enabled":true,"variables":[{"name":"max_speed","value":"Dx1.0"}]}]},{"name":"zAutonomous_BLUE_Audience","actions":[]},{"name":"zAutonomous_BLUE_Backstage","actions":[]},{"name":"zAutonomous_RED_Audience","actions":[]}],"presets":{"groups":[],"actions":[{"name":"ClawArmMove","comment":"@ClawArmMove - Move claw arm","enabled":true,"variables":[{"name":"angle","value":"Dx0"},{"name":"power","value":"Dx0.5"},{"name":"timeoutMS","value":"Dx1000"},{"name":"toleranceAngle","value":"Dx1.5"}]},{"name":"Move","comment":"@Move - Move ta wheels matey!","enabled":true,"variables":[{"name":"distanceMM","value":"Dx300"},{"name":"lerpMM_DOWN","value":"Dx100"},{"name":"lerpMM_UP","value":"Dx100"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx250"},{"name":"strafe","value":"Bxfalse"},{"name":"timeoutMS","value":"Ix2000"},{"name":"toleranceMM","value":"Dx10"}]},{"name":"PathEnactor","comment":"@PathEnactor - Enact path","enabled":true,"variables":[]},{"name":"PathSelector","comment":"@PathSelector - Pick path","enabled":true,"variables":[{"name":"fallbackPath","value":"Ix0"},{"name":"minConfidence","value":"Dx0.7"},{"name":"timeoutMS","value":"Ix2000"}]},{"name":"Rotate","comment":"@Rotate - Rotate robot to face degree heading","enabled":true,"variables":[{"name":"headingDEGREES","value":"Dx90"},{"name":"lerpDEGREES","value":"Dx90.0"},{"name":"maxVelocityMM","value":"Dx610"},{"name":"minVelocityMM","value":"Dx50"},{"name":"timeoutMS","value":"Ix5000"},{"name":"toleranceDEGREES","value":"Dx0.5"}]},{"name":"ServoMove","comment":"@ServoMove - Move servo to position","enabled":true,"variables":[{"name":"lerp","value":"Bxtrue"},{"name":"lerpMS","value":"Dx1000"},{"name":"position","value":"Dx0.5"},{"name":"servoName","value":"SxleftClaw"},{"name":"timeoutMS","value":"Ix2000"}]}]}}} \ No newline at end of file diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java new file mode 100644 index 0000000..b564390 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpDebuggingOnlyEngine.java @@ -0,0 +1,47 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@TeleOp(name = "Cyberarm Red Crab TeleOp DEBUGGING", group = "MINIBOT") +public class RedCrabTeleOpDebuggingOnlyEngine extends CyberarmEngine { + @Override + public void setup() { + threadless(); + + addState(new CyberarmState() { + final RedCrabMinibot robot = new RedCrabMinibot(false); + + @Override + public void exec() { + double velocity = -engine.gamepad1.left_stick_y * Utilities.motorAngleToTicks( + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, + RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES); + +// robot.clawArm.setVelocity(velocity); + + robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.clawArm.setPower(-engine.gamepad1.left_stick_y * 0.5); + } + + @Override + public void telemetry() { + robot.standardTelemetry(); + } + }); + } + + @Override + public void loop() { + Utilities.hubsClearBulkReadCache(hardwareMap); + + super.loop(); + } +} 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 a4f4762..ccf20f3 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 @@ -1,5 +1,6 @@ package dev.cyberarm.minibots.red_crab.states; +import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -20,6 +21,9 @@ public class Pilot extends CyberarmState { private boolean droneLaunchRequested = false; private double droneLastLaunchRequestStartMS = 0; private boolean robotSlowMode = false; + private boolean triggersControlClawArm = false; + private double deltaTime = 0; + private double lastLoopTimeMS = 0; public Pilot(RedCrabMinibot robot) { this.robot = robot; @@ -27,6 +31,8 @@ public class Pilot extends CyberarmState { @Override public void exec() { + deltaTime = runTime() - lastLoopTimeMS; + drivetrain(); clawArmAndWristController(); @@ -34,6 +40,8 @@ public class Pilot extends CyberarmState { droneLatchController(); hookArmController(); // disabled for swrist debug winchController(); + + lastLoopTimeMS = runTime(); } @Override @@ -71,6 +79,17 @@ public class Pilot extends CyberarmState { case "dpad_right": clawArmPosition = RedCrabMinibot.ClawArm_COLLECT_FLOAT; break; + case "back": + // FIXME: trigger controls don't yet work well. + triggersControlClawArm = false; //!triggersControlClawArm; + + if (triggersControlClawArm) + { + engine.telemetry.speak("Claw Arm Manual"); + } else { + engine.telemetry.speak("Winch Manual"); + } + break; case "start": robot.reloadConfig(); break; @@ -140,6 +159,23 @@ public class Pilot extends CyberarmState { } private void clawArmAndWristController() { + if (triggersControlClawArm) + { + double triggerPower = engine.gamepad1.right_trigger - engine.gamepad1.left_trigger; + + robot.clawArm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + double maxVelocity = Utilities.motorAngleToTicks( + RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, + RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, + RedCrabMinibot.CLAW_ARM_MAX_VELOCITY_DEGREES); + robot.clawArm.setTargetPosition((int)(deltaTime * maxVelocity)); + + robot.clawArm.setVelocity(maxVelocity * triggerPower); + return; + } else { + robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION); + } + switch (clawArmPosition) { case RedCrabMinibot.ClawArm_STOW: robot.clawArm.setTargetPosition(Utilities.motorAngleToTicks( @@ -180,10 +216,10 @@ public class Pilot extends CyberarmState { robot.clawArm.getCurrentPosition() >= Utilities.motorAngleToTicks( RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION, RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO, - RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 25.0) { -// robot.clawArm.setPower(0); -// } else { -// robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED); + RedCrabMinibot.CLAW_ARM_COLLECT_ANGLE) - 5.0) { + robot.clawArm.setPower(0); + } else { + robot.clawArm.setPower(RedCrabMinibot.CLAW_ARM_MAX_SPEED); } } @@ -206,12 +242,22 @@ public class Pilot extends CyberarmState { } private void winchController() { + if (triggersControlClawArm) + { + return; + } + if (engine.gamepad1.right_trigger > 0) { + robot.winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.winch.setPower(engine.gamepad1.right_trigger * RedCrabMinibot.WINCH_MAX_SPEED); + robot.winch.setTargetPosition(robot.winch.getCurrentPosition()); } else if (engine.gamepad1.left_trigger > 0) { + robot.winch.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); robot.winch.setPower(-engine.gamepad1.left_trigger * RedCrabMinibot.WINCH_MAX_SPEED); + robot.winch.setTargetPosition(robot.winch.getCurrentPosition()); } else { - robot.winch.setPower(0); + robot.winch.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.winch.setPower(RedCrabMinibot.WINCH_MAX_SPEED); } } } From 1dd80512020c549be1b9b3cafdc531ceda390bc2 Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Mon, 1 Jan 2024 20:47:15 -0600 Subject: [PATCH 2/2] More work on Pizza bot's teleop- Launcher works, beginning the distance sensor coding --- .../Common/SodiPizzaMinibotObject.java | 3 +- .../TeleOp/States/SodiPizzaTeleOPState.java | 56 +++++++++++++++++-- 2 files changed, 52 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java index 71ff2ff..9deae07 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/SodiPizzaMinibotObject.java @@ -19,7 +19,7 @@ public class SodiPizzaMinibotObject extends Robot { public HardwareMap hardwareMap; public DcMotor leftFront, rightFront, leftBack, rightBack; - public Servo shoulder, gripper; + public Servo shoulder, gripper, launcher; public IMU imu; public Rev2mDistanceSensor distSensor; private String string; @@ -72,6 +72,7 @@ public class SodiPizzaMinibotObject extends Robot { //Servo Defining shoulder = engine.hardwareMap.servo.get("arm"); gripper = engine.hardwareMap.servo.get("gripper"); + launcher = engine.hardwareMap.servo.get("launcher"); //Distance Sensor diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java index d8a7ba5..bbd8167 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/SodiPizzaTeleOPState.java @@ -9,6 +9,7 @@ import static org.timecrafters.CenterStage.Common.SodiPizzaMinibotObject.GRIPPER import com.qualcomm.robotcore.hardware.DcMotor; +import org.checkerframework.checker.units.qual.A; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.timecrafters.CenterStage.Common.CompetitionRobotV1; @@ -20,12 +21,15 @@ import dev.cyberarm.engine.V2.GamepadChecker; public class SodiPizzaTeleOPState extends CyberarmState { final private SodiPizzaMinibotObject robot; - private long lastMoveTime; + private long lastMoveTime, lastDistRead /* <- last time Distance was read*/; public double drivePower; public final double minInput = 0.1 /* <- Minimum input from stick to send command */; - public double lastToldAngle /* <- The angle the bot was last told to stop at */; - private int armPos; + public double lastToldAngle /* <- The angle the bot was last told to stop at */, proportion, integral, derivative; + public float approxObjPos /* <- Approximate distance away the nearest obstruction is */, objData1, objData2, objData3; + private int armPos, objectPos; + private boolean droneLaunched; private char buttonPressed; + private GamepadChecker gp1checker, gp2checker; YawPitchRollAngles imuInitAngle; @@ -34,12 +38,24 @@ public class SodiPizzaTeleOPState extends CyberarmState { robot.setup(); } + public float getApproxObjPos() { + if (System.currentTimeMillis() - lastDistRead >= 500) { + /*Pseudocode: take objData1, wait, take 2, wait, take 3*/ + } + approxObjPos = (objData1 + objData2 + objData3)/3; + return approxObjPos; + } + @Override public void telemetry() { engine.telemetry.addData("Arm should be at Position ", armPos); engine.telemetry.addData("Arm servo is at ", robot.shoulder.getPosition()); engine.telemetry.addData("Button Pressed = ", buttonPressed); + + engine.telemetry.addLine(); + engine.telemetry.addData("Launcher Servo: ", robot.launcher.getPosition()); + engine.telemetry.addData("Drone Launched?", droneLaunched); } @Override @@ -50,12 +66,15 @@ public class SodiPizzaTeleOPState extends CyberarmState { robot.leftBack.setPower(0); robot.rightBack.setPower(0); + robot.launcher.setPosition(0); + droneLaunched = false; + robot.imu.resetYaw(); imuInitAngle = robot.imu.getRobotYawPitchRollAngles(); lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); - GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); - GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); + gp1checker = new GamepadChecker(engine, engine.gamepad1); + gp2checker = new GamepadChecker(engine, engine.gamepad2); lastMoveTime = System.currentTimeMillis(); armPos = 0; @@ -139,6 +158,30 @@ public class SodiPizzaTeleOPState extends CyberarmState { lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); } + if (engine.gamepad2.left_stick_button) { + if (System.currentTimeMillis() - lastMoveTime >= 200) { + robot.launcher.setPosition(0.98); + lastMoveTime = System.currentTimeMillis(); + } + } else if (engine.gamepad2.right_stick_button) { + if (System.currentTimeMillis() - lastMoveTime >= 100) { + robot.launcher.setPosition(robot.launcher.getPosition() - 0.2); + lastMoveTime = System.currentTimeMillis(); + } + } else if (robot.launcher.getPosition() >= 0.95) { + if (System.currentTimeMillis() - lastMoveTime >= 1000) { + droneLaunched = true; + lastMoveTime = System.currentTimeMillis(); + } + } + + if (!engine.gamepad2.left_stick_button && droneLaunched) { + if (System.currentTimeMillis() - lastMoveTime >= 200) { + robot.launcher.setPosition(robot.launcher.getPosition() - 0.025); + lastMoveTime = System.currentTimeMillis(); + } + } + // This moves the arm to Collect position, which is at servo position 0.00. if (engine.gamepad2.a && !engine.gamepad2.start) { armPos = 1; @@ -274,7 +317,8 @@ public class SodiPizzaTeleOPState extends CyberarmState { robot.gripper.setPosition(GRIPPER_CLOSED); } - + gp1checker.update(); + gp2checker.update(); }