TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 14:22:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added and mostly debugged the strafing

Browse Source
This commit is contained in:
Sodi
2023-02-02 20:26:56 -06:00
parent 078a791abc
commit 6a7d9c6de9
2 changed files with 37 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java
View File

@@ -113,14 +113,14 @@ public void exec() {
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); 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*/ { robot.ArmMotor.getCurrentPosition() > armMotorMed)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); 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*/ { robot.ArmMotor.getCurrentPosition() < armMotorMed)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
@@ -150,7 +150,7 @@ public void exec() {
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); 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.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ { robot.ArmMotor.getCurrentPosition() > armMotorLow)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
@@ -158,7 +158,7 @@ public void exec() {
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); 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*/ { robot.ArmMotor.getCurrentPosition() < armMotorLow)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
@@ -166,7 +166,7 @@ public void exec() {
} }
} }
if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 && if (robot.LowRiserLeft.getPosition() > servoLowLow - 5 &&
robot.LowRiserLeft.getPosition() <= servoLowLow + 5 && robot.LowRiserLeft.getPosition() <= servoLowLow &&
robot.ArmMotor.getCurrentPosition() >= armMotorLow) { robot.ArmMotor.getCurrentPosition() >= armMotorLow) {
Endeavour = 0; Endeavour = 0;
} }
@@ -174,19 +174,19 @@ public void exec() {
if (Endeavour == 1) { if (Endeavour == 1) {
robot.ArmMotor.setTargetPosition(armMotorCollect); 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) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.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*/ { robot.ArmMotor.getCurrentPosition() > armMotorCollect)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) { if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize); robot.ArmMotor.setTargetPosition(robot.ArmMotor.getTargetPosition() - ArmMotorStepSize);
} }
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 5 && } else if (robot.LowRiserLeft.getPosition() <= servoCollectLow &&
robot.ArmMotor.getCurrentPosition() <= armMotorCollect) { robot.ArmMotor.getCurrentPosition() <= armMotorCollect) {
Endeavour = 0; Endeavour = 0;
} }
@@ -203,6 +203,7 @@ public void exec() {
if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) { if (engine.gamepad2.dpad_left && Peanut == 1 || engine.gamepad2.dpad_right && Peanut == 2) {
robot.collectorLeft.setPower(0); robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0); robot.collectorRight.setPower(0);
Peanut = 0;
} }
if (Peanut == 1) { if (Peanut == 1) {

48
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java
View File

@@ -12,7 +12,6 @@ public class TeleOPTankDriver extends CyberarmState {
private long lastStepTime = 0; private long lastStepTime = 0;
private double drivePower = 0.3; private double drivePower = 0.3;
private double RobotRotation; private double RobotRotation;
private double currentDriveCommand;
private double RotationTarget, DeltaRotation; private double RotationTarget, DeltaRotation;
private double MinimalPower = 0.2; private double MinimalPower = 0.2;
private int DeltaOdometerR, Endeavour, Spirit; private int DeltaOdometerR, Endeavour, Spirit;
@@ -37,7 +36,16 @@ public class TeleOPTankDriver extends CyberarmState {
@Override @Override
public void exec() { 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; drivePower = engine.gamepad1.left_stick_y;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower);
@@ -45,7 +53,7 @@ public class TeleOPTankDriver extends CyberarmState {
robot.frontRightDrive.setPower(drivePower); 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; drivePower = engine.gamepad1.left_stick_x;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower);
@@ -53,12 +61,27 @@ public class TeleOPTankDriver extends CyberarmState {
robot.frontRightDrive.setPower(drivePower); 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) { if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) {
drivePower = engine.gamepad1.right_stick_x; 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.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower); robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.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 @Override
public void buttonUp(Gamepad gamepad, String button) { public void buttonUp(Gamepad gamepad, String button) {
if (engine.gamepad1 == gamepad && button.equals("start")) { if (engine.gamepad1 == gamepad && button.equals("start")) {