diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java index 8dd4318..ddafd37 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java @@ -1,7 +1,10 @@ package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines; import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DepositorDoor; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit; import org.timecrafters.FreightFrenzy.Competition.Common.Robot; public class RedWarehouseEngine extends CyberarmEngine { @@ -9,6 +12,14 @@ public class RedWarehouseEngine extends CyberarmEngine { public void setup() { Robot robot = new Robot(this); - addState(new DriveState(robot, 2000, 2000,1,1)); + addState(new TurretOrbit(robot, robot.turretServoWhite, 3, 1)); + addState(new TurretArmExtension(robot, robot.whiteArmBobbin, 3000, .75, 150)); + addState(new DepositorDoor(robot.whiteDispenser, .5, 1)); + addState(new DepositorDoor(robot.whiteDispenser, 0, 0)); + addState(new TurretArmExtension(robot, robot.whiteArmBobbin, 0, 1, 150)); + addState(new TurretOrbit(robot, robot.turretServoWhite, 3, -1)); + addState(new DriveState(robot, 1500, 500, 1, .75)); + addState(new DriveState(robot, 1500, 1500, 1, 1)); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java new file mode 100644 index 0000000..a799949 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java @@ -0,0 +1,35 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmState; + +public class CollectorToggle extends CyberarmState { + final static public int MODE_REVERSE = -1; + final static public int MODE_COLLECT = 1; + final static public int MODE_STOPPED = 0; + + CRServo servo; + int mode; + + public CollectorToggle(CRServo servo, int mode) { + this.servo = servo; + this.mode = mode; + } + + @Override + public void exec() { + if (mode == MODE_REVERSE){ + servo.setPower(-1); + } + + else if (mode == MODE_COLLECT){ + servo.setPower(1); + } + + else { + servo.setPower(0); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DepositorDoor.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DepositorDoor.java new file mode 100644 index 0000000..9cc9e48 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DepositorDoor.java @@ -0,0 +1,29 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmState; + +public class DepositorDoor extends CyberarmState { + + Servo servo; + double targetPosition; + long time; + + public DepositorDoor(Servo servo, double targetPosition, long time) { + this.servo = servo; + this.targetPosition = targetPosition; + this.time = time; + } + + @Override + public void exec() { + if (runTime() < time) { + servo.setPosition(targetPosition); + } + else { + setHasFinished(true); + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java index 23a1fae..fb06799 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java @@ -9,18 +9,29 @@ public class TurretArmExtension extends CyberarmState { private Robot robot; private DcMotor motor; - private int position; + private int targetPosition, tolerance; private double power; - public TurretArmExtension(Robot robot, DcMotor motor, int position, double power) { + public TurretArmExtension(Robot robot, DcMotor motor, int targetPosition, double power, int tolerance) { this.robot = robot; this.motor = motor; - this.position = position; + this.targetPosition = targetPosition; this.power = power; + this.tolerance = tolerance; } @Override public void exec() { + if (motor.getCurrentPosition() < targetPosition + tolerance){ + motor.setPower(power); + } + else if (motor.getCurrentPosition() > targetPosition - tolerance){ + motor.setPower(-power); + } + else { + setHasFinished(true); + motor.setPower(0); + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java new file mode 100644 index 0000000..f1f6c6b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java @@ -0,0 +1,37 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +public class TurretArmRiser extends CyberarmState { + + private Robot robot; + private DcMotor motor; + private int targetPosition, tolerance; + private double power; + + public TurretArmRiser(Robot robot, DcMotor motor, int targetPosition, double power, int tolerance) { + this.robot = robot; + this.motor = motor; + this.targetPosition = targetPosition; + this.power = power; + this.tolerance = tolerance; + } + + @Override + public void exec() { + if (motor.getCurrentPosition() < targetPosition + tolerance){ + motor.setPower(power); + } + + else if (motor.getCurrentPosition() > targetPosition - tolerance){ + motor.setPower(-power); + } + else { + setHasFinished(true); + motor.setPower(0); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/VuforiaState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/VuforiaState.java new file mode 100644 index 0000000..50f2115 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/VuforiaState.java @@ -0,0 +1,36 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +public class VuforiaState extends CyberarmState { + + Robot robot; + + public VuforiaState(Robot robot) { + this.robot = robot; + } + + @Override + public void start() { + robot.activateVuforia(); + robot.activateTensorflow(); + } + + @Override + public void exec() { + for (Recognition recognition : robot.tensorflowDetections()) { + } + } + + @Override + public void telemetry() { + for (Recognition recognition : robot.tensorflowDetections()) { + engine.telemetry.addData("Label", recognition.getLabel()); + engine.telemetry.addData("Left", recognition.getLeft()); + engine.telemetry.addData("Top", recognition.getTop()); + engine.telemetry.addData("Confidence", recognition.getConfidence()); + } + } +}