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

Adding semi-autonomous functions and transferring Teleop code, variable names are *NOT* inspired by the story of Spirit and Opportunity, the names are a coincidence.

Browse Source
This commit is contained in:
Sodi
2023-01-24 20:37:03 -06:00
parent e287f3be66
commit 8bed3ffefd
3 changed files with 174 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPState.java
View File

@@ -493,5 +493,4 @@ public class PhoenixTeleOPState extends CyberarmState {
// Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5; // Distance = (Distance_1 + Distance_2 + Distance_3 + Distance_4 + Distance_5)/5;
// return Distance; // return Distance;
} }

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

@@ -13,6 +13,15 @@ public class TeleOPArmDriver extends CyberarmState {
private GamepadChecker gamepad2Checker; private GamepadChecker gamepad2Checker;
private int Opportunity, Endeavour; private int Opportunity, Endeavour;
private double drivePower; private double drivePower;
private double MinimalPower = 0.25, topServoOffset = -0.05, lowServoOffset = -0.05;
private double servoCollectLow = 0.40; //Low servos, A button
private double servoCollectHigh = 0.40; //High servos, A button
private double servoLowLow = 0.5; //Low servos, X button
private double servoLowHigh = 0.75; //High servos, X button
private double servoMedLow = 0.5; //Low servos, B button
private double servoMedHigh = 0.9; //High servos, B button
private double servoHighLow = 0.8; //Low servos, Y button
private double servoHighHigh = 0.9; //High servos, Y button
public TeleOPArmDriver(PhoenixBot1 robot) { public TeleOPArmDriver(PhoenixBot1 robot) {
this.robot = robot; this.robot = robot;
@@ -38,23 +47,147 @@ public class TeleOPArmDriver extends CyberarmState {
robot.HighRiserLeft.setPosition(0.45); robot.HighRiserLeft.setPosition(0.45);
robot.HighRiserRight.setPosition(0.45); robot.HighRiserRight.setPosition(0.45);
Opportunity = 0; Opportunity = 0;
Endeavour = 10; Endeavour = 0;
gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); gamepad2Checker = new GamepadChecker(engine, engine.gamepad2);
} }
@Override @Override
public void exec() { public void exec() {
if (robot.collectorDistance.getDistance(DistanceUnit.MM) < 275 && robot.collectorDistance.getDistance(DistanceUnit.MM) > 100) {
Endeavour = 0;
}
if (robot.collectorDistance.getDistance(DistanceUnit.MM) < 550 && robot.collectorDistance.getDistance(DistanceUnit.MM) > 275) { if (engine.gamepad2.y) {
Endeavour = 4;
}
if (engine.gamepad2.b) {
Endeavour = 3;
}
if (engine.gamepad2.x) {
Endeavour = 2;
}
if (engine.gamepad2.a) {
Endeavour = 1; Endeavour = 1;
} }
if (Endeavour == 4) {
if (robot.HighRiserLeft.getPosition() < servoHighHigh - 0.01)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < servoHighLow - 0.01)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.HighRiserLeft.getPosition() >= servoHighHigh &&
robot.LowRiserLeft.getPosition() >= servoHighLow) {
Endeavour = 0;
}
} }
if (Endeavour == 3) {
if (robot.LowRiserLeft.getPosition() > servoMedLow + 0.01)/* <-- 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);
}
}
if (robot.LowRiserLeft.getPosition() < servoMedLow - 0.01)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() <= servoMedLow + 0.01 &&
robot.HighRiserLeft.getPosition() > servoMedHigh + 0.01)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < servoMedLow + 0.01 &&
robot.HighRiserLeft.getPosition() < servoMedHigh - 0.01)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() > servoMedLow - 0.01 &&
robot.LowRiserLeft.getPosition() <= servoMedLow &&
robot.HighRiserLeft.getPosition() > servoMedHigh - 0.01 &&
robot.HighRiserLeft.getPosition() <= servoMedHigh) {
Endeavour = 0;
}
}
} if (Endeavour == 2) {
if (robot.LowRiserLeft.getPosition() > servoLowLow + 0.01)/* <-- 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);
}
}
if (robot.LowRiserLeft.getPosition() < servoLowLow - 0.01)/* <-- low level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 &&
robot.HighRiserLeft.getPosition() > servoLowHigh)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
if (robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
robot.HighRiserLeft.getPosition() < servoLowHigh - 0.01)/* <-- high level too low*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() > servoLowLow - 0.01 &&
robot.LowRiserLeft.getPosition() <= servoLowLow + 0.01 &&
robot.HighRiserLeft.getPosition() > servoLowHigh - 0.01 &&
robot.HighRiserLeft.getPosition() <= servoLowHigh + 0.01) {
Endeavour = 0;
}
}
if (Endeavour == 1) {
if (robot.LowRiserLeft.getPosition() >= servoCollectLow + 0.01)/* <-- 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 &&
robot.HighRiserLeft.getPosition() > servoCollectHigh)/* <-- high level too high*/ {
if (System.currentTimeMillis() - lastStepTime >= 100) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
} else if (robot.LowRiserLeft.getPosition() <= servoCollectLow + 0.01 &&
robot.HighRiserLeft.getPosition() <= servoCollectHigh) {
Endeavour = 0;
}
}
}
}

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

@@ -12,11 +12,13 @@ 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;
private boolean FreeSpirit; private boolean FreeSpirit;
private GamepadChecker gamepad1Checker; private GamepadChecker gamepad1Checker;
public TeleOPTankDriver(PhoenixBot1 robot) { public TeleOPTankDriver(PhoenixBot1 robot) {
this.robot = robot; this.robot = robot;
} }
@@ -46,7 +48,8 @@ public class TeleOPTankDriver extends CyberarmState {
} }
if (FreeSpirit) { if (FreeSpirit) {
drivePower = -engine.gamepad1.left_stick_y; getCurrentDriveCommand();
drivePower = -currentDriveCommand;
robot.backLeftDrive.setPower(drivePower); robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower);
@@ -61,33 +64,50 @@ public class TeleOPTankDriver extends CyberarmState {
robot.frontRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower);
} }
if () 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);
}
} }
public void CalculateDeltaRotation() { public void CalculateDeltaRotation() {
if (RotationTarget >= 0 && RobotRotation >= 0) { if (RotationTarget >= 0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation); DeltaRotation = Math.abs(RotationTarget - RobotRotation);
} } else if (RotationTarget <= 0 && RobotRotation <= 0) {
else if (RotationTarget <= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget - RobotRotation); DeltaRotation = Math.abs(RotationTarget - RobotRotation);
} } else if (RotationTarget >= 0 && RobotRotation <= 0) {
else if (RotationTarget >= 0 && RobotRotation <= 0) {
DeltaRotation = Math.abs(RotationTarget + RobotRotation); DeltaRotation = Math.abs(RotationTarget + RobotRotation);
} } else if (RotationTarget <= 0 && RobotRotation >= 0) {
else if (RotationTarget <=0 && RobotRotation >= 0) {
DeltaRotation = Math.abs(RobotRotation + RotationTarget); DeltaRotation = Math.abs(RobotRotation + RotationTarget);
} }
} }
public double getDeltaOdometerR() { public void getDeltaOdometerR() {
Spirit = robot.OdometerEncoder.getCurrentPosition(); Spirit = robot.OdometerEncoder.getCurrentPosition();
if (System.currentTimeMillis() - lastStepTime >= 1000) { if (System.currentTimeMillis() - lastStepTime >= 1000) {
lastStepTime = System.currentTimeMillis(); lastStepTime = System.currentTimeMillis();
DeltaOdometerR = robot.OdometerEncoder.getCurrentPosition() - Spirit; DeltaOdometerR = robot.OdometerEncoder.getCurrentPosition() - Spirit;
} }
return DeltaOdometerR; }
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
@@ -104,4 +124,4 @@ public class TeleOPTankDriver extends CyberarmState {
} }
} }
} }