diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java index 2c2d1d3..a7ff840 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/YellowMinibot.java @@ -3,22 +3,37 @@ package dev.cyberarm.minibots.yellow; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.arcrobotics.ftclib.hardware.motors.MotorGroup; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.IMU; +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; public class YellowMinibot { + private final CyberarmEngine engine; + public final TimeCraftersConfiguration config; public MotorEx leftFront, rightFront, leftBack, rightBack; public MotorGroup left, right; public IMU imu; + public CRServo droneLauncher; + public final double wheelCircumference, distancePerTick, imuAngleOffset, initialFacing; public YellowMinibot(CyberarmEngine engine) { + this.engine = engine; + + this.config = new TimeCraftersConfiguration("Vexy_2023"); + leftFront = new MotorEx(engine.hardwareMap, "leftFront"); rightFront = new MotorEx(engine.hardwareMap, "rightFront"); leftBack = new MotorEx(engine.hardwareMap, "leftBack"); rightBack = new MotorEx(engine.hardwareMap, "rightBack"); - double distancePerTick = 1962.5; // 3.14 * 25mm * 25mm + wheelCircumference = Math.PI * (50.0 * 2); //wheelRadius * wheelRadius; + distancePerTick = 288 / wheelCircumference; // raw motor encoder * gear ratio + leftFront.setDistancePerPulse(distancePerTick); rightFront.setDistancePerPulse(distancePerTick); leftBack.setDistancePerPulse(distancePerTick); @@ -34,5 +49,94 @@ public class YellowMinibot { RevHubOrientationOnRobot.UsbFacingDirection.RIGHT)); imu.initialize(parameters); + + imuAngleOffset = 0; + initialFacing = facing(); + + droneLauncher = engine.hardwareMap.crservo.get("droneLauncher"); /// Port 5 + } + + public void standardTelemetry() { + engine.telemetry.addLine(); + + engine.telemetry.addLine("Motors"); + engine.telemetry.addData( + "Left Front", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + leftFront.motorEx.getPower(), + leftFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + leftFront.motorEx.getCurrentPosition()); + engine.telemetry.addData( + "Right Front", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + rightFront.motorEx.getPower(), + rightFront.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + rightFront.motorEx.getCurrentPosition()); + engine.telemetry.addData( + "Left Back", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + leftBack.motorEx.getPower(), + leftBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + leftBack.motorEx.getCurrentPosition()); + engine.telemetry.addData( + "Right Back", + "Power: %.2f, Current: %.2f mAmp, Position: %d", + rightBack.motorEx.getPower(), + rightBack.motorEx.getCurrent(CurrentUnit.MILLIAMPS), + rightBack.motorEx.getCurrentPosition()); + + engine.telemetry.addLine(); + engine.telemetry.addLine("Servos"); + engine.telemetry.addData("droneLauncher", droneLauncher.getPower()); + + engine.telemetry.addLine(); + } + + public void teleopTelemetry() { + engine.telemetry.addLine(); + + 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/yellow/engines/YellowMinibotAutonomousBlueRightEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotAutonomousBlueRightEngine.java new file mode 100644 index 0000000..aa86cb7 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotAutonomousBlueRightEngine.java @@ -0,0 +1,78 @@ +package dev.cyberarm.minibots.yellow.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.yellow.YellowMinibot; +import dev.cyberarm.minibots.yellow.states.Rotate; +import dev.cyberarm.minibots.yellow.states.TankMove; + +@Autonomous(name = "Cyberarm Yellow Autonomous BLUE RIGHT", group = "MINIBOT", preselectTeleOp = "Cyberarm Yellow Teleop") +public class YellowMinibotAutonomousBlueRightEngine extends CyberarmEngine { + @Override + public void setup() { + YellowMinibot robot = new YellowMinibot(this); + robot.imu.resetYaw(); + + /// Move Purple pixel into position + addState(new TankMove( + robot, + 1450, + 1450, + -1.0, + -1.0, + 10, + 5000 + )); + /// Move away from pixel + addState(new TankMove( + robot, + 250, + 250, + 1.0, + 1.0, + 10, + 5000 + )); + /// Turn around + addState(new Rotate( + robot, + 182, + 1.0, + 0.25, + 1, + 5000 + )); + /// Move back towards yellow pixel + addState(new TankMove( + robot, + 850, + 850, + -1.0, + -1.0, + 10, + 5000 + )); + /// Turn around + addState(new Rotate( + robot, + 270, + 1.0, + 0.25, + 1, + 5000 + )); + /// Move towards backstage + addState(new TankMove( + robot, + 4000, + 4000, + -1.0, + -1.0, + 10, + 8000 + )); + } +} + + diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotAutonomousRedLeftEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotAutonomousRedLeftEngine.java new file mode 100644 index 0000000..e33e684 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/engines/YellowMinibotAutonomousRedLeftEngine.java @@ -0,0 +1,76 @@ +package dev.cyberarm.minibots.yellow.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.yellow.YellowMinibot; +import dev.cyberarm.minibots.yellow.states.Rotate; +import dev.cyberarm.minibots.yellow.states.TankMove; + +@Autonomous(name = "Cyberarm Yellow Autonomous RED LEFT", group = "MINIBOT", preselectTeleOp = "Cyberarm Yellow Teleop") +public class YellowMinibotAutonomousRedLeftEngine extends CyberarmEngine { + @Override + public void setup() { + YellowMinibot robot = new YellowMinibot(this); + robot.imu.resetYaw(); + + /// Move Purple pixel into position + addState(new TankMove( + robot, + 1450, + 1450, + -1.0, + -1.0, + 10, + 5000 + )); + /// Move away from pixel + addState(new TankMove( + robot, + 250, + 250, + 1.0, + 1.0, + 10, + 5000 + )); + /// Turn around + addState(new Rotate( + robot, + 178, + 1.0, + 0.25, + 1, + 5000 + )); + /// Move back towards yellow pixel + addState(new TankMove( + robot, + 850, + 850, + -1.0, + -1.0, + 10, + 5000 + )); + /// Turn around + addState(new Rotate( + robot, + 90, + 1.0, + 0.25, + 1, + 5000 + )); + /// Move towards backstage + addState(new TankMove( + robot, + 4000, + 4000, + -1.0, + -1.0, + 10, + 8000 + )); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java deleted file mode 100644 index 1622b77..0000000 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Move.java +++ /dev/null @@ -1,34 +0,0 @@ -package dev.cyberarm.minibots.yellow.states; - -import dev.cyberarm.engine.V2.CyberarmState; -import dev.cyberarm.minibots.yellow.YellowMinibot; - -public class Move extends CyberarmState { - final YellowMinibot robot; - final double leftDistance, rightDistance, leftPower, rightPower; - public Move(YellowMinibot robot, double leftDistance, double rightDistance, double leftPower, double rightPower) { - this.robot = robot; - - this.leftDistance = leftDistance; - this.rightDistance = rightDistance; - this.leftPower = leftPower; - this.rightPower = rightPower; - } - - @Override - public void start() { - robot.leftFront.setTargetDistance(leftDistance); - robot.leftBack.setTargetDistance(leftDistance); - - robot.leftFront.setTargetDistance(rightDistance); - robot.leftBack.setTargetDistance(rightDistance); - } - - @Override - public void exec() { - if (robot.leftFront.atTargetPosition()) { - robot.leftFront.setVelocity(0); - robot.leftBack.setVelocity(0); - } - } -} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java index 9a5d919..6117c15 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Pilot.java @@ -18,6 +18,13 @@ public class Pilot extends CyberarmState { robot.imu.resetYaw(); } + /// --- DRONE --- /// + if (engine.gamepad1.y) { + robot.droneLauncher.setPower(1.0); + } else if (engine.gamepad1.a) { + robot.droneLauncher.setPower(0.0); + } + /// --- DRIVE --- /// // robot.left.set(engine.gamepad1.left_stick_y); // robot.right.set(engine.gamepad1.right_stick_y); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java new file mode 100644 index 0000000..3594496 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/Rotate.java @@ -0,0 +1,86 @@ +package dev.cyberarm.minibots.yellow.states; + +import com.qualcomm.robotcore.util.Range; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; +import dev.cyberarm.minibots.yellow.YellowMinibot; + +public class Rotate extends CyberarmState { + final private YellowMinibot robot; + final private String groupName, actionName; + + final private double heading, maxVelocity, minVelocity, tolerance; + final private int timeoutMS; + + public Rotate(YellowMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.groupName = groupName; + this.actionName = actionName; + + this.heading = robot.config.variable(groupName, actionName, "heading").value(); + + this.minVelocity = robot.config.variable(groupName, actionName, "minVelocity").value(); + this.maxVelocity = robot.config.variable(groupName, actionName, "maxVelocity").value(); + + this.tolerance = robot.config.variable(groupName, actionName, "tolerance").value(); + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + } + + public Rotate(YellowMinibot robot, double heading, double maxVelocity, double minVelocity, double tolerance, int timeoutMS) { + this.groupName = ""; + this.actionName = ""; + + this.robot = robot; + this.heading = heading; + this.maxVelocity = maxVelocity; + this.minVelocity = minVelocity; + 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); + } + + double angleDiff = robot.angleDiff(robot.facing() + robot.turnRate(), heading); + double velocity = robot.lerp(minVelocity, maxVelocity, Range.clip(Math.abs(angleDiff) / 90.0, 0.0, 1.0)); + + if (angleDiff > 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/yellow/states/ServoMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/ServoMove.java new file mode 100644 index 0000000..a4ec4f7 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/ServoMove.java @@ -0,0 +1,56 @@ +package dev.cyberarm.minibots.yellow.states; + +import com.qualcomm.robotcore.hardware.Servo; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; +import dev.cyberarm.minibots.yellow.YellowMinibot; + +public class ServoMove extends CyberarmState { + final private YellowMinibot robot; + final private String groupName, actionName; + + final private String servoName; + final private double position; + final private int timeoutMS; + + final private Servo servo; + public ServoMove(YellowMinibot 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(YellowMinibot 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/yellow/states/StrafeMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java new file mode 100644 index 0000000..95bdfe7 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/StrafeMove.java @@ -0,0 +1,63 @@ +package dev.cyberarm.minibots.yellow.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; +import dev.cyberarm.minibots.yellow.YellowMinibot; + +public class StrafeMove extends CyberarmState { + final private YellowMinibot robot; + final private String groupName, actionName; + + final private double distanceMM, velocity; + final private int tolerance, timeoutMS; + + public StrafeMove(YellowMinibot 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(YellowMinibot 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/yellow/states/TankMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java new file mode 100644 index 0000000..e797bfe --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/yellow/states/TankMove.java @@ -0,0 +1,93 @@ +package dev.cyberarm.minibots.yellow.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.pizza.PizzaMinibot; +import dev.cyberarm.minibots.yellow.YellowMinibot; + +public class TankMove extends CyberarmState { + final private YellowMinibot robot; + final private String groupName, actionName; + + final private double leftDistanceMM, rightDistanceMM; + final private double leftVelocity, rightVelocity; + final private int tolerance, timeoutMS; + + public TankMove(YellowMinibot 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(YellowMinibot 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()); + } +}