From df86f9c11d7d5f22026a011fe4c369b5b7cd598b Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 13 Nov 2023 20:19:16 -0600 Subject: [PATCH] An Autonomous implemented for Pizza on RED RIGHT and BLUE LEFT --- .../cyberarm/minibots/pizza/PizzaMinibot.java | 66 ++++++- .../PizzaAutonomousBlueLeftEngine.java | 185 ++++++++++++++++++ .../PizzaAutonomousRedRightEngine.java | 157 +++++++++++++++ .../minibots/pizza/states/Rotate.java | 80 ++++++++ .../minibots/pizza/states/ServoMove.java | 55 ++++++ .../minibots/pizza/states/StrafeMove.java | 62 ++++++ .../minibots/pizza/states/TankMove.java | 92 +++++++++ 7 files changed, 695 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousBlueLeftEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousRedRightEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Rotate.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/ServoMove.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/StrafeMove.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/TankMove.java diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java index cd8a325..ddb0b33 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/PizzaMinibot.java @@ -1,5 +1,6 @@ package dev.cyberarm.minibots.pizza; +import com.arcrobotics.ftclib.hardware.motors.Motor; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; @@ -7,7 +8,9 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; import dev.cyberarm.engine.V2.CyberarmEngine; @@ -29,11 +32,18 @@ public class PizzaMinibot { public IMU imu; public Servo gripper, arm; public int armStackPosition = -1; + final public TimeCraftersConfiguration config; + final public double wheelCircumference, distancePerTick; + + public final double imuAngleOffset, initialFacing; private CyberarmEngine engine; public PizzaMinibot(CyberarmEngine engine) { this.engine = engine; + + this.config = new TimeCraftersConfiguration("Pizza_2023"); + leftFront = new MotorEx(engine.hardwareMap, "leftFront"); rightFront = new MotorEx(engine.hardwareMap, "rightFront"); @@ -46,8 +56,15 @@ public class PizzaMinibot { leftFront.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); leftBack.motorEx.setDirection(DcMotorSimple.Direction.REVERSE); - double wheelRadius = 75.0 / 2; // mm - double distancePerTick = Math.PI * wheelRadius * wheelRadius; + leftFront.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + rightFront.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + + leftBack.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); + + wheelCircumference = Math.PI * (75.0 * 4); //wheelRadius * wheelRadius; //-- times 2 is a hack... --// + distancePerTick = (28 * 20) / wheelCircumference; // raw motor encoder * gear ratio + leftFront.setDistancePerPulse(distancePerTick); rightFront.setDistancePerPulse(distancePerTick); leftBack.setDistancePerPulse(distancePerTick); @@ -64,6 +81,9 @@ public class PizzaMinibot { imu.initialize(parameters); + imuAngleOffset = 0; + initialFacing = facing(); + gripper = engine.hardwareMap.servo.get("gripper"); arm = engine.hardwareMap.servo.get("arm"); @@ -114,4 +134,46 @@ public class PizzaMinibot { engine.telemetry.addLine(); } + + public double distanceMM(int ticks) { + return distancePerTick * ticks; + } + + + public double initialFacing() { + return initialFacing; + } + + public double facing() { + double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + + return (((imuDegrees + 360.0) % 360.0) + imuAngleOffset) % 360.0; + } + + public double heading() { + return AngleUnit.normalizeRadians(-facing() * Math.PI / 180.0); +// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + } + + 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; + } + + // Adapted from: https://github.com/gosu/gosu/blob/980d64de2ce52e4b16fdd5cb9c9e11c8bbb80671/src/Math.cpp#L38 + public double angleDiff(double from, double to) { + double value = (to - from + 180); + + double fmod = (value - 0.0) % (360.0 - 0.0); + + return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180; + } + + public double lerp(double min, double max, double t) + { + return min + (max - min) * t; + } } diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousBlueLeftEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousBlueLeftEngine.java new file mode 100644 index 0000000..5e180d4 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousBlueLeftEngine.java @@ -0,0 +1,185 @@ +package dev.cyberarm.minibots.pizza.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.pizza.PizzaMinibot; +import dev.cyberarm.minibots.pizza.states.Rotate; +import dev.cyberarm.minibots.pizza.states.ServoMove; +import dev.cyberarm.minibots.pizza.states.StrafeMove; +import dev.cyberarm.minibots.pizza.states.TankMove; + +@Autonomous(name = "Cyberarm Pizza Autonomous BLUE LEFT", group = "MINIBOT", preselectTeleOp = "Cyberarm Pizza Teleop") +public class PizzaAutonomousBlueLeftEngine extends CyberarmEngine { + private PizzaMinibot robot; + @Override + public void setup() { + this.robot = new PizzaMinibot(this); + robot.imu.resetYaw(); + + /// Close the gripper + addState(new ServoMove( + robot, + "gripper", + PizzaMinibot.GRIPPER_CLOSED, + 100 + )); + + /// Move Purple Pixel into position + addState(new TankMove( + robot, + -900, + -900, + -0.15, + -0.17, + 10, + 5000 + )); + addState(new Rotate( + robot, + 0, + 0.15, + 1, + 5000 + )); + + /// Retreat from pixel + addState(new TankMove( + robot, + 150, + 150, + 0.15, + 0.17, + 10, + 5000 + )); + /// Rotate "towards" backstage board + addState(new Rotate( + robot, + 90, + 0.15, + 1, + 5000 + )); + + /// Move clear of the pixel + addState(new TankMove( + robot, + 500, + 500, + 0.15, + 0.17, + 10, + 5000 + )); + /// Strafe so arm is in range of board + addState(new StrafeMove( + robot, + 1000, + 0.5, + 10, + 5000 + )); + /// Correct robots rotation "towards" backstage board + addState(new Rotate( + robot, + 90, + 0.15, + 1, + 5000 + )); + + /// Move up to the backboard and pre-position arm + addState(new TankMove( + robot, + 500, + 500, + 0.15, + 0.17, + 10, + 5000 + )); + addParallelStateToLastState(new ServoMove( + robot, + "arm", + PizzaMinibot.ARM_DELIVER, + 1000 + )); + + /// Correct rotation; Rotate "towards" backstage board + addState(new Rotate( + robot, + 90, + 0.15, + 1, + 5000 + )); + /// Move up against the backboard + addState(new TankMove( + robot, + 200, + 200, + 0.15, + 0.17, + 10, + 1250 + )); + /// Correct rotation again + addState(new Rotate( + robot, + 90, + 0.15, + 1, + 5000 + )); + /// Deposit pixel on backboard + addState(new ServoMove( + robot, + "gripper", + PizzaMinibot.GRIPPER_OPEN, + 1000 + )); + /// Stow the arm + addState(new ServoMove( + robot, + "arm", + PizzaMinibot.ARM_STOW, + 250 + )); + /// Move away from the backboard + addState(new TankMove( + robot, + -250, + -250, + -0.15, + -0.17, + 10, + 5000 + )); + /// Rotate towards the corner + addState(new Rotate( + robot, + 178, + 0.15, + 1, + 5000 + )); + /// Move into corner + addState(new TankMove( + robot, + -1000, + -1000, + -0.15, + -0.17, + 10, + 5000 + )); + } + + @Override + public void loop() { + robot.standardTelemetry(); + + super.loop(); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousRedRightEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousRedRightEngine.java new file mode 100644 index 0000000..fedfa46 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/engines/PizzaAutonomousRedRightEngine.java @@ -0,0 +1,157 @@ +package dev.cyberarm.minibots.pizza.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.pizza.PizzaMinibot; +import dev.cyberarm.minibots.pizza.states.Rotate; +import dev.cyberarm.minibots.pizza.states.ServoMove; +import dev.cyberarm.minibots.pizza.states.TankMove; + +@Autonomous(name = "Cyberarm Pizza Autonomous RED RIGHT", group = "MINIBOT", preselectTeleOp = "Cyberarm Pizza Teleop") +public class PizzaAutonomousRedRightEngine extends CyberarmEngine { + private PizzaMinibot robot; + @Override + public void setup() { + this.robot = new PizzaMinibot(this); + robot.imu.resetYaw(); + + /// Close the gripper + addState(new ServoMove( + robot, + "gripper", + PizzaMinibot.GRIPPER_CLOSED, + 100 + )); + + /// Move Purple Pixel into position + addState(new TankMove( + robot, + -900, + -900, + -0.15, + -0.17, + 10, + 5000 + )); + addState(new Rotate( + robot, + 0, + 0.15, + 1, + 5000 + )); + + /// Retreat from pixel + addState(new TankMove( + robot, + 150, + 150, + 0.15, + 0.17, + 10, + 5000 + )); + /// Rotate "towards" backstage board + addState(new Rotate( + robot, + 270, + 0.15, + 1, + 5000 + )); + + /// Move up to the backboard and pre-position arm + addState(new TankMove( + robot, + 900, + 900, + 0.15, + 0.17, + 10, + 5000 + )); + addParallelStateToLastState(new ServoMove( + robot, + "arm", + PizzaMinibot.ARM_DELIVER, + 1000 + )); + + /// Correct rotation; Rotate "towards" backstage board + addState(new Rotate( + robot, + 270, + 0.15, + 1, + 5000 + )); + /// Move up against the backboard + addState(new TankMove( + robot, + 200, + 200, + 0.15, + 0.17, + 10, + 5000 + )); + /// Correct rotation again + addState(new Rotate( + robot, + 270, + 0.15, + 1, + 5000 + )); + /// Deposit pixel on backboard + addState(new ServoMove( + robot, + "gripper", + PizzaMinibot.GRIPPER_OPEN, + 1000 + )); + /// Stow the arm + addState(new ServoMove( + robot, + "arm", + PizzaMinibot.ARM_STOW, + 250 + )); + /// Move away from the backboard + addState(new TankMove( + robot, + -250, + -250, + -0.15, + -0.17, + 10, + 5000 + )); + /// Rotate towards the corner + addState(new Rotate( + robot, + 170, + 0.15, + 1, + 5000 + )); + /// Move into corner + addState(new TankMove( + robot, + -500, + -500, + -0.15, + -0.17, + 10, + 5000 + )); + } + + @Override + public void loop() { + robot.standardTelemetry(); + + super.loop(); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Rotate.java new file mode 100644 index 0000000..1402349 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/Rotate.java @@ -0,0 +1,80 @@ +package dev.cyberarm.minibots.pizza.states; + +import com.qualcomm.robotcore.hardware.Servo; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; + +public class Rotate extends CyberarmState { + final private PizzaMinibot robot; + final private String groupName, actionName; + + final private double heading, velocity, tolerance; + final private int timeoutMS; + + public Rotate(PizzaMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.groupName = groupName; + this.actionName = actionName; + + this.heading = robot.config.variable(groupName, actionName, "heading").value(); + + this.velocity = robot.config.variable(groupName, actionName, "velocity").value(); + + this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value(); + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + } + + public Rotate(PizzaMinibot robot, double heading, double velocity, double tolerance, int timeoutMS) { + this.groupName = ""; + this.actionName = ""; + + this.robot = robot; + this.heading = heading; + this.velocity = velocity; + this.tolerance = tolerance; + this.timeoutMS = timeoutMS; + } + + @Override + public void start() { + } + + @Override + public void exec() { + if (Math.abs(robot.angleDiff(robot.facing(), heading)) <= tolerance) { + robot.left.set(0); + robot.right.set(0); + + setHasFinished(true); + + return; + } + + if (runTime() >= timeoutMS) { + robot.left.set(0); + robot.right.set(0); + + setHasFinished(true); + } + + if (robot.angleDiff(robot.facing() + robot.turnRate(), heading) < 0) { + robot.left.set(-velocity); + robot.right.set(velocity); + } else { + robot.left.set(velocity); + robot.right.set(-velocity); + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + engine.telemetry.addData("Robot Heading", robot.facing()); + engine.telemetry.addData("Robot Target Heading", heading); + engine.telemetry.addData("Robot Angle Diff", robot.angleDiff(robot.facing() + robot.turnRate(), heading)); + engine.telemetry.addData("Robot Turn Rate", robot.turnRate()); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/ServoMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/ServoMove.java new file mode 100644 index 0000000..51db27e --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/ServoMove.java @@ -0,0 +1,55 @@ +package dev.cyberarm.minibots.pizza.states; + +import com.qualcomm.robotcore.hardware.Servo; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; + +public class ServoMove extends CyberarmState { + final private PizzaMinibot robot; + final private String groupName, actionName; + + final private String servoName; + final private double position; + final private int timeoutMS; + + final private Servo servo; + public ServoMove(PizzaMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.groupName = groupName; + this.actionName = actionName; + + this.servoName = robot.config.variable(groupName, actionName, "servoName").value(); + + this.position = robot.config.variable(groupName, actionName, "position").value(); + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + + this.servo = engine.hardwareMap.servo.get(servoName); + } + + public ServoMove(PizzaMinibot robot, String servoName, double position, int timeoutMS) { + this.groupName = ""; + this.actionName = ""; + + this.robot = robot; + this.servoName = servoName; + this.position = position; + this.timeoutMS = timeoutMS; + + this.servo = engine.hardwareMap.servo.get(servoName); + } + + @Override + public void start() { + this.servo.setPosition(this.position); + } + + @Override + public void exec() { + if (runTime() >= timeoutMS) { + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/StrafeMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/StrafeMove.java new file mode 100644 index 0000000..0d9553b --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/StrafeMove.java @@ -0,0 +1,62 @@ +package dev.cyberarm.minibots.pizza.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; + +public class StrafeMove extends CyberarmState { + final private PizzaMinibot robot; + final private String groupName, actionName; + + final private double distanceMM, velocity; + final private int tolerance, timeoutMS; + + public StrafeMove(PizzaMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.groupName = groupName; + this.actionName = actionName; + + this.distanceMM = robot.config.variable(groupName, actionName, "distanceMM").value(); + + this.velocity = robot.config.variable(groupName, actionName, "velocity").value(); + + this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value(); + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + } + + public StrafeMove(PizzaMinibot robot, double distanceMM, double velocity, int tolerance, int timeoutMS) { + this.groupName = ""; + this.actionName = ""; + + this.robot = robot; + this.distanceMM = distanceMM; + this.velocity = velocity; + this.tolerance = tolerance; + this.timeoutMS = timeoutMS; + } + + @Override + public void start() { + robot.leftFront.setTargetDistance(distanceMM); + + robot.left.setPositionTolerance(tolerance); + robot.right.setPositionTolerance(tolerance); + + double motorVelocity = (distanceMM < 0 ? velocity * -1 : velocity); + + robot.leftFront.set(motorVelocity); + robot.rightFront.set(-motorVelocity); + + robot.leftBack.set(-motorVelocity); + robot.rightBack.set(motorVelocity); + } + + @Override + public void exec() { + if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(distanceMM) || runTime() >= timeoutMS) { + robot.left.set(0); + robot.right.set(0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/TankMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/TankMove.java new file mode 100644 index 0000000..c544223 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/pizza/states/TankMove.java @@ -0,0 +1,92 @@ +package dev.cyberarm.minibots.pizza.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; + +public class TankMove extends CyberarmState { + final private PizzaMinibot robot; + final private String groupName, actionName; + + final private double leftDistanceMM, rightDistanceMM; + final private double leftVelocity, rightVelocity; + final private int tolerance, timeoutMS; + + public TankMove(PizzaMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.groupName = groupName; + this.actionName = actionName; + + this.leftDistanceMM = robot.config.variable(groupName, actionName, "leftDistanceMM").value(); + this.rightDistanceMM = robot.config.variable(groupName, actionName, "rightDistanceMM").value(); + + this.leftVelocity = robot.config.variable(groupName, actionName, "leftVelocity").value(); + this.rightVelocity = robot.config.variable(groupName, actionName, "rightVelocity").value(); + + this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value(); + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + } + + public TankMove(PizzaMinibot robot, double leftDistanceMM, double rightDistanceMM, double leftVelocity, double rightVelocity, int tolerance, int timeoutMS) { + this.groupName = ""; + this.actionName = ""; + + this.robot = robot; + this.leftDistanceMM = leftDistanceMM; + this.rightDistanceMM = rightDistanceMM; + this.leftVelocity = leftVelocity; + this.rightVelocity = rightVelocity; + this.tolerance = tolerance; + this.timeoutMS = timeoutMS; + } + + @Override + public void start() { + robot.left.resetEncoder(); + robot.right.resetEncoder(); + + robot.left.setTargetDistance(leftDistanceMM); + robot.right.setTargetDistance(rightDistanceMM); + + robot.left.setPositionTolerance(tolerance); + robot.right.setPositionTolerance(tolerance); + + robot.left.set(leftVelocity); + robot.right.set(rightVelocity); + } + + @Override + public void exec() { + if (robot.left.atTargetPosition() || Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM)) { + robot.left.set(0); + } + if (robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) { + robot.right.set(0); + } + + if ( + (robot.left.atTargetPosition() && robot.right.atTargetPosition()) || + (Math.abs(robot.leftFront.getDistance()) >= Math.abs(leftDistanceMM) && robot.right.atTargetPosition() || Math.abs(robot.rightFront.getDistance()) >= Math.abs(leftDistanceMM)) || + runTime() >= timeoutMS) { + robot.left.set(0); + robot.right.set(0); + setHasFinished(true); + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + + engine.telemetry.addData("Left distance", robot.leftFront.getDistance()); + engine.telemetry.addData("Left position", robot.left.getPositions().get(0)); + engine.telemetry.addData("Left speed", robot.left.getSpeeds().get(0)); + engine.telemetry.addData("Left velocity", robot.left.getVelocity()); + engine.telemetry.addLine(); + + engine.telemetry.addData("Right distance", robot.rightFront.getDistance()); + engine.telemetry.addData("Right position", robot.right.getPositions().get(0)); + engine.telemetry.addData("Right speed", robot.right.getSpeeds().get(0)); + engine.telemetry.addData("Right velocity", robot.right.getVelocity()); + } +}