diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java new file mode 100644 index 0000000..e94a68e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java @@ -0,0 +1,58 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class CollectorState extends CyberarmState { + + private final PrototypeBot1 robot; + private boolean collecting; + private long duration; + private long BeginningofActionTime; + + + public CollectorState(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + + this.duration = robot.configuration.variable(groupName, actionName, "Duration").value(); + this.collecting = robot.configuration.variable(groupName, actionName, "Collecting").value(); + } + + + @Override + public void telemetry() { + engine.telemetry.addData("High Riser Right Position", robot.HighRiserRight.getPosition()); + engine.telemetry.addData("High Riser Left Position", robot.HighRiserLeft.getPosition()); + } + + @Override + public void start() { + BeginningofActionTime = System.currentTimeMillis(); + } + + @Override + public void exec() { + + if (System.currentTimeMillis() - BeginningofActionTime < duration) { + if (collecting) { + robot.collectorRight.setPower(1); + robot.collectorLeft.setPower(1); + } else { + robot.collectorRight.setPower(-1); + robot.collectorLeft.setPower(-1); + + } + + } else { + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); + setHasFinished(true); + } + + + + } + + } + + diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java new file mode 100644 index 0000000..ba1da5c --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -0,0 +1,36 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class RotationState extends CyberarmState { + PrototypeBot1 robot; + public RotationState(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); +// this.RobotRotation = robot.configuration.variable(groupName, actionName, "RobotRotation").value(); + } + + private float RobotRotation; + private double drivePower; + + @Override + public void exec() { + + + RobotRotation = robot.imu.getAngularOrientation().firstAngle; + if (RobotRotation <= 45) { + robot.backLeftDrive.setPower(-drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(-drivePower); + robot.frontRightDrive.setPower(drivePower); + } else + { + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixWingEngine.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixWingEngine.java new file mode 100644 index 0000000..94e6770 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixWingEngine.java @@ -0,0 +1,27 @@ +package org.timecrafters.testing.states; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +import org.cyberarm.engine.V2.CyberarmEngine; + +public class PhoenixWingEngine { + + public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight; + private final CyberarmEngine engine; + + public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive; + + public CRServo collectorLeft, collectorRight; + + public BNO055IMU imu; + +// public Servo collectorWrist; + + public PhoenixWingEngine(CyberarmEngine engine) { + this.engine = engine; + + setupRobot(); + } diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixWingState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixWingState.java new file mode 100644 index 0000000..d18601b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixWingState.java @@ -0,0 +1,10 @@ +package org.timecrafters.testing.states; + +import org.cyberarm.engine.V2.CyberarmState; + +public class PhoenixWingState extends CyberarmState { + @Override + public void exec() { + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index bbeb323..0149198 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -58,6 +58,8 @@ public class PrototypeBot1 { // Collector collectorLeft = engine.hardwareMap.crservo.get("Collector Left"); collectorRight = engine.hardwareMap.crservo.get("Collector Right"); + collectorLeft.setDirection(CRServo.Direction.REVERSE); + collectorRight.setDirection(CRServo.Direction.FORWARD); // Arm LowRiserLeft = engine.hardwareMap.servo.get("LowRiserLeft");