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 9ba3884..0718295 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 @@ -2,6 +2,7 @@ package dev.cyberarm.minibots.red_crab; 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; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; @@ -9,6 +10,9 @@ 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 java.sql.Time; import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.engine.V2.Utilities; @@ -58,11 +62,17 @@ public class RedCrabMinibot { public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm; public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; + public final MotorGroup left, right; + final CyberarmEngine engine; + public final TimeCraftersConfiguration config; + public RedCrabMinibot() { engine = CyberarmEngine.instance; + config = new TimeCraftersConfiguration("cyberarm_RedCrab"); + /// IMU /// /// ------------------------------------------------------------------------------------ /// imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0 @@ -98,6 +108,10 @@ public class RedCrabMinibot { backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); backRight.setZeroPowerBehavior((Motor.ZeroPowerBehavior.BRAKE)); + /// --- MOTOR GROUPS + left = new MotorGroup(frontLeft, backLeft); + right = new MotorGroup(frontRight, backRight); + /// WINCH /// /// ------------------------------------------------------------------------------------ /// winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ?? diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java new file mode 100644 index 0000000..78f857f --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousBlueAudienceEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_BLUE_Audience" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java new file mode 100644 index 0000000..e4b080c --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousBlueBackstageEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_BLUE_Backstage" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java new file mode 100644 index 0000000..46271b6 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousRedAudienceEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_RED_Audience" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java new file mode 100644 index 0000000..a285070 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousRedBackstageEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_RED_Backstage" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java new file mode 100644 index 0000000..e9d640f --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java @@ -0,0 +1,65 @@ +package dev.cyberarm.minibots.red_crab.states; + +import com.qualcomm.robotcore.util.Range; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.yellow.YellowMinibot; + +public class Rotate extends CyberarmState { + final private RedCrabMinibot robot; + final private String groupName, actionName; + + final private double maxPower, minPower, lerpDegrees, headingDegrees, toleranceDegrees; + final private int timeoutMS; + + public Rotate(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.groupName = groupName; + this.actionName = actionName; + + this.minPower = robot.config.variable(groupName, actionName, "minPower").value(); + this.maxPower = robot.config.variable(groupName, actionName, "maxPower").value(); + + this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value(); + this.headingDegrees = robot.config.variable(groupName, actionName, "headingDEGREES").value(); + this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREEES").value(); + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + } + + @Override + public void exec() { + double angleDiff = Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees); + + if (Math.abs(angleDiff) <= toleranceDegrees || runTime() >= timeoutMS) { + robot.left.set(0); + robot.right.set(0); + + this.finished(); + + return; + } + + double power = Utilities.lerp(minPower, maxPower, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0)); + + if (angleDiff > 0) { + robot.left.set(-power); + robot.right.set(power); + } else { + robot.left.set(power); + robot.right.set(-power); + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + engine.telemetry.addData("Robot Heading", Utilities.facing(robot.imu)); + engine.telemetry.addData("Robot Target Heading", headingDegrees); + engine.telemetry.addData("Robot Angle Diff", Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees)); + engine.telemetry.addData("Robot Turn Rate", Utilities.turnRate(robot.imu)); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java new file mode 100644 index 0000000..7bfd28a --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java @@ -0,0 +1,19 @@ +package dev.cyberarm.minibots.red_crab.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class StrafeMove extends CyberarmState { + private final RedCrabMinibot robot; + private final String groupName, actionName; + public StrafeMove(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void exec() { + + } +}