diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java index 86656de..91dd568 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/Engines/ProtoBotEngineSodi.java @@ -7,14 +7,14 @@ import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi; import dev.cyberarm.engine.V2.CyberarmEngine; -@Autonomous(name = "Autonomous (Sodi)", group = "PROTOTYPE") +@Autonomous(name = "Autonomous- Sodi", group = "Simple Test") public class ProtoBotEngineSodi extends CyberarmEngine { private ProtoBotSodi robot; @Override public void setup() { - this.robot = new ProtoBotSodi("Hello World"); + this.robot = new ProtoBotSodi("Autonomous - Sodi"); this.robot.setup(); - addState(new ProtoBotStateSodi()); + addState(new ProtoBotStateSodi(robot)); } } 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 a50be03..50abf75 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 @@ -11,12 +11,13 @@ import org.timecrafters.CenterStage.Common.ProtoBotSodi; public class ProtoBotStateSodi extends CyberarmState { ProtoBotSodi robot; private double avgVelocity, avgDrivePower; - public ProtoBotStateSodi() { - + private long lastTimeChecked; + public ProtoBotStateSodi(ProtoBotSodi robot) { + this.robot = robot; } public void telemetry() { - engine.telemetry.addData("Avg Drive Velocity", avgVelocity); - engine.telemetry.addData("Avg Drive Power", avgDrivePower); +// engine.telemetry.addData("Avg Drive Velocity", avgVelocity); +// engine.telemetry.addData("Avg Drive Power", avgDrivePower); engine.telemetry.addData("Front Left Velocity", robot.flDrive.getVelocity()); engine.telemetry.addData("Front Right Velocity", robot.frDrive.getVelocity()); engine.telemetry.addData("Back Left Velocity", robot.blDrive.getVelocity()); @@ -28,15 +29,15 @@ public class ProtoBotStateSodi extends CyberarmState { } - public double getAvgDrivePower() { - avgDrivePower = (robot.flDrive.motor.getPower() + robot.frDrive.motor.getPower() + robot.blDrive.motor.getPower() + robot.brDrive.motor.getPower())/4; - return avgDrivePower; - } +// public double getAvgDrivePower() { +// avgDrivePower = (robot.flDrive.motor.getPower() + robot.frDrive.motor.getPower() + robot.blDrive.motor.getPower() + robot.brDrive.motor.getPower())/4; +// return avgDrivePower; +// } - public double getAvgVelocity() { - avgVelocity = (robot.flDrive.getVelocity() + robot.frDrive.getVelocity() + robot.blDrive.getVelocity() + robot.brDrive.getVelocity())/4; - return avgVelocity; - } +// public double getAvgVelocity() { +// avgVelocity = (robot.flDrive.getVelocity() + robot.frDrive.getVelocity() + robot.blDrive.getVelocity() + robot.brDrive.getVelocity())/4; +// return avgVelocity; +// } @Override public void init() { @@ -46,46 +47,52 @@ public class ProtoBotStateSodi extends CyberarmState { robot.brDrive.motor.setPower(0); robot.liftMotor.motor.setPower(0); -// robot.fang.setPosition(0); -// robot.jaw.setPosition(0); -// robot.neck.setPosition(0); -// robot.shoulder.setPosition(0); -// robot.wrist.setPosition(0); -// robot.hand.setPosition(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(); + } @Override public void exec() { - if (System.currentTimeMillis() >= 500 && System.currentTimeMillis() < 2500) { + 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() >= 2500 && System.currentTimeMillis() < 3500) { + } 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() >= 3500 && System.currentTimeMillis() < 4500) { + } 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); - } else if (System.currentTimeMillis() >= 4500 && System.currentTimeMillis() < 5500) { + 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); - } else { + 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); } } } 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 499ef2a..0d341a4 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -1,5 +1,8 @@ package org.timecrafters.CenterStage.Common; +import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.FORWARD; +import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.REVERSE; + import org.timecrafters.Library.Robot; import com.arcrobotics.ftclib.drivebase.HDrive; import com.arcrobotics.ftclib.hardware.RevIMU; @@ -23,8 +26,7 @@ public class ProtoBotSodi extends Robot { public HardwareMap hardwareMap; public MotorEx flDrive, frDrive, blDrive, brDrive, liftMotor; - public Servo fang, jaw, neck, shoulder, wrist, hand; - public ProtoBotSodi robot; + public Servo grabJaw, grabElbow, grabShoulder, dropShoulder, dropElbow, dropJaw; private String string; private CyberarmEngine engine; @@ -40,33 +42,39 @@ public class ProtoBotSodi extends Robot { TimeCraftersConfiguration configuration = new TimeCraftersConfiguration("Robbie"); - //Motors - //MOTORS - frDrive = new MotorEx(hardwareMap, "frontRight"); - flDrive = new MotorEx(hardwareMap, "frontLeft"); - brDrive = new MotorEx(hardwareMap, "backRight"); - blDrive = new MotorEx(hardwareMap, "backLeft"); + //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"); - robot.flDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - robot.frDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - robot.blDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - robot.brDrive.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - robot.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); - //Servos - robot.jaw.setDirection(Servo.Direction.FORWARD); - robot.fang.setDirection(Servo.Direction.FORWARD); - robot.neck.setDirection(Servo.Direction.FORWARD); - robot.shoulder.setDirection(Servo.Direction.FORWARD); - robot.wrist.setDirection(Servo.Direction.FORWARD); - robot.hand.setDirection(Servo.Direction.FORWARD); + flDrive.motor.setDirection(FORWARD); + frDrive.motor.setDirection(REVERSE); + blDrive.motor.setDirection(FORWARD); + 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); -// fang = hardwareMap.servo.get("Fang"); -// jaw = hardwareMap.servo.get("Jaw"); -// neck = hardwareMap.servo.get("Neck"); -// shoulder = hardwareMap.servo.get("Shoulder"); -// wrist = hardwareMap.servo.get("Wrist"); -// hand = hardwareMap.servo.get("Hand"); } }