From 9532b9e8afe0b380be9b2bff1f15aa52dd268430 Mon Sep 17 00:00:00 2001 From: Spencer Date: Sun, 5 Dec 2021 16:04:35 -0600 Subject: [PATCH] added configuration --- TC_Config/FreightFrenzy.json | 2 +- .../Engines/RedWarehouseEngine.java | 40 ++++++++++-- .../Autonomous/States/CollectorToggle.java | 28 +++++---- .../Autonomous/States/DriveState.java | 15 ++++- .../Autonomous/States/FaceState.java | 15 +++++ .../Autonomous/States/TurretArmExtension.java | 10 ++- .../Autonomous/States/TurretArmRiser.java | 10 ++- .../Autonomous/States/TurretOrbit.java | 22 ++++--- .../Competition/Common/Robot.java | 10 ++- .../TeleOp/States/TeleOpState.java | 61 ++++++++----------- 10 files changed, 141 insertions(+), 72 deletions(-) diff --git a/TC_Config/FreightFrenzy.json b/TC_Config/FreightFrenzy.json index c5d49e6..a7635f1 100644 --- a/TC_Config/FreightFrenzy.json +++ b/TC_Config/FreightFrenzy.json @@ -1 +1 @@ -{"config":{"created_at":"2021-12-02 19:33:56 -0600","updated_at":"2021-12-02 20:11:14 -0600","spec_version":2,"revision":38},"data":{"groups":[{"name":"autonomous","actions":[{"name":"01_0","comment":"turret rotation (;","enabled":true,"variables":[{"name":"power","value":"Dx1"},{"name":"time","value":"Lx3"}]},{"name":"02_0","comment":"Turret Arm Extension","enabled":true,"variables":[{"name":"power","value":"Dx0.75"},{"name":"targetPosition","value":"Ix3000"},{"name":"tolerance","value":"Ix150"}]},{"name":"03_0","comment":"Depositor Door","enabled":true,"variables":[{"name":"TargetPosition","value":"Dx0.5"},{"name":"time","value":"Lx1"}]},{"name":"04_0","comment":"Depositor Door","enabled":true,"variables":[{"name":"TargetPosition","value":"Dx0"},{"name":"time","value":"Lx0"}]},{"name":"05_0","comment":"Turret Arm Extension","enabled":true,"variables":[{"name":"power","value":"Dx1"},{"name":"targetPosition","value":"Ix0"},{"name":"tolerance","value":"Ix150"}]},{"name":"06_0","comment":"turret rotation (;","enabled":true,"variables":[{"name":"power","value":"Dx-1"},{"name":"time","value":"Lx3"}]},{"name":"07_0","comment":"Drive State","enabled":true,"variables":[{"name":"distanceLeft","value":"Ix1500"},{"name":"distanceRight","value":"Ix1500"},{"name":"powerLeft","value":"Dx1"},{"name":"powerRight","value":"Dx1"}]},{"name":"08_0","comment":"Drive State","enabled":true,"variables":[{"name":"distanceLeft","value":"Ix1500"},{"name":"distanceRight","value":"Ix500"},{"name":"powerLeft","value":"Dx1"},{"name":"powerRight","value":"Dx0.75"}]}]}],"presets":{"groups":[],"actions":[{"name":"DepositorDoor","comment":"Depositor Door","enabled":true,"variables":[{"name":"TargetPosition","value":"Dx0.5"},{"name":"time","value":"Lx1"}]},{"name":"DriveState","comment":"Drive State","enabled":true,"variables":[{"name":"distanceLeft","value":"Ix1500"},{"name":"distanceRight","value":"Ix500"},{"name":"powerLeft","value":"Dx1"},{"name":"powerRight","value":"Dx0.75"}]},{"name":"TurretArmExtension","comment":"Turret Arm Extension","enabled":true,"variables":[{"name":"power","value":"Dx0.75"},{"name":"targetPosition","value":"Ix3000"},{"name":"tolerance","value":"Ix150"}]},{"name":"TurretOrbit","comment":"turret rotation (;","enabled":true,"variables":[{"name":"power","value":"Dx1"},{"name":"time","value":"Lx3"}]}]}}} \ No newline at end of file +{"config":{"created_at":"2021-12-02 19:33:56 -0600","updated_at":"2021-12-05 15:55:06 -0600","spec_version":2,"revision":95},"data":{"groups":[{"name":"RedWarehouseAutonomous","actions":[{"name":"01_0","comment":"turret rotation (;","enabled":true,"variables":[{"name":"power","value":"Dx-1"},{"name":"time","value":"Dx3"}]},{"name":"02_0","comment":"Turret Arm Extension","enabled":true,"variables":[{"name":"power","value":"Dx0.75"},{"name":"targetPosition","value":"Ix3000"},{"name":"tolerance","value":"Ix300"}]},{"name":"03_0","comment":"Depositor Door","enabled":true,"variables":[{"name":"power","value":"Ix1"},{"name":"time","value":"Dx1000"}]},{"name":"04_0","comment":"Depositor Door","enabled":true,"variables":[{"name":"power","value":"Ix0"},{"name":"time","value":"Dx0"}]},{"name":"05_0","comment":"Turret Arm Extension","enabled":true,"variables":[{"name":"power","value":"Dx1"},{"name":"targetPosition","value":"Ix-2700"},{"name":"tolerance","value":"Ix150"}]},{"name":"06_0","comment":"turret rotation (;","enabled":true,"variables":[{"name":"power","value":"Dx-1"},{"name":"time","value":"Dx3.0"}]},{"name":"07_0","comment":"Drive State","enabled":true,"variables":[{"name":"distanceLeft","value":"Ix1250"},{"name":"distanceRight","value":"Ix1250"},{"name":"powerLeft","value":"Dx1"},{"name":"powerRight","value":"Dx1"}]},{"name":"08_0","comment":"Drive State","enabled":true,"variables":[{"name":"distanceLeft","value":"Ix1000"},{"name":"distanceRight","value":"Ix1000"},{"name":"powerLeft","value":"Dx0.5"},{"name":"powerRight","value":"Dx-0.5"}]},{"name":"09_0","comment":"Drive State","enabled":true,"variables":[{"name":"distanceLeft","value":"Ix2500"},{"name":"distanceRight","value":"Ix2500"},{"name":"powerLeft","value":"Dx0.75"},{"name":"powerRight","value":"Dx0.75"}]}]}],"presets":{"groups":[],"actions":[{"name":"DepositorDoor","comment":"Depositor Door","enabled":true,"variables":[{"name":"TargetPosition","value":"Dx0.5"},{"name":"time","value":"Dx1"}]},{"name":"DriveState","comment":"Drive State","enabled":true,"variables":[{"name":"distanceLeft","value":"Ix1500"},{"name":"distanceRight","value":"Ix500"},{"name":"powerLeft","value":"Dx1"},{"name":"powerRight","value":"Dx0.75"}]},{"name":"TurretArmExtension","comment":"Turret Arm Extension","enabled":true,"variables":[{"name":"power","value":"Dx0.75"},{"name":"targetPosition","value":"Ix3000"},{"name":"tolerance","value":"Ix150"}]},{"name":"TurretOrbit","comment":"turret rotation (;","enabled":true,"variables":[{"name":"power","value":"Dx1"},{"name":"time","value":"Dx3"}]}]}}} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java index 8229a45..ddb2c62 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java @@ -1,25 +1,53 @@ package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.CollectorToggle; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DepositorDoor; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension; import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit; import org.timecrafters.FreightFrenzy.Competition.Common.Robot; - +@Autonomous (name = "Red WareHouse", group = "red") public class RedWarehouseEngine extends CyberarmEngine { + Robot robot; + @Override public void setup() { - Robot robot = new Robot(this); + this.robot = new Robot(this); - addState(new TurretOrbit(robot, robot.turretServoWhite, "RedWarehouseAutonomous", "01_0")); + addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "RedWarehouseAutonomous", "01_0")); addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "02_0")); - addState(new DepositorDoor(robot, robot.whiteDispenser, "RedWarehouseAutonomous", "03_0")); - addState(new DepositorDoor(robot, robot.whiteDispenser, "RedWarehouseAutonomous", "04_0")); + addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "03_0")); + addState(new CollectorToggle(robot, robot.collectorWhite, "RedWarehouseAutonomous", "04_0")); addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "05_0")); - addState(new TurretOrbit(robot, robot.turretServoWhite, "RedWarehouseAutonomous", "06_0")); addState(new DriveState(robot,"RedWarehouseAutonomous", "07_0")); addState(new DriveState(robot, "RedWarehouseAutonomous", "08_0")); + addState(new DriveState(robot, "RedWarehouseAutonomous", "09_0")); } + + public void loop() { + super.loop(); + + telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition()); + telemetry.addData("White Riser Arm", robot.whiteArmRiser.getCurrentPosition()); + telemetry.addData("white Turret Switch", robot.whiteMag.isPressed()); + telemetry.addData("white Turret orbit Power", robot.turretServoWhite.getPower()); + telemetry.addData("White Door Position", robot.whiteDispenser.getPosition()); + telemetry.addLine(); + + telemetry.addData("Orange Riser Arm", robot.orangeArmRiser.getCurrentPosition()); + telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition()); + telemetry.addData("orange Turret Switch", robot.orangeMag.isPressed()); + telemetry.addData("orange Turret Orbit Power", robot.turretServoOrange.getPower()); + telemetry.addData("Orange Door Position", robot.orangeDispenser.getPosition()); + telemetry.addLine(); + + telemetry.addData("driveWarehouseLeft", robot.driveWarehouseLeft.getCurrentPosition()); + telemetry.addData("driveWarehouseRight", robot.driveWarehouseRight.getCurrentPosition()); + telemetry.addData("driveGoalLeft", robot.driveGoalLeft.getCurrentPosition()); + telemetry.addData("driveGoalRight", robot.driveGoalRight.getCurrentPosition()); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java index a799949..27d2277 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java @@ -4,32 +4,38 @@ import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.Servo; import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; public class CollectorToggle extends CyberarmState { final static public int MODE_REVERSE = -1; final static public int MODE_COLLECT = 1; final static public int MODE_STOPPED = 0; - + double time; CRServo servo; - int mode; + int power; - public CollectorToggle(CRServo servo, int mode) { + public CollectorToggle(Robot robot, CRServo servo, String groupName, String actionName) { this.servo = servo; - this.mode = mode; + this.power = robot.configuration.variable(groupName, actionName, "power").value(); + this.time = robot.configuration.variable(groupName, actionName, "time").value(); + } @Override public void exec() { - if (mode == MODE_REVERSE){ - servo.setPower(-1); - } + servo.setPower(power); - else if (mode == MODE_COLLECT){ - servo.setPower(1); - } + if (runTime() >= time){ - else { servo.setPower(0); + + setHasFinished(true); } } + + @Override + public void telemetry() { + engine.telemetry.addData("runtime", runTime()); + engine.telemetry.addData("time", time); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DriveState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DriveState.java index 1833cf6..d16ddf5 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DriveState.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/DriveState.java @@ -7,7 +7,8 @@ import org.timecrafters.FreightFrenzy.Competition.Common.Robot; public class DriveState extends CyberarmState { Robot robot; - double distanceLeft, distanceRight, powerLeft, powerRight; + int distanceLeft, distanceRight; + double powerLeft, powerRight; public DriveState(Robot robot, String groupName, String actionName) { this.robot = robot; @@ -32,7 +33,8 @@ public class DriveState extends CyberarmState { @Override public void exec() { - if (Math.abs(robot.driveGoalLeft.getCurrentPosition()) >= distanceLeft && Math.abs(robot.driveGoalRight.getCurrentPosition()) >= distanceRight) { + if (Math.abs(robot.driveGoalLeft.getCurrentPosition()) >= distanceLeft && + Math.abs(robot.driveGoalRight.getCurrentPosition()) >= distanceRight) { robot.driveGoalLeft.setPower(0); robot.driveGoalRight.setPower(0); robot.driveWarehouseRight.setPower(0); @@ -45,6 +47,15 @@ public class DriveState extends CyberarmState { robot.driveWarehouseLeft.setPower(powerLeft); } + + } + @Override + public void telemetry() { + engine.telemetry.addData("distance left", distanceLeft); + engine.telemetry.addData("distance right", distanceRight); + engine.telemetry.addData("power left", powerLeft); + engine.telemetry.addData("power Right", powerRight); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java index ce8e29b..5785b3b 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java @@ -1,5 +1,7 @@ package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; +import com.qualcomm.robotcore.hardware.DcMotor; + import org.cyberarm.engine.V2.CyberarmState; import org.timecrafters.FreightFrenzy.Competition.Common.Robot; @@ -13,6 +15,19 @@ public class FaceState extends CyberarmState { this.power = power; } + @Override + public void start() { + robot.driveGoalLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.driveGoalRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.driveWarehouseLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.driveWarehouseRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.driveGoalLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.driveGoalRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.driveWarehouseLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.driveWarehouseRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + @Override public void exec() { robot.updateRobotOrientation(); diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java index 4db76d0..a636e47 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java @@ -20,13 +20,19 @@ public class TurretArmExtension extends CyberarmState { this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value(); } + @Override + public void start() { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + @Override public void exec() { - if (motor.getCurrentPosition() < targetPosition + tolerance){ + if (motor.getCurrentPosition() < targetPosition - tolerance){ motor.setPower(power); } - else if (motor.getCurrentPosition() > targetPosition - tolerance){ + else if (motor.getCurrentPosition() > targetPosition + tolerance){ motor.setPower(-power); } else { diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java index f1f6c6b..e0cd681 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java @@ -20,13 +20,19 @@ public class TurretArmRiser extends CyberarmState { this.tolerance = tolerance; } + @Override + public void start() { + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + @Override public void exec() { - if (motor.getCurrentPosition() < targetPosition + tolerance){ + if (motor.getCurrentPosition() < targetPosition - tolerance){ motor.setPower(power); } - else if (motor.getCurrentPosition() > targetPosition - tolerance){ + else if (motor.getCurrentPosition() > targetPosition + tolerance){ motor.setPower(-power); } else { diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java index 7d31bcc..7f1611d 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java @@ -1,5 +1,6 @@ package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.hardware.CRServo; import org.cyberarm.engine.V2.CyberarmState; @@ -10,24 +11,25 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act public class TurretOrbit extends CyberarmState { private Robot robot; private CRServo servo; - private long time; + private double time; private double power; - - public TurretOrbit(Robot robot, CRServo servo, String groupName, String actionName) { + private RevTouchSensor magnetSwitch; + public TurretOrbit(Robot robot, CRServo servo, RevTouchSensor magnetSwitch, String groupName, String actionName) { this.robot = robot; this.servo = servo; this.time = robot.configuration.variable(groupName, actionName, "time").value(); this. power = robot.configuration.variable(groupName, actionName, "power").value(); + this. magnetSwitch = magnetSwitch; } @Override public void exec() { - if (runTime() < time) { - servo.setPower(power); - } - else { - servo.setPower(0); - setHasFinished(true); - } + if (magnetSwitch.isPressed() || runTime() < time ){ + servo.setPower(0); + setHasFinished(true); + } + else { + servo.setPower(power); + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java index bf05954..3c260be 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java @@ -6,6 +6,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference. import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -69,6 +70,8 @@ public class Robot { // Sensors public BNO055IMU imu; + public RevTouchSensor whiteMag; + public RevTouchSensor orangeMag; // Drivetrain public static final double WHEEL_CIRCUMFERENCE = Math.PI * 8; @@ -90,7 +93,7 @@ public class Robot { public Robot(CyberarmEngine engine) { this.engine = engine; - + initMagnetSwitches(); initConfiguration(); initBlinkin(); initIMU(); @@ -102,6 +105,11 @@ public class Robot { // initTensorflow(); } + private void initMagnetSwitches() { + whiteMag = engine.hardwareMap.get(RevTouchSensor.class, "whiteMag"); + orangeMag = engine.hardwareMap.get(RevTouchSensor.class, "orangeMag"); + } + public double heading() { return orientation.firstAngle; } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java index 31041a2..070fb76 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java @@ -68,22 +68,6 @@ public class TeleOpState extends CyberarmState { } - // if either of these buttons... move the servo - // turretServo1 = orange - /*if (engine.gamepad1.dpad_right || engine.gamepad1.dpad_left) { - - if (engine.gamepad1.dpad_right) { - robot.turretServoOrange.setPower(-1); - } - - if (engine.gamepad1.dpad_left) { - robot.turretServoOrange.setPower(1); - } - } - // if neither of these buttons... power off - else { - robot.turretServoOrange.setPower(0); - }*/ turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRedDriver1); // if dpad verticles pressed arm rises or lowers... @@ -138,22 +122,7 @@ public class TeleOpState extends CyberarmState { robot.whiteDispenser.setPosition(0); } - // if either of these buttons move the servo - // turretServo2 = white -// if (engine.gamepad2.dpad_right || engine.gamepad2.dpad_left) { -// -// if (engine.gamepad2.dpad_right) { -// robot.turretServoWhite.setPower(-1); -// } -// -// if (engine.gamepad2.dpad_left) { -// robot.turretServoWhite.setPower(1); -// } -// } -// // if neither of these buttons power off -// else { -// robot.turretServoWhite.setPower(0); -// } + turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRedDriver2); // if dpad verticles pressed arm rises or lowers if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) { @@ -175,13 +144,31 @@ public class TeleOpState extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition()); - engine.telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition()); - engine.telemetry.addData("arm limit toggle", armLimitToggle); - engine.telemetry.addData("collector toggle orange", orangeCollectorToggle); - engine.telemetry.addData("collector toggle white", whiteCollectorToggle); + engine.telemetry.addData("driver A turret inverted", allianceRedDriver1); + engine.telemetry.addData("driver B turret inverted", allianceRedDriver2); + engine.telemetry.addLine(); + engine.telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition()); + engine.telemetry.addData("White Riser Arm", robot.whiteArmRiser.getCurrentPosition()); + engine.telemetry.addData("white collector toggle ", whiteCollectorToggle); + engine.telemetry.addData("white Turret Switch", robot.whiteMag.isPressed()); + engine.telemetry.addData("white Turret orbit Power", robot.turretServoWhite.getPower()); + engine.telemetry.addData("White Door Position", robot.whiteDispenser.getPosition()); + engine.telemetry.addLine(); + + engine.telemetry.addData("Orange Riser Arm", robot.orangeArmRiser.getCurrentPosition()); + engine.telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition()); + engine.telemetry.addData("orange collector toggle ", orangeCollectorToggle); + engine.telemetry.addData("orange Turret Switch", robot.orangeMag.isPressed()); + engine.telemetry.addData("orange Turret Orbit Power", robot.turretServoOrange.getPower()); + engine.telemetry.addData("Orange Door Position", robot.orangeDispenser.getPosition()); + engine.telemetry.addLine(); + + engine.telemetry.addData("driveWarehouseLeft", robot.driveWarehouseLeft.getCurrentPosition()); + engine.telemetry.addData("driveWarehouseRight", robot.driveWarehouseRight.getCurrentPosition()); + engine.telemetry.addData("driveGoalLeft", robot.driveGoalLeft.getCurrentPosition()); + engine.telemetry.addData("driveGoalRight", robot.driveGoalRight.getCurrentPosition()); } @Override