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

Drive forward tolerance added

Browse Source
This commit is contained in:
SpencerPiha
2022-12-17 12:17:00 -06:00
parent 11cf076fc1
commit acce2b6a9e
1 changed files with 10 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -10,12 +10,14 @@ public class DriverStateWithOdometer extends CyberarmState {
PhoenixBot1 robot; PhoenixBot1 robot;
private int RampUpDistance; private int RampUpDistance;
private int RampDownDistance; private int RampDownDistance;
private int maximumTolerance;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) { public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot; this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
} }
@@ -66,11 +68,18 @@ public class DriverStateWithOdometer extends CyberarmState {
drivePower = drivePower * -1; drivePower = drivePower * -1;
} }
if (Math.abs(CurrentPosition) < traveledDistance){ if (Math.abs(CurrentPosition) < traveledDistance - maximumTolerance){
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);
}
else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
robot.backLeftDrive.setPower(targetDrivePower * -0.25);
robot.backRightDrive.setPower(targetDrivePower * -0.25);
robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
robot.frontRightDrive.setPower(targetDrivePower * -0.25);
} else { } else {
robot.backLeftDrive.setPower(0); robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0); robot.backRightDrive.setPower(0);
@@ -90,19 +99,6 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition()); engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
engine.telemetry.addData("drivePower", drivePower); engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower); engine.telemetry.addData("targetDrivePower", targetDrivePower);