From 977a90eb48081049b2c5a78dbd031a79293d1a2a Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 21 Jan 2023 21:06:38 -0600 Subject: [PATCH] 'Fixed' issue with GamepadChecker- the engine already has once for each, adding one in a state directly is an error, misc. refactoring, stubbed autonomous states, Move state is almost functional- in theory, added experimental custom localizer, added field and obstacle classes to manage collision avoidance of static objects. --- .../cyberarm/engine/V2/GamepadChecker.java | 6 +- .../minibots/cyberarm/chiron/Robot.java | 42 +++++- .../engines/AutonomousRedLeftSideEngine.java | 40 +++++ .../ConfigureAndTestHardwareEngine.java | 3 +- .../engines/GamepadCheckerDebugEngine.java | 47 ++++++ .../cyberarm/chiron/engines/TeleOpEngine.java | 5 +- .../chiron/states/autonomous/Arm.java | 11 ++ .../chiron/states/autonomous/Gripper.java | 11 ++ .../chiron/states/autonomous/Move.java | 124 ++++++++++++++++ .../chiron/states/autonomous/Rotate.java | 11 ++ .../autonomous/SelectParkingPosition.java | 11 ++ .../states/autonomous/SignalProcessor.java | 11 ++ .../{ => teleop}/DriverControlState.java | 2 +- .../cyberarm/chiron/tasks/FieldLocalizer.java | 140 ++++++++++++++++++ .../chiron/tasks/field_localizer/Field.java | 65 ++++++++ .../tasks/field_localizer/Obstacle.java | 18 +++ 16 files changed, 534 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/GamepadCheckerDebugEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java rename TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/{ => teleop}/DriverControlState.java (99%) create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Field.java create mode 100644 TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Obstacle.java diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java index 0bb5447..e9b551c 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/GamepadChecker.java @@ -13,7 +13,7 @@ public class GamepadChecker { private final Gamepad gamepad; private final HashMap buttons = new HashMap<>(); private final HashMap buttonsDebounceInterval = new HashMap<>(); - private final long debounceTime = 50L; // ms + private final long debounceTime = 20L; // ms public GamepadChecker(CyberarmEngine engine, Gamepad gamepad) { this.engine = engine; @@ -52,16 +52,12 @@ public class GamepadChecker { if (button) { if (!buttons.get(btn)) { engine.buttonDown(gamepad, btn); - - Log.d(TAG, "Button '" + btn + "' is DOWN [" + buttons.size() + "]"); } buttons.put(btn, true); } else { if (buttons.get(btn)) { engine.buttonUp(gamepad, btn); - - Log.d(TAG, "Button '" + btn + "' is UP"); } buttons.put(btn, false); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index 5b10469..071dc75 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java @@ -1,5 +1,6 @@ package org.timecrafters.minibots.cyberarm.chiron; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; @@ -16,6 +17,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; +import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer; public class Robot { public final DcMotorEx backLeftDrive, frontRightDrive, frontLeftDrive, backRightDrive, arm; @@ -48,10 +50,20 @@ public class Robot { private final CyberarmEngine engine; private final TimeCraftersConfiguration configuration; + private final FieldLocalizer fieldLocalizer; + private final double radius, diameter; - public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration) { + public Robot(CyberarmEngine engine, TimeCraftersConfiguration configuration, FieldLocalizer fieldLocalizer) { this.engine = engine; this.configuration = configuration; + this.fieldLocalizer = fieldLocalizer; + + this.fieldLocalizer.setRobot(this); + this.fieldLocalizer.standardSetup(); + + radius = tuningConfig("field_localizer_robot_radius").value(); + diameter = ((double)tuningConfig("field_localizer_robot_radius").value()) * 2; + imuAngleOffset = hardwareConfig("imu_angle_offset").value(); // FIXME: Rename motors in configuration @@ -225,11 +237,29 @@ public class Robot { engine.telemetry.addData(" Facing (Degrees)", facing()); engine.telemetry.addData(" Heading (Radians)", heading()); engine.telemetry.addData(" Turn Rate", turnRate()); + + engine.telemetry.addLine(); + + // Field Localizer + engine.telemetry.addLine("Field Localizer"); + + Vector2d pos = fieldLocalizer.position(); + Vector2d vel = fieldLocalizer.velocity(); + engine.telemetry.addData(" Position X", pos.getX()); + engine.telemetry.addData(" Position Y", pos.getY()); + engine.telemetry.addData(" Velocity X", vel.getX()); + engine.telemetry.addData(" Velocity Y", vel.getY()); + + engine.telemetry.addLine(); } - public TimeCraftersConfiguration getConfiguration() { - return configuration; - } + public double getRadius() { return radius; } + + public double getDiameter() { return diameter; } + + public TimeCraftersConfiguration getConfiguration() { return configuration; } + + public FieldLocalizer getFieldLocalizer() { return fieldLocalizer; } // For: Drive Wheels public int unitToTicks(DistanceUnit unit, double distance) { @@ -304,4 +334,8 @@ public class Robot { public double turnRate() { return imu.getRobotAngularVelocity(AngleUnit.DEGREES).yRotationRate; // NOTE: UNTESTED } + + public boolean isBetween(double value, double min, double max) { + return value >= min && value <= max; + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java new file mode 100644 index 0000000..aaf999f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/AutonomousRedLeftSideEngine.java @@ -0,0 +1,40 @@ +package org.timecrafters.minibots.cyberarm.chiron.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; +import org.timecrafters.minibots.cyberarm.chiron.Robot; +import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer; + +@Autonomous(name = "CHIRON | RED Left Side", group = "CHIRON", preselectTeleOp = "CHIRON | TeleOp") +public class AutonomousRedLeftSideEngine extends CyberarmEngine { + private Robot robot; + private FieldLocalizer fieldLocalizer; + private TimeCraftersConfiguration configuration; + + @Override + public void setup() { + configuration = new TimeCraftersConfiguration("CHIRON"); + + fieldLocalizer = new FieldLocalizer( + configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_x_in_inches").value(), + configuration.variable("Autonomous", "Tuning_Red_LeftSide", "starting_position_y_in_inches").value() + ); + + robot = new Robot( + this, + configuration, + fieldLocalizer + ); + + addTask(fieldLocalizer); + + setupFromConfig( + robot.getConfiguration(), + "org.timecrafters.minibots.cyberarm.chiron.states.autonomous", + robot, + Robot.class, + "AutonomousRedLeftSide"); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java index aea281b..ecedafe 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/ConfigureAndTestHardwareEngine.java @@ -10,6 +10,7 @@ import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.GamepadChecker; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.minibots.cyberarm.chiron.Robot; +import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer; @TeleOp(name = "CHIRON | Diagnostics", group = "CHIRON") public class ConfigureAndTestHardwareEngine extends CyberarmEngine { @@ -24,7 +25,7 @@ public class ConfigureAndTestHardwareEngine extends CyberarmEngine { @Override public void setup() { - robot = new Robot(this, new TimeCraftersConfiguration("CHIRON")); + robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"), new FieldLocalizer(0, 0)); addState(new MotorSetupState("Back Left Drive", robot.backLeftDrive, "back_left_drive_direction_forward", robot)); addState(new MotorSetupState("Front Right Drive", robot.frontRightDrive, "front_right_drive_direction_forward", robot)); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/GamepadCheckerDebugEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/GamepadCheckerDebugEngine.java new file mode 100644 index 0000000..0abe9eb --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/GamepadCheckerDebugEngine.java @@ -0,0 +1,47 @@ +package org.timecrafters.minibots.cyberarm.chiron.engines; + +import android.util.Log; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.android.AndroidTextToSpeech; + +//@TeleOp(name = "CHIRON | Gamepad Checker Debug", group = "CHIRON") +public class GamepadCheckerDebugEngine extends CyberarmEngine { + @Override + public void setup() { + addState(new CyberarmState() { + private AndroidTextToSpeech speech; + + @Override + public void init() { + Log.d(TAG, "INITIALING STATE"); + speech = new AndroidTextToSpeech(); + speech.initialize(); + } + + @Override + public void exec() { + } + + @Override + public void telemetry() { + engine.telemetry.addData("Run Time", runTime()); + } + + @Override + public void buttonDown(Gamepad gamepad, String button) { + if (gamepad != engine.gamepad1) { + return; + } + + if (button.equals("guide")) { + speech.speak("GUIDE"); + } + } + }); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java index 38b970b..f12d579 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/engines/TeleOpEngine.java @@ -5,14 +5,15 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import org.timecrafters.minibots.cyberarm.chiron.Robot; -import org.timecrafters.minibots.cyberarm.chiron.states.DriverControlState; +import org.timecrafters.minibots.cyberarm.chiron.states.teleop.DriverControlState; +import org.timecrafters.minibots.cyberarm.chiron.tasks.FieldLocalizer; @TeleOp(name = "CHIRON | TeleOp", group = "CHIRON") public class TeleOpEngine extends CyberarmEngine { private Robot robot; @Override public void setup() { - this.robot = new Robot(this, new TimeCraftersConfiguration("CHIRON")); + this.robot = new Robot(this, new TimeCraftersConfiguration("CHIRON"), new FieldLocalizer()); addState(new DriverControlState(robot)); } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java new file mode 100644 index 0000000..208ed12 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java @@ -0,0 +1,11 @@ +package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; + +import org.cyberarm.engine.V2.CyberarmState; + +public class Arm extends CyberarmState { + @Override + public void exec() { + // FIXME: NO OP + setHasFinished(true); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java new file mode 100644 index 0000000..db170ac --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Gripper.java @@ -0,0 +1,11 @@ +package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; + +import org.cyberarm.engine.V2.CyberarmState; + +public class Gripper extends CyberarmState { + @Override + public void exec() { + // FIXME: NO OP + setHasFinished(true); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java new file mode 100644 index 0000000..937b10b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java @@ -0,0 +1,124 @@ +package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.timecrafters.minibots.cyberarm.chiron.Robot; + +public class Move extends CyberarmState { + private final Robot robot; + private final String groupName, actionName; + + private final double positionXInInches, positionYInInches, toleranceInInches, facing, targetVelocity, timeInMS; + + private final double maxSpeed; + private double speed; + + private int distanceAlreadyTravelled; + private final boolean stateDisabled; + + public Move(Robot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + + positionXInInches = robot.getConfiguration().variable(groupName, actionName, "positionXInInches").value(); + positionYInInches = robot.getConfiguration().variable(groupName, actionName, "positionYInInches").value(); + toleranceInInches = robot.getConfiguration().variable(groupName, actionName, "toleranceInInches").value(); + facing = robot.getConfiguration().variable(groupName, actionName, "facing").value(); + targetVelocity = robot.getConfiguration().variable(groupName, actionName, "targetVelocity").value(); + timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value(); + stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled; + + maxSpeed = robot.tuningConfig("drivetrain_max_power").value(); + speed = 0.0; + } + + @Override + public void start() { + // TODO: Use a dead wheel for this + distanceAlreadyTravelled = robot.frontLeftDrive.getCurrentPosition(); + } + + @Override + public void exec() { + if (stateDisabled) { + setHasFinished(true); + + return; + } + + if (runTime() >= timeInMS) { + stop(); + + setHasFinished(true); + + return; + } + + // TODO: Double check maths +// int travelledDistance = (robot.frontLeftDrive.getCurrentPosition() - distanceAlreadyTravelled); +// int targetDistance = robot.unitToTicks(DistanceUnit.INCH, distanceInInches); +// int tolerance = robot.unitToTicks(DistanceUnit.INCH, toleranceInInches); +// +// if (robot.isBetween(targetDistance, travelledDistance - tolerance, travelledDistance + tolerance)) { +// stop(); +// +// setHasFinished(true); +// +// return; +// } + + move(); + } + + private void move() { + if (Math.abs(speed) > maxSpeed) { + speed = speed < 0 ? -maxSpeed : maxSpeed; + } + + double forwardSpeed = 1.0; //distanceInInches < 0 ? -1.0 : 1.0; + double rightSpeed = 0.0; + double rotateRightSpeed = 0.0; + + if (!robot.isBetween((robot.facing() + robot.turnRate()), robot.facing() - 1, robot.facing() + 1)) { + if (robot.facing() < facing) { + rotateRightSpeed = -0.5; + } else { + rotateRightSpeed = 0.5; + } + } + + // FIXME: targetSpeed * speed[Limiter] will cause infinitesimal power issues + double y = -forwardSpeed * speed; + double x = (-rightSpeed * speed) * 1.1; // Counteract imperfect strafing. TODO: make tunable + double rx = -rotateRightSpeed * speed; + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + + + double frontLeftPower = 0, frontRightPower = 0, backLeftPower = 0 , backRightPower = 0; + + double heading = robot.heading(); + double rotX = x * Math.cos(heading) - y * Math.sin(heading); + double rotY = x * Math.sin(heading) + y * Math.cos(heading); + + frontLeftPower = (rotY + rotX + rx) / denominator; + backLeftPower = (rotY - rotX - rx) / denominator; + frontRightPower = (rotY - rotX + rx) / denominator; + backRightPower = (rotY + rotX - rx) / denominator; + + robot.frontLeftDrive.setPower(frontLeftPower); + robot.frontRightDrive.setPower(frontRightPower); + + robot.backLeftDrive.setPower(backLeftPower); + robot.backRightDrive.setPower(backRightPower); + } + + @Override + public void stop() { + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java new file mode 100644 index 0000000..526d9dd --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java @@ -0,0 +1,11 @@ +package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; + +import org.cyberarm.engine.V2.CyberarmState; + +public class Rotate extends CyberarmState { + @Override + public void exec() { + // FIXME: NO OP + setHasFinished(true); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java new file mode 100644 index 0000000..b1165e8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SelectParkingPosition.java @@ -0,0 +1,11 @@ +package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; + +import org.cyberarm.engine.V2.CyberarmState; + +public class SelectParkingPosition extends CyberarmState { + @Override + public void exec() { + // FIXME: NO OP + setHasFinished(true); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java new file mode 100644 index 0000000..e9111f5 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java @@ -0,0 +1,11 @@ +package org.timecrafters.minibots.cyberarm.chiron.states.autonomous; + +import org.cyberarm.engine.V2.CyberarmState; + +public class SignalProcessor extends CyberarmState { + @Override + public void exec() { + // FIXME: NO OP + setHasFinished(true); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java similarity index 99% rename from TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java rename to TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java index c31be76..5ea2d2e 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/DriverControlState.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/teleop/DriverControlState.java @@ -1,4 +1,4 @@ -package org.timecrafters.minibots.cyberarm.chiron.states; +package org.timecrafters.minibots.cyberarm.chiron.states.teleop; import android.util.Log; diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java new file mode 100644 index 0000000..ceed20b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/FieldLocalizer.java @@ -0,0 +1,140 @@ +package org.timecrafters.minibots.cyberarm.chiron.tasks; + +import com.acmerobotics.roadrunner.geometry.Vector2d; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.minibots.cyberarm.chiron.Robot; +import org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer.Field; +import org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer.Obstacle; + +public class FieldLocalizer extends CyberarmState { + private static double posX, posY; + private Robot robot; + private Field field; + + private double deltaFrontLeft, deltaFrontRight, deltaBackLeft, deltaBackRight; + private double displacementFrontLeft, displacementFrontRight, displacementBackLeft, displacementBackRight, displacementAverage; + private double devFrontLeft, devFrontRight, devBackLeft, devBackRight; + private double lastFrontLeft, lastFrontRight, lastBackLeft, lastBackRight; + + private double deltaXRotation, deltaYRotation, deltaX, deltaY; + private double lastX, lastY, theta; + + private final double twoSqrtTwo = 2.0 / Math.sqrt(2.0); + + private double wheelDisplacementPerEncoderTick = 1; // FIXME + + // Use this constructor to preserve displacement between opmodes + public FieldLocalizer() { + } + + public FieldLocalizer(double posX, double posY) { + FieldLocalizer.posX = posX; + FieldLocalizer.posY = posY; + } + + public void standardSetup() { + field = new Field(robot); + + lastFrontLeft = 0; + lastFrontRight = 0; + lastBackLeft = 0; + lastBackRight = 0; + + deltaFrontLeft = 0; + deltaFrontRight = 0; + deltaBackLeft = 0; + deltaBackRight = 0; + + displacementFrontLeft = 0; + displacementFrontRight = 0; + displacementBackLeft = 0; + displacementBackRight = 0; + + displacementAverage = 0; + + devFrontLeft = 0; + devFrontRight = 0; + devBackLeft = 0; + devBackRight = 0; + + deltaX = 0; + deltaY = 0; + deltaXRotation = 0; + deltaYRotation = 0; + + // Preserve field position, of argument-less construction is used + lastX = FieldLocalizer.posX; + lastY = FieldLocalizer.posY; + + // TODO: Preserve robot IMU offset between opmodes + theta = robot.heading(); + } + + public void setRobot(Robot robot) { + this.robot = robot; + } + + // FIXME: Return position in INCHES + public Vector2d position() { + return new Vector2d(posX, posY); + } + + // FIXME: Return velocity in INCHES + public Vector2d velocity() { + return new Vector2d(deltaX, deltaY); + } + + /** + * Calculates direction vector to avoid collision with static field elements + * @return Vector2d if collision projected or null of no correction needed + */ + public Vector2d collisionAvoidanceDirection() { + for (Obstacle o : field.getObstacles()) { + // TODO: Calculate if robot will collide with obstacle + } + + return null; + } + + @Override + public void exec() { + // REF: https://www.bridgefusion.com/blog/2019/4/10/robot-localization-dead-reckoning-in-first-tech-challenge-ftc + + deltaFrontLeft = robot.frontLeftDrive.getCurrentPosition() - lastFrontLeft; + deltaFrontRight = robot.frontRightDrive.getCurrentPosition() - lastFrontRight; + deltaBackLeft = robot.backLeftDrive.getCurrentPosition() - lastBackLeft; + deltaBackRight = robot.backRightDrive.getCurrentPosition() - lastBackRight; + + displacementFrontLeft = deltaFrontLeft * wheelDisplacementPerEncoderTick; + displacementFrontRight = deltaFrontRight * wheelDisplacementPerEncoderTick; + displacementBackLeft = deltaBackLeft * wheelDisplacementPerEncoderTick; + displacementBackRight = deltaBackRight * wheelDisplacementPerEncoderTick; + + displacementAverage = (displacementFrontLeft + displacementFrontRight + + displacementBackLeft + displacementBackRight) / 4.0; // motor count + + devFrontLeft = displacementFrontLeft - displacementAverage; + devFrontRight = displacementFrontRight - displacementAverage; + devBackLeft = displacementBackLeft - displacementAverage; + devBackRight = displacementBackRight - displacementAverage; + + deltaXRotation = (devFrontLeft + devFrontRight - devBackRight - devBackLeft) / twoSqrtTwo; + deltaYRotation = (devFrontLeft - devFrontRight - devBackRight - devBackLeft) / twoSqrtTwo; + + theta = robot.heading(); + double sinTheta = Math.sin(theta); + double cosineTheta = Math.cos(theta); + + deltaX = deltaXRotation * cosineTheta - deltaYRotation * sinTheta; + deltaY = deltaYRotation * cosineTheta + deltaXRotation * sinTheta; + + posX = lastX + deltaX; + posY = lastY + deltaY; + + lastFrontLeft = robot.frontLeftDrive.getCurrentPosition(); + lastFrontRight = robot.frontRightDrive.getCurrentPosition(); + lastBackLeft = robot.backLeftDrive.getCurrentPosition(); + lastBackRight = robot.backRightDrive.getCurrentPosition(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Field.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Field.java new file mode 100644 index 0000000..281befd --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Field.java @@ -0,0 +1,65 @@ +package org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer; + +import org.timecrafters.minibots.cyberarm.chiron.Robot; + +import java.util.ArrayList; + +public class Field { + private final ArrayList obstacles = new ArrayList<>(); + private final Robot robot; + + public Field(Robot robot) { + this.robot = robot; + + populateField(); + } + + private void populateField() { + // ORIGIN is field CENTER point with RED alliance on the RIGHT + + // Ground Junctions + double posX = -48; + double posY = -48; + + for (int y = 0; y < 3; y++) { + posY += (y * 48); // Ground Junctions are 48 inches apart + + for (int x = 0; x < 3; x++) { + posX += (x * 48); + + addGroundJunction(posX, posY); + } + } + + // Pole Junctions + posX = -48; + posY = -48; + + for (int y = 0; y < 5; y++) { + posY += (y * 24); // Pole Junctions are 24 inches apart + + for (int x = 0; x < 5; x++) { + posX += (x * 24); + + // FIXME: Don't add phantom poles where ground junctions are placed + addPole(posX, posY); + } + } + } + + private void addPole(double x, double y) { + obstacles.add( + new Obstacle(x, y, robot.getConfiguration().variable("Field", "Obstacles", "pole_radius_in_inches").value()) + ); + } + + private void addGroundJunction(double x, double y) { + obstacles.add( + new Obstacle(x, y, robot.getConfiguration().variable("Field", "Obstacles", "ground_junction_radius_in_inches").value()) + ); + } + + public ArrayList getObstacles() { + return obstacles; + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Obstacle.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Obstacle.java new file mode 100644 index 0000000..eeebf2a --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/tasks/field_localizer/Obstacle.java @@ -0,0 +1,18 @@ +package org.timecrafters.minibots.cyberarm.chiron.tasks.field_localizer; + +import com.acmerobotics.roadrunner.geometry.Vector2d; + +public class Obstacle { + public final double x, y, radius; + + public Obstacle(double x, double y, double radius) { + this.x = x; + this.y = y; + this.radius = radius; + } + + // TODO: Do the maths to get a normalized vector pointing AWAY from the obstacle's origin + public Vector2d antiCollisionNormal(Vector2d robotVelocity) { + return new Vector2d(0, 0); + } +}