From b5d77f6b7975b09dd8493c593b5b4452e73919d7 Mon Sep 17 00:00:00 2001 From: Sodi Date: Thu, 17 Nov 2022 20:33:44 -0600 Subject: [PATCH] Compensating for shifted init --- .../testing/states/PhoenixBot1.java | 8 +- .../testing/states/PhoenixTeleOPState.java | 114 +++++++++--------- 2 files changed, 64 insertions(+), 58 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java index fb39155..6c184df 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java @@ -1,6 +1,8 @@ package org.timecrafters.testing.states; +import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor; import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cColorSensor; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; @@ -46,6 +48,8 @@ public class PhoenixBot1 { public TimeCraftersConfiguration configuration; + public AdafruitI2cColorSensor AdafruitEncoder; + public PhoenixBot1(CyberarmEngine engine) { this.engine = engine; @@ -71,6 +75,7 @@ public class PhoenixBot1 { imu.startAccelerationIntegration(new Position(), new Velocity(), 10); configuration = new TimeCraftersConfiguration("Phoenix"); + AdafruitEncoder = engine.hardwareMap.AdafruitI2cColorSensor.get("adafruit"); //motors configuration frontLeftDrive = engine.hardwareMap.dcMotor.get("Front Left"); @@ -155,4 +160,5 @@ public class PhoenixBot1 { tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia); tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS); } - } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java index 02a21c0..f249fc1 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java @@ -172,9 +172,9 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.dpad_left) { RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = 135; + RotationTarget = -45; CalculateDeltaRotation(); - if (RobotRotation > -45 && RobotRotation < 134) {//CCW + if (RobotRotation > -45 && RobotRotation <= 135) {//CCW drivePower = (-1 * DeltaRotation/180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); @@ -188,7 +188,7 @@ public class PhoenixTeleOPState extends CyberarmState { robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } - if (RobotRotation < 136 && RobotRotation > 134) { + if (RobotRotation < -44 && RobotRotation > -46) { drivePower = 0; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); @@ -198,6 +198,60 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad1.x) { + RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RotationTarget = -90; + CalculateDeltaRotation(); + if (RobotRotation < 90 && RobotRotation < -89) {//CCW + drivePower = (-1 * DeltaRotation/180) - MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation > 90 || RobotRotation < -91) {//CW + drivePower = (1 * DeltaRotation/180) + MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation > -91 && RobotRotation < -89) { + drivePower = 0; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + } + + if (engine.gamepad1.dpad_right) { + RobotRotation = robot.imu.getAngularOrientation().firstAngle; + RotationTarget = 45; + CalculateDeltaRotation(); + if (RobotRotation > -135 && RobotRotation < 44) {//CCW + drivePower = (-1 * DeltaRotation/180) - MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation < -135 || RobotRotation < 46) {//CW + drivePower = (1 * DeltaRotation/180) + MinimalPower; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + if (RobotRotation < 46 && RobotRotation > 44) { + drivePower = 0; + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(-drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(-drivePower); + } + } + + if (engine.gamepad1.b) { RobotRotation = robot.imu.getAngularOrientation().firstAngle; RotationTarget = 90; CalculateDeltaRotation(); @@ -224,60 +278,6 @@ public class PhoenixTeleOPState extends CyberarmState { } } - if (engine.gamepad1.dpad_right) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = -135; - CalculateDeltaRotation(); - if (RobotRotation < 45 && RobotRotation > -134) {//CCW - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation > 45 || RobotRotation < -136) {//CW - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < -134 && RobotRotation > -136) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - - if (engine.gamepad1.b) { - RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = -90; - CalculateDeltaRotation(); - if (RobotRotation < 90 && RobotRotation > -89) {//CCW - drivePower = (1 * DeltaRotation/180) + MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation > 90 || RobotRotation < -91) {//CW - drivePower = (-1 * DeltaRotation/180) - MinimalPower; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - if (RobotRotation < -89 && RobotRotation > -91) { - drivePower = 0; - robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); - robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); - } - } - if (engine.gamepad2.dpad_left) { robot.collectorLeft.setPower(-1); robot.collectorRight.setPower(-1);