diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java new file mode 100644 index 0000000..2c13994 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/UpperArm.java @@ -0,0 +1,32 @@ +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; + 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(); + } + + @Override + public void exec() { + if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos) { + if (System.currentTimeMillis() - lastStepTime >= time) { + lastStepTime = System.currentTimeMillis(); + robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - UpperRiserLeftPos); + robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - UpperRiserRightPos); + + + } + } + } +}