From 91e3da0fb0aa08159830fd0b23c9f89f48dbf95c Mon Sep 17 00:00:00 2001 From: SpencerPiha <88354753+SpencerPiha@users.noreply.github.com> Date: Sat, 4 Nov 2023 11:59:05 -0500 Subject: [PATCH] got the positions for the depositor. --- .../CenterStage/Common/PrototypeRobot.java | 20 +++--- .../TeleOp/Engines/ArmPosTestEngine.java | 23 +++++++ .../CenterStage/TeleOp/States/ArmPosTest.java | 45 +++++++++++++ .../States/PrototypeRobotDrivetrainState.java | 64 ------------------- 4 files changed, 79 insertions(+), 73 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/ArmPosTestEngine.java create mode 100644 TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java 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 cd28bdd..00d855d 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -40,7 +40,7 @@ public class PrototypeRobot extends Robot { public MotorEx frontLeft, frontRight, backLeft, backRight, lift; public DcMotor odometerR, odometerL, odometerA; public IMU imu; -// public Servo depositorShoulder, depositorElbow, depositor; + public Servo depositorShoulder, depositorElbow, collectorShoulder, collectorElbow, depositor; private HDrive xDrive; private String string; public double xMultiplier = 1; @@ -106,10 +106,10 @@ public class PrototypeRobot extends Robot { imu = engine.hardwareMap.get(IMU.class, "imu"); //MOTORS - frontRight = new MotorEx(hardwareMap, "FrontRight"); - frontLeft = new MotorEx(hardwareMap, "FrontLeft"); - backRight = new MotorEx(hardwareMap, "BackRight"); - backLeft = new MotorEx(hardwareMap, "BackLeft"); + frontRight = new MotorEx(hardwareMap, "frontRight"); + frontLeft = new MotorEx(hardwareMap, "frontLeft"); + backRight = new MotorEx(hardwareMap, "backRight"); + backLeft = new MotorEx(hardwareMap, "backLeft"); lift = new MotorEx(hardwareMap, "Lift"); configuration = new TimeCraftersConfiguration("Blue Crab"); @@ -136,10 +136,12 @@ public class PrototypeRobot extends Robot { imu.initialize(parameters); -// //SERVO -// depositorShoulder = hardwareMap.servo.get("depositor_shoulder"); -// depositorElbow = hardwareMap.servo.get("depositor_elbow"); -// depositor = hardwareMap.servo.get("depositor"); + //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"); + depositor = hardwareMap.servo.get("depositor"); // input motors exactly as shown below xDrive = new HDrive(frontLeft, frontRight, diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/ArmPosTestEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/ArmPosTestEngine.java new file mode 100644 index 0000000..595b96b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/ArmPosTestEngine.java @@ -0,0 +1,23 @@ +package org.timecrafters.CenterStage.TeleOp.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.timecrafters.CenterStage.Common.MiniYTeleOPBot; +import org.timecrafters.CenterStage.Common.PrototypeRobot; +import org.timecrafters.CenterStage.TeleOp.States.ArmPosTest; +import org.timecrafters.CenterStage.TeleOp.States.YellowMinibotTeleOP; + +import dev.cyberarm.engine.V2.CyberarmEngine; + +@TeleOp(name = "arm test prototype bot") + + public class ArmPosTestEngine extends CyberarmEngine { + private PrototypeRobot robot; + @Override + public void setup() { + this.robot = new PrototypeRobot("hello world"); + this.robot.setup(); + + addState(new ArmPosTest(robot)); + } +} 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 new file mode 100644 index 0000000..0789b56 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/ArmPosTest.java @@ -0,0 +1,45 @@ +package org.timecrafters.CenterStage.TeleOp.States; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.timecrafters.CenterStage.Common.PrototypeRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class ArmPosTest extends CyberarmState { + + private float armPos; + private long lastMeasuredTime; + private long waitTime; + PrototypeRobot robot; + public ArmPosTest(PrototypeRobot robot) { + this.robot = robot; + } + + @Override + public void start() { + armPos = 0F; + lastMeasuredTime = System.currentTimeMillis(); + waitTime = 500; + + robot.depositorShoulder.setPosition(0); + robot.depositorElbow.setPosition(0); + } + + @Override + public void exec(){ + + if (engine.gamepad1.a){ + robot.depositorShoulder.setPosition(0); + robot.depositorElbow.setPosition(0); + } else if (engine.gamepad1.y){ + + robot.depositorShoulder.setPosition(0.9); + robot.depositorElbow.setPosition(0.22); + } + } + + @Override + public void telemetry() { + engine.telemetry.addData("arm pos current", armPos); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java index a3478c0..eaeba79 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/PrototypeRobotDrivetrainState.java @@ -15,67 +15,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { public PrototypeRobotDrivetrainState(PrototypeRobot robot) { this.robot = robot; } - private void depositorAutomation(){ - // TODO: 10/7/2023 Workout Logic to move each limb step by step - -// lastCheckedTime = System.currentTimeMillis(); -// -// if (engine.gamepad2.a){ -// // setting Servo Positions to do time Math -// robot.depositorShoulder.setPosition(robot.currentSetPosShoulder); -// // running math function to determine time -// robot.ShoulderServoWaitTime(); -// // determining if the time is met to do the next action -// if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){ -// robot.lastSetPosShoulder = robot.currentSetPosShoulder; -// // setting Servo Positions to do time Math -// robot.currentSetPosElbow = robot.ELBOW_COLLECT; -// robot.depositorElbow.setPosition(robot.currentSetPosElbow); -// robot.lastSetPosElbow = robot.currentSetPosElbow; -// } -// } -// if (engine.gamepad2.y){ -// // setting Servo Positions to do time Math -// robot.currentSetPosShoulder = robot.SHOULDER_DEPOSIT; -// robot.depositorShoulder.setPosition(robot.currentSetPosShoulder); -// // running math function to determine time -// robot.ShoulderServoWaitTime(); -// // determining if the time is met to do the next action -// if (lastCheckedTime - System.currentTimeMillis() >= robot.servoWaitTime){ -// robot.lastSetPosShoulder = robot.currentSetPosShoulder; -// // setting Servo Positions to do time Math -// robot.currentSetPosElbow = robot.ELBOW_COLLECT; -// robot.depositorElbow.setPosition(robot.currentSetPosElbow); -// robot.lastSetPosElbow = robot.currentSetPosElbow; -// -// } -// } -// } - - // --------------------------------------------------------------------------------------------------------- Depositor control function -// private void depositorTeleOp(){ -// // flip arms -// if (engine.gamepad1.x) { -// // shoulder deposit -// robot.depositorShoulder.setPosition(robot.SHOULDER_DEPOSIT); -// // shoulder collect -// } else if (engine.gamepad1.a) { -// robot.depositorShoulder.setPosition(robot.SHOULDER_COLLECT); -// } -// if (engine.gamepad1.y){ -// // elbow deposit -// robot.depositorElbow.setPosition(robot.ELBOW_DEPOSIT); -// // elbow collect -// } else if (engine.gamepad1.b){ -// robot.depositorElbow.setPosition(robot.ELBOW_COLLECT); // Collect / transfer = 0 -// } - -// // depositor -// if (engine.gamepad1.right_bumper) { -// robot.depositor.setPosition(0.8); -// } else if (engine.gamepad1.left_bumper) { -// robot.depositor.setPosition(0.2); - } // } // --------------------------------------------------------------------------------------------------------- Slider control function @@ -107,9 +46,6 @@ public class PrototypeRobotDrivetrainState extends CyberarmState { // drivetrain robot.driveTrainTeleOp(); - // depositor -// depositorTeleOp(); - // lift sliderTeleOp();