diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java index 36303a4..bea0a49 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ArmStateSodi.java @@ -1,5 +1,9 @@ package org.timecrafters.CenterStage.Autonomous.States; +/**BEGAN WITH 43 LINES**/ + +import com.arcrobotics.ftclib.hardware.motors.Motor; + import org.timecrafters.CenterStage.Common.ProtoBotSodi; import org.timecrafters.CenterStage.Common.PrototypeRobot; @@ -10,12 +14,24 @@ public class ArmStateSodi extends CyberarmState { private final boolean stateDisabled; ProtoBotSodi robot; private int testSequence; + private int targetPos; + private int currentPos; + private int totalDist; + public ArmStateSodi(ProtoBotSodi robot, String groupName, String actionName) { this.robot = robot; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } + private int getTotalDist() { + int totalDist = targetPos - currentPos; + return totalDist; + } + + + + @Override public void start() { //add variables that need to be reinitialized @@ -23,18 +39,34 @@ public class ArmStateSodi extends CyberarmState { } @Override - public void exec() { - - if (robot.liftMotor.motor.getCurrentPosition() < 0) { - robot.liftMotor.motor.setPower(0); - } - if (robot.liftMotor.motor.getCurrentPosition() >= 0 && robot.liftMotor.motor.getCurrentPosition() <= 49) { - - } - if (robot.liftMotor.motor.getCurrentPosition() >= 50 && robot.liftMotor.motor.getCurrentPosition() <= 250) { - - } + public void init() { + } + + @Override + public void exec() { + +// if (robot.liftMotor.motor.getCurrentPosition() >= 2800 && +// Math.abs(robot.liftMotor.motor.getPower()) != robot.liftMotor.motor.getPower()) { +// robot.liftMotor.motor.setPower(0); +// } +// +// if (robot.liftMotor.motor.getCurrentPosition() < 0 && +// Math.abs(robot.liftMotor.motor.getPower()) != robot.liftMotor.motor.getPower()) { +// robot.liftMotor.motor.setPower(0); +// } +// +// if (robot.liftMotor.motor.getCurrentPosition() >= 0 && +// robot.liftMotor.motor.getCurrentPosition() <= 50) { +// +// } +// +// if (robot.liftMotor.motor.getCurrentPosition() >= 50 && +// robot.liftMotor.motor.getCurrentPosition() <= 250) { +// +// } +// +// } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java index ed2851a..182ff88 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/ProtoBotStateSodi.java @@ -14,7 +14,7 @@ public class ProtoBotStateSodi extends CyberarmState { private int testSequence; private int targetPos; private int currentPos; - private int totalDist; // + private int totalDist; public ProtoBotStateSodi(ProtoBotSodi robot) { this.robot = robot; @@ -37,14 +37,14 @@ public class ProtoBotStateSodi extends CyberarmState { robot.frDrive.motor.setPower(0); robot.blDrive.motor.setPower(0); robot.brDrive.motor.setPower(0); - robot.liftMotor.motor.setPower(0); - - robot.grabJaw.setPosition(0); - robot.grabElbow.setPosition(0); - robot.grabShoulder.setPosition(0); - robot.dropShoulder.setPosition(0); - robot.dropElbow.setPosition(0); - robot.dropJaw.setPosition(0); +// robot.liftMotor.motor.setPower(0); +// +// robot.grabJaw.setPosition(0); +// robot.grabElbow.setPosition(0); +// robot.grabShoulder.setPosition(0); +// robot.dropShoulder.setPosition(0); +// robot.dropElbow.setPosition(0); +// robot.dropJaw.setPosition(0); lastTimeChecked = System.currentTimeMillis(); testSequence = 0; @@ -57,55 +57,42 @@ public class ProtoBotStateSodi extends CyberarmState { @Override public void exec() { - currentPos = robot.liftMotor.motor.getCurrentPosition(); -// -// if (System.currentTimeMillis() - lastTimeChecked >= 500 && System.currentTimeMillis() - lastTimeChecked < 2500) { -// robot.flDrive.motor.setPower(0.5); -// robot.frDrive.motor.setPower(0.5); -// robot.blDrive.motor.setPower(0.5); -// robot.brDrive.motor.setPower(0.5); -// robot.liftMotor.motor.setPower(0.5); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 2500 && System.currentTimeMillis() - lastTimeChecked < 4500) { -// robot.flDrive.motor.setPower(-0.5); -// robot.frDrive.motor.setPower(-0.5); -// robot.blDrive.motor.setPower(-0.5); -// robot.brDrive.motor.setPower(-0.5); -// robot.liftMotor.motor.setPower(-0.5); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 4500 && System.currentTimeMillis() - lastTimeChecked < 6500) { -// robot.flDrive.motor.setPower(0.5); -// robot.frDrive.motor.setPower(0.5); -// robot.blDrive.motor.setPower(-0.5); -// robot.brDrive.motor.setPower(-0.5); -// robot.liftMotor.motor.setPower(0); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 6500 && System.currentTimeMillis() - lastTimeChecked < 8500) { -// robot.flDrive.motor.setPower(-0.5); -// robot.frDrive.motor.setPower(-0.5); -// robot.blDrive.motor.setPower(0.5); -// robot.brDrive.motor.setPower(0.5); -// robot.liftMotor.motor.setPower(0); -// } else if (System.currentTimeMillis() - lastTimeChecked >= 8600){ -// robot.flDrive.motor.setPower(0); -// robot.frDrive.motor.setPower(0); -// robot.blDrive.motor.setPower(0); -// robot.brDrive.motor.setPower(0); -// robot.liftMotor.motor.setPower(0); -// setHasFinished(true); -// } + currentPos = robot.flDrive.motor.getCurrentPosition(); + + if (testSequence < 1) { + testSequence = 1; + } switch (testSequence) { case 1: - robot.liftMotor.motor.setPower(0.4); - robot.liftMotor.motor.setTargetPosition(targetPos); - //lift motor go up for some way - //wait for about 0.25 + robot.flDrive.motor.setTargetPosition(500); + robot.frDrive.motor.setTargetPosition(500); + robot.blDrive.motor.setTargetPosition(500); + robot.brDrive.motor.setTargetPosition(500); + + if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() - 50) { + robot.flDrive.motor.setPower(0.5 * (robot.flDrive.motor.getTargetPosition() - robot.flDrive.motor.getCurrentPosition())); + robot.frDrive.motor.setPower(0.5 * (robot.frDrive.motor.getTargetPosition() - robot.frDrive.motor.getCurrentPosition())); + robot.blDrive.motor.setPower(0.5 * (robot.blDrive.motor.getTargetPosition() - robot.blDrive.motor.getCurrentPosition())); + robot.brDrive.motor.setPower(0.5 * (robot.brDrive.motor.getTargetPosition() - robot.brDrive.motor.getCurrentPosition())); + } + else if (robot.flDrive.motor.getCurrentPosition() < robot.flDrive.motor.getTargetPosition() + 50 || + robot.flDrive.motor.getCurrentPosition() > robot.flDrive.motor.getTargetPosition() - 50) { + + robot.flDrive.motor.setPower(0); + robot.frDrive.motor.setPower(0); + robot.blDrive.motor.setPower(0); + robot.brDrive.motor.setPower(0); + +// testSequence = 2; + } + + break; +// case 2: + - case 2: - robot.liftMotor.motor.setTargetPosition(targetPos); - //lift motor go down - //repeat - //wait for about 0.25 } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index 793834d..8630b1d 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -25,8 +25,8 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class ProtoBotSodi extends Robot { public HardwareMap hardwareMap; - public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor; - public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; + public MotorEx flDrive, frDrive, blDrive, brDrive /*liftMotor*/; +// public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; private String string; private CyberarmEngine engine; @@ -43,40 +43,40 @@ public class ProtoBotSodi extends Robot { this.hardwareMap = CyberarmEngine.instance.hardwareMap; this.engine = CyberarmEngine.instance; - TimeCraftersConfiguration configuration = new TimeCraftersConfiguration("Robbie"); +// TimeCraftersConfiguration configuration = new TimeCraftersConfiguration(); //Motors frDrive = new MotorEx(hardwareMap, "FrontRight"); flDrive = new MotorEx(hardwareMap, "FrontLeft"); brDrive = new MotorEx(hardwareMap, "BackRight"); blDrive = new MotorEx(hardwareMap, "BackLeft"); - liftMotor = new MotorEx(hardwareMap, "Lift"); +// liftMotor = new MotorEx(hardwareMap, "Lift"); - flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); +// liftMotor.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); flDrive.motor.setDirection(FORWARD); - frDrive.motor.setDirection(REVERSE); +// frDrive.motor.setDirection(REVERSE); blDrive.motor.setDirection(FORWARD); - brDrive.motor.setDirection(REVERSE); +// brDrive.motor.setDirection(REVERSE); //Servos - grabJaw = hardwareMap.servo.get("GrabJaw"); - grabElbow = hardwareMap.servo.get("GrabElbow"); - grabShoulder = hardwareMap.servo.get("GrabShoulder"); - dropShoulder = hardwareMap.servo.get("DropShoulder"); - dropElbow = hardwareMap.servo.get("DropElbow"); - dropJaw = hardwareMap.servo.get("DropJaw"); - - grabElbow.setDirection(Servo.Direction.FORWARD); - grabJaw.setDirection(Servo.Direction.FORWARD); - grabShoulder.setDirection(Servo.Direction.FORWARD); - dropShoulder.setDirection(Servo.Direction.FORWARD); - dropElbow.setDirection(Servo.Direction.FORWARD); - dropJaw.setDirection(Servo.Direction.FORWARD); +// grabJaw = hardwareMap.servo.get("GrabJaw"); +// grabElbow = hardwareMap.servo.get("GrabElbow"); +// grabShoulder = hardwareMap.servo.get("GrabShoulder"); +// dropShoulder = hardwareMap.servo.get("DropShoulder"); +// dropElbow = hardwareMap.servo.get("DropElbow"); +// dropJaw = hardwareMap.servo.get("DropJaw"); +// +// grabElbow.setDirection(Servo.Direction.FORWARD); +// grabJaw.setDirection(Servo.Direction.FORWARD); +// grabShoulder.setDirection(Servo.Direction.FORWARD); +// dropShoulder.setDirection(Servo.Direction.FORWARD); +// dropElbow.setDirection(Servo.Direction.FORWARD); +// dropJaw.setDirection(Servo.Direction.FORWARD); }