From 6a7d9c6de9817f7093b8870c5188ee459c77a9f9 Mon Sep 17 00:00:00 2001 From: Sodi Date: Thu, 2 Feb 2023 20:26:56 -0600 Subject: [PATCH] Added and mostly debugged the strafing --- .../TeleOp/states/TeleOPArmDriver.java | 17 +++---- .../TeleOp/states/TeleOPTankDriver.java | 48 +++++++++++-------- 2 files changed, 37 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java index 534eb87..f46cca8 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java @@ -113,14 +113,14 @@ public void exec() { robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } } - if (robot.LowRiserLeft.getPosition() <= servoMedLow + 5 && + if (robot.LowRiserLeft.getPosition() <= servoMedLow && robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); } } - if (robot.LowRiserLeft.getPosition() < servoMedLow + 5 && + if (robot.LowRiserLeft.getPosition() < servoMedLow && robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); @@ -150,7 +150,7 @@ public void exec() { robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); } } - if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && + if (robot.LowRiserLeft.getPosition() <= servoLowLow && robot.LowRiserLeft.getPosition() > servoLowLow - 5 && robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { @@ -158,7 +158,7 @@ public void exec() { robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); } } - if (robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && + if (robot.LowRiserLeft.getPosition() <= servoLowLow && robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); @@ -166,7 +166,7 @@ public void exec() { } } if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && - robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && + robot.LowRiserLeft.getPosition() <= servoLowLow && robot.ArmMotor.getCurrentPosition() >= armMotorLow) { Endeavour = 0; } @@ -174,19 +174,19 @@ public void exec() { if (Endeavour == 1) { robot.ArmMotor.setTargetPosition(armMotorCollect); - if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 5)/* <-- low level too high*/ { + if (robot.LowRiserLeft.getPosition() >= servoCollectLow)/* <-- low level too high*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); } - } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 && + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ { if (System.currentTimeMillis() - lastStepTime >= 100) { lastStepTime = System.currentTimeMillis(); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); } - } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 && + } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow && robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { Endeavour = 0; } @@ -203,6 +203,7 @@ public void exec() { if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) { robot.collectorLeft.setPower(0); robot.collectorRight.setPower(0); + Peanut = 0; } if (Peanut == 1) { diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java index 250317f..3fbf337 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java @@ -12,7 +12,6 @@ public class TeleOPTankDriver extends CyberarmState { private long lastStepTime = 0; private double drivePower = 0.3; private double RobotRotation; - private double currentDriveCommand; private double RotationTarget, DeltaRotation; private double MinimalPower = 0.2; private int DeltaOdometerR, Endeavour, Spirit; @@ -37,7 +36,16 @@ public class TeleOPTankDriver extends CyberarmState { @Override public void exec() { - if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) { + double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = engine.gamepad1.right_stick_x; + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) { drivePower = engine.gamepad1.left_stick_y; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); @@ -45,7 +53,7 @@ public class TeleOPTankDriver extends CyberarmState { robot.frontRightDrive.setPower(drivePower); } - if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) { + if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) < 0.1) { drivePower = engine.gamepad1.left_stick_x; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); @@ -53,12 +61,27 @@ public class TeleOPTankDriver extends CyberarmState { robot.frontRightDrive.setPower(drivePower); } + if (Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.left_stick_y) > 0.1) { + robot.frontLeftDrive.setPower(frontLeftPower * drivePower); + robot.backLeftDrive.setPower(backLeftPower * drivePower); + robot.frontRightDrive.setPower(frontRightPower * drivePower); + robot.backRightDrive.setPower(backRightPower * drivePower); + } + if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { drivePower = engine.gamepad1.right_stick_x; + robot.backLeftDrive.setPower(-drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(-drivePower); + robot.frontRightDrive.setPower(drivePower); + } + + if (engine.gamepad1.left_stick_y == 0 && engine.gamepad1.left_stick_x == 0 && engine.gamepad1.right_stick_x == 0) { + drivePower = 0; robot.backLeftDrive.setPower(drivePower); - robot.backRightDrive.setPower(-drivePower); + robot.backRightDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); - robot.frontRightDrive.setPower(-drivePower); + robot.frontRightDrive.setPower(drivePower); } @@ -85,21 +108,6 @@ public class TeleOPTankDriver extends CyberarmState { } } - public void getCurrentDriveCommand() { - if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) { - currentDriveCommand = engine.gamepad1.left_stick_y; - } else if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) { - currentDriveCommand = engine.gamepad1.left_stick_x; - } else if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { - currentDriveCommand = engine.gamepad1.right_stick_x; - } else if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) { - currentDriveCommand = engine.gamepad1.right_stick_y; - } else if ((Math.abs(engine.gamepad1.left_stick_y)) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_x) > 0.1 && Math.abs(engine.gamepad1.right_stick_y) > 0.1) { - currentDriveCommand = 0; - } - - } - @Override public void buttonUp(Gamepad gamepad, String button) { if (engine.gamepad1 == gamepad && button.equals("start")) {