diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MotorPortTestRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MotorPortTestRobot.java new file mode 100644 index 0000000..6023538 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MotorPortTestRobot.java @@ -0,0 +1,38 @@ +package org.timecrafters.CenterStage.Common; + +import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; + +import org.timecrafters.Library.Robot; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +public class MotorPortTestRobot extends Robot { + private HardwareMap hardwareMap; + private String string; + private CyberarmEngine engine; + + public MotorEx eh0, eh1, eh2, eh3, ch0, ch1, ch2, ch3; + + public MotorPortTestRobot(String string) { + this.engine = engine; + this.string = string; + } + + @Override + public void setup() { + eh0 = new MotorEx(hardwareMap, "eh0"); + eh1 = new MotorEx(hardwareMap, "eh1"); + eh2 = new MotorEx(hardwareMap, "eh2"); + eh3 = new MotorEx(hardwareMap, "eh3"); + ch0 = new MotorEx(hardwareMap, "ch0"); + ch1 = new MotorEx(hardwareMap, "ch1"); + ch2 = new MotorEx(hardwareMap, "ch2"); + ch3 = new MotorEx(hardwareMap, "ch3"); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java index 00d855d..aac935c 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -139,8 +139,8 @@ public class PrototypeRobot extends Robot { //SERVO depositorShoulder = hardwareMap.servo.get("depositor_shoulder"); depositorElbow = hardwareMap.servo.get("depositor_elbow"); -// collectorShoulder = hardwareMap.servo.get("collector_shoulder"); -// collectorElbow = hardwareMap.servo.get("collector_elbow"); + collectorShoulder = hardwareMap.servo.get("collector_shoulder"); + collectorElbow = hardwareMap.servo.get("collector_elbow"); depositor = hardwareMap.servo.get("depositor"); // input motors exactly as shown below diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/DriveMotorPortTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/DriveMotorPortTestEngine.java new file mode 100644 index 0000000..adab912 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/DriveMotorPortTestEngine.java @@ -0,0 +1,22 @@ +package org.timecrafters.CenterStage.TeleOp.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.timecrafters.CenterStage.Common.MotorPortTestRobot; +import org.timecrafters.CenterStage.Common.PrototypeRobot; +import org.timecrafters.CenterStage.TeleOp.TestingState.MotorPortTestingState; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp (name = "Motor Port Test") +public class DriveMotorPortTestEngine extends CyberarmEngine { + + MotorPortTestRobot robot; + @Override + public void setup() { + robot = new MotorPortTestRobot("Hello World"); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java index 0789b56..6fbedf7 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java @@ -27,17 +27,27 @@ public class ArmPosTest extends CyberarmState { @Override public void exec(){ + robot.collectorElbow.setPosition(armPos); if (engine.gamepad1.a){ - robot.depositorShoulder.setPosition(0); - robot.depositorElbow.setPosition(0); + robot.collectorShoulder.setPosition(0.75); } else if (engine.gamepad1.y){ + robot.collectorShoulder.setPosition(0.65); + }else if (engine.gamepad1.x){ + robot.collectorShoulder.setPosition(0.4); + } - robot.depositorShoulder.setPosition(0.9); - robot.depositorElbow.setPosition(0.22); + if (engine.gamepad2.y && System.currentTimeMillis() - lastMeasuredTime > 500){ + lastMeasuredTime = System.currentTimeMillis(); + armPos += 0.05; + } else if (engine.gamepad2.a && System.currentTimeMillis() - lastMeasuredTime > 500){ + lastMeasuredTime = System.currentTimeMillis(); + armPos -= 0.05; } } + + @Override public void telemetry() { engine.telemetry.addData("arm pos current", armPos); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/TestingState/MotorPortTestingState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/TestingState/MotorPortTestingState.java new file mode 100644 index 0000000..74337b2 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/TestingState/MotorPortTestingState.java @@ -0,0 +1,26 @@ +package org.timecrafters.CenterStage.TeleOp.TestingState; + +import org.timecrafters.CenterStage.Common.MotorPortTestRobot; +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class MotorPortTestingState extends CyberarmState { + + MotorPortTestRobot robot; + private long lastMeasuredTime; + private int CurrentMotorPort; + public MotorPortTestingState(MotorPortTestRobot robot) { + this.robot = robot; + } + + @Override + public void start() { + lastMeasuredTime = System.currentTimeMillis(); + } + + @Override + public void exec() { + + } +}