diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java index 83a3dec..6188e11 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java @@ -5,9 +5,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.Autonomous.States.CollectorState; import org.timecrafters.Autonomous.States.DriverState; -import org.timecrafters.Autonomous.States.LowerArm; +import org.timecrafters.Autonomous.States.BottomArm; import org.timecrafters.Autonomous.States.RotationState; -import org.timecrafters.Autonomous.States.UpperArm; +import org.timecrafters.Autonomous.States.TopArm; import org.timecrafters.testing.states.PrototypeBot1; @Autonomous (name = "Autonomous Test") @@ -24,25 +24,25 @@ public class TestAutonomousEngine extends CyberarmEngine { //turn towards high pole addState(new RotationState(robot, "TestAutonomous", "02-0")); //lift the upper arm - addState(new UpperArm(robot, "TestAutonomous", "03-0")); + addState(new TopArm(robot, "TestAutonomous", "03-0")); //lift the lower arm - addState(new LowerArm(robot, "TestAutonomous", "04-0")); + addState(new BottomArm(robot, "TestAutonomous", "04-0")); //drive forward addState(new DriverState(robot, "TestAutonomous", "05-0")); //lower the bottom arm to get closer - addState(new LowerArm(robot, "TestAutonomous", "06-0")); + addState(new BottomArm(robot, "TestAutonomous", "06-0")); //make collector release the cone - addState(new CollectorState(robot, "TestAutnomous", "07-0")); + addState(new CollectorState(robot, "TestAutonomous", "07-0")); //lift the lower arm to clear the pole - addState(new LowerArm(robot, "TestAutonomous", "08-0")); + addState(new BottomArm(robot, "TestAutonomous", "08-0")); //Drive Backwards addState(new DriverState(robot, "TestAutonomous", "09-0")); // Rotate to either set up for cones to grab or park somewhere addState(new RotationState(robot, "TestAutonomous", "10-0")); // lower the bottom arm so we dont fall over - addState(new LowerArm(robot, "TestAutonomous", "11-0")); + addState(new BottomArm(robot, "TestAutonomous", "11-0")); // lower the upper arm so we dont fall over - addState(new UpperArm(robot, "TestAutonomous", "12-0")); + addState(new TopArm(robot, "TestAutonomous", "12-0")); //either dont move if your in the right zone otherwise drive forward a specific variable if we aren't already in the right spot addState(new DriverState(robot, "TestAutonomous", "13-0")); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java new file mode 100644 index 0000000..4c044f8 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java @@ -0,0 +1,59 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class BottomArm extends CyberarmState { + + PrototypeBot1 robot; + double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance; + long time; + long lastStepTime = 0; + boolean up; + + public BottomArm(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.LowerRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value(); + this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value(); + this.time = robot.configuration.variable(groupName, actionName, "time").value(); + this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); + + } + + @Override + public void start() { + up = robot.LowRiserLeft.getPosition() < LowerRiserLeftPos; + } + + @Override + public void exec() { + + if (System.currentTimeMillis() - lastStepTime >= time) { + lastStepTime = System.currentTimeMillis(); + + if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); + + } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); + + } else { + setHasFinished(true); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("LowerRiserRightPos",LowerRiserRightPos); + engine.telemetry.addData("LowerRiserLeftPos",LowerRiserLeftPos); + engine.telemetry.addData("AddedDistance",AddedDistance); + engine.telemetry.addData("left servo", robot.LowRiserLeft.getPosition()); + engine.telemetry.addData("right servo", robot.LowRiserRight.getPosition()); + engine.telemetry.addData("time",time); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java index 623a30f..d010627 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java @@ -8,16 +8,18 @@ public class DriverState extends CyberarmState { public DriverState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); + this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + } private float RobotRotation; private double drivePower; - private int RobotPosition,RobotStartingPosition; + private int RobotPosition,RobotStartingPosition,traveledDistance; @Override public void exec() { - if (RobotPosition - RobotStartingPosition < 2500){ + if (RobotPosition - RobotStartingPosition < traveledDistance){ drivePower = 1; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); @@ -28,6 +30,7 @@ public class DriverState extends CyberarmState { robot.backRightDrive.setPower(0); robot.frontLeftDrive.setPower(0); robot.frontRightDrive.setPower(0); + setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java deleted file mode 100644 index e626f67..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/LowerArm.java +++ /dev/null @@ -1,32 +0,0 @@ -package org.timecrafters.Autonomous.States; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.testing.states.PrototypeBot1; - -public class LowerArm extends CyberarmState { - - PrototypeBot1 robot; - double LowerRiserRightPos, LowerRiserLeftPos, AddedDistance; - long time; - long lastStepTime = 0; - - public LowerArm(PrototypeBot1 robot, String groupName, String actionName) { - this.robot = robot; - this.LowerRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value(); - this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value(); - this.time = robot.configuration.variable(groupName, actionName, "time").value(); - this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); - - } - - @Override - public void exec() { - if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos) { - if (System.currentTimeMillis() - lastStepTime >= time) { - lastStepTime = System.currentTimeMillis(); - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); - } - } - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java index ba1da5c..2dc9d19 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -8,18 +8,20 @@ public class RotationState extends CyberarmState { 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(); + this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value(); + } - private float RobotRotation; private double drivePower; + private float targetRotation; + float RobotRotation; @Override public void exec() { RobotRotation = robot.imu.getAngularOrientation().firstAngle; - if (RobotRotation <= 45) { + if (RobotRotation - 3 <= targetRotation || RobotRotation + 3 <= targetRotation) { robot.backLeftDrive.setPower(-drivePower); robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(-drivePower); @@ -30,7 +32,15 @@ public class RotationState extends CyberarmState { robot.backRightDrive.setPower(0); robot.frontLeftDrive.setPower(0); robot.frontRightDrive.setPower(0); + setHasFinished(true); } } + + @Override + public void telemetry() { + engine.telemetry.addData("Robot Rotation", RobotRotation); + engine.telemetry.addData("Drive Power", drivePower); + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java new file mode 100644 index 0000000..baaf08e --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java @@ -0,0 +1,60 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class TopArm extends CyberarmState { + + PrototypeBot1 robot; + double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance; + long time; + long lastStepTime = 0; + boolean up; + + public TopArm(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "UpperRiserLeftPos").value(); + this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "UpperRiserRightPos").value(); + this.time = robot.configuration.variable(groupName, actionName, "time").value(); + this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); + } + + @Override + public void start() { + up = robot.HighRiserLeft.getPosition() < UpperRiserLeftPos; + } + + @Override + public void exec() { + if (System.currentTimeMillis() - lastStepTime >= time) { + lastStepTime = System.currentTimeMillis(); + + if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) { + + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance); + + } else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) { + + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance); + + } else { + setHasFinished(true); + } + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("UpperRiserRightPos",UpperRiserRightPos); + engine.telemetry.addData("UpperRiserLeftPos",UpperRiserLeftPos); + engine.telemetry.addData("AddedDistance",AddedDistance); + engine.telemetry.addData("time",time); + engine.telemetry.addData("servo position left", robot.HighRiserLeft.getPosition()); + engine.telemetry.addData("servo position Right", robot.HighRiserRight.getPosition()); + + + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java deleted file mode 100644 index 8823247..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.timecrafters.Autonomous.States; - -import org.cyberarm.engine.V2.CyberarmState; -import org.timecrafters.testing.states.PrototypeBot1; - -public class UpperArm extends CyberarmState { - - PrototypeBot1 robot; - double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance; - long time; - long lastStepTime = 0; - - public UpperArm(PrototypeBot1 robot, String groupName, String actionName) { - this.robot = robot; - this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "LowerRiserLeftPos").value(); - this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value(); - this.time = robot.configuration.variable(groupName, actionName, "time").value(); - this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); - } - - @Override - public void exec() { - if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos) { - if (System.currentTimeMillis() - lastStepTime >= time) { - lastStepTime = System.currentTimeMillis(); - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance); - } else { - setHasFinished(true); - - } - } - } -} 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 101d577..d689026 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -48,8 +48,8 @@ public class PrototypeBot1 { public PrototypeBot1(CyberarmEngine engine) { this.engine = engine; - initVuforia(); - initTfod(); +// initVuforia(); +// initTfod(); setupRobot(); } @@ -66,7 +66,7 @@ public class PrototypeBot1 { imu.startAccelerationIntegration(new Position(), new Velocity(), 10); - ///configuration = new TimeCraftersConfiguration("Phoenix"); + configuration = new TimeCraftersConfiguration("Phoenix"); //motors configuration frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left"); @@ -99,6 +99,17 @@ public class PrototypeBot1 { backRightDrive.setDirection(DcMotorSimple.Direction.FORWARD); backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + HighRiserLeft.setDirection(Servo.Direction.REVERSE); + HighRiserRight.setDirection(Servo.Direction.FORWARD); + LowRiserLeft.setDirection(Servo.Direction.FORWARD); + LowRiserRight.setDirection(Servo.Direction.REVERSE); + + LowRiserLeft.setPosition(0.45); + LowRiserRight.setPosition(0.45); + HighRiserLeft.setPosition(0.45); + HighRiserRight.setPosition(0.45); + } private void initVuforia(){