From 089ca3dee9b8fb419b39f59c9e5ab99a183bfa2e Mon Sep 17 00:00:00 2001 From: NerdyBirdy460 <111399933+NerdyBirdy460@users.noreply.github.com> Date: Sat, 14 Oct 2023 12:46:00 -0500 Subject: [PATCH] Still working on making the Controller Hub recognize my programs, setup buggy --- .../Engines/ProtoBotEngineSodi.java | 7 ++- .../Autonomous/States/ProtoBotStateSodi.java | 52 ++++++++++++++----- .../CenterStage/Common/ProtoBotSodi.java | 39 +++++++++++--- 3 files changed, 74 insertions(+), 24 deletions(-) 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 a92934f..6584d82 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 @@ -1,19 +1,18 @@ package org.timecrafters.CenterStage.Autonomous.Engines; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +//import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.timecrafters.CenterStage.Common.ProtoBotSodi; import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi; -import org.timecrafters.CenterStage.Autonomous.States.ProtoBotStateSodi; import dev.cyberarm.engine.V2.CyberarmEngine; -@TeleOp(name = "Rigel", group = "Robot") +@Autonomous(name = "Rigel", group = "PROTOTYPE") public class ProtoBotEngineSodi extends CyberarmEngine { private ProtoBotSodi robot; @Override public void setup() { - this.robot = new ProtoBotSodi(); + this.robot = new ProtoBotSodi("Hello World"); this.robot.setup(); addState(new ProtoBotStateSodi()); 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 2121628..c126f99 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 @@ -1,6 +1,7 @@ package org.timecrafters.CenterStage.Autonomous.States; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; import dev.cyberarm.engine.V2.CyberarmState; @@ -9,25 +10,34 @@ import org.timecrafters.CenterStage.Common.ProtoBotSodi; public class ProtoBotStateSodi extends CyberarmState { ProtoBotSodi robot; + private double avgVelocity, avgDrivePower; public ProtoBotStateSodi() { - this.robot = robot; + } public void telemetry() { + 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()); + engine.telemetry.addData("Back Right Velocity", robot.brDrive.getVelocity()); + engine.telemetry.addData("Front Left Power", robot.flDrive.motor.getPower()); + engine.telemetry.addData("Front Right Power", robot.frDrive.motor.getPower()); + engine.telemetry.addData("Back Left Power", robot.blDrive.motor.getPower()); + engine.telemetry.addData("Back Right Power", robot.brDrive.motor.getPower()); } - @Override - public void start() { - //Motors - 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.bloodWorm.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - //Servos - robot.jaw.setDirection(Servo.Direction.FORWARD); + 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; + } + @Override public void init() { robot.flDrive.motor.setPower(0); @@ -35,6 +45,14 @@ public class ProtoBotStateSodi extends CyberarmState { robot.blDrive.motor.setPower(0); robot.brDrive.motor.setPower(0); robot.bloodWorm.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); + } @Override @@ -52,6 +70,16 @@ public class ProtoBotStateSodi extends CyberarmState { robot.blDrive.motor.setPower(-0.5); robot.brDrive.motor.setPower(-0.5); robot.bloodWorm.motor.setPower(-0.5); + } else if (System.currentTimeMillis() >= 3500 && System.currentTimeMillis() < 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); + } else if (System.currentTimeMillis() >= 4500 && System.currentTimeMillis() < 5500) { + 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.flDrive.motor.setPower(0); robot.frDrive.motor.setPower(0); 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 8a54e75..6da745e 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -23,24 +23,47 @@ public class ProtoBotSodi extends Robot { public HardwareMap hardwareMap; public MotorEx flDrive, frDrive, blDrive, brDrive, bloodWorm; - public CRServo fang; - public Servo jaw; - + public Servo fang, jaw, neck, shoulder, wrist, hand; + public ProtoBotSodi robot; private TimeCraftersConfiguration configuration; + private String string; + private CyberarmEngine engine; + + public ProtoBotSodi(String string) { + this.engine = engine; + this.string = string; + } @Override - public void setup() { + public void setup() {System.out.println("Bacon: " + this.string); + this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.engine = CyberarmEngine.instance; configuration = new TimeCraftersConfiguration("Robbie"); - //Motors - flDrive = new MotorEx(hardwareMap, "frontLeft"); + //Motors + //MOTORS frDrive = new MotorEx(hardwareMap, "frontRight"); - blDrive = new MotorEx(hardwareMap, "backLeft"); + flDrive = new MotorEx(hardwareMap, "frontLeft"); brDrive = new MotorEx(hardwareMap, "backRight"); - bloodWorm = new MotorEx(hardwareMap, "lift"); + blDrive = new MotorEx(hardwareMap, "backLeft"); + + 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.bloodWorm.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + //The motor bloodWorm is the one that raises the deliverer. //The reason it's called bloodworm is because of what bloodworms are. Better left unsaid. + //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); + } }