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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
SpencerPiha
2023-02-02 20:36:08 -06:00
parent 8f9a850087 6a7d9c6de9
commit 7e667b154b
3 changed files with 52 additions and 29 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);
}
}
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) {

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 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")) {

16
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/SignalProcessor.java
View File

@@ -14,7 +14,7 @@ public class SignalProcessor extends CyberarmState {
private final int fallbackPosition;
private final boolean stateDisabled;
private List<Recognition> updatedRecognitions;
private List<Recognition> updatedRecognitions = null;
public SignalProcessor(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -54,7 +54,15 @@ public class SignalProcessor extends CyberarmState {
// the last time that call was made.
List<Recognition> recognitions = robot.getTfod().getUpdatedRecognitions();
boolean secondPass = false;
if (recognitions != null) {
/* Since recognitions will be null unless there has been a change and updatedRecognitions
will be null unless we have had a first pass of recognitions we can safely assume that
we are on our second pass. */
if (updatedRecognitions != null) {
secondPass = true;
}
updatedRecognitions = recognitions;
}
@@ -76,6 +84,12 @@ public class SignalProcessor extends CyberarmState {
}
}
}
/* Attempt to speed up this state by assuming that the second pass of recognitions is
good enough and finish the state */
if (secondPass) {
setHasFinished(true);
}
}
}