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

Added debugging telemetry to CollectorDistanceState that showed that there was most likely not a bug with the distance delta- but that the 2M distance sensor is flakey at long ranges, Fixed mecanum minibot unable to grab

Browse Source
This commit is contained in:
Cyberarm
2022-11-05 16:58:32 -05:00
parent ddba73b6cb
commit f5805567e7
2 changed files with 27 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
View File

@@ -17,6 +17,9 @@ public class CollectorDistanceState extends CyberarmState {
private double lastMeasuredTime;
private double currentDistance;
private double LastDistanceRead;
private double distanceDelta;
private double debugRunTime;
private String debugStatus = "?";
public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
@@ -30,20 +33,30 @@ public class CollectorDistanceState extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addLine();
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
engine.telemetry.addLine();
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addLine();
engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Current Distance", currentDistance);
engine.telemetry.addData("last Distance", LastDistanceRead);
engine.telemetry.addLine();
engine.telemetry.addData("distanceDelta", distanceDelta);
engine.telemetry.addData("DEBUG RunTime", debugRunTime);
engine.telemetry.addData("DEBUG Status", debugStatus);
}
@@ -82,15 +95,19 @@ public class CollectorDistanceState extends CyberarmState {
currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM);
if (LastDistanceRead - currentDistance >= 0.0 || currentDistance > 500) {
distanceDelta = LastDistanceRead - currentDistance;
if (distanceDelta >= 0.0 || currentDistance > 500) {
// I am moving forward
// and im close too my target.
LastDistanceRead = currentDistance;
debugRunTime = runTime();
debugStatus = "RUNNING";
} else {
// I have stopped
targetDrivePower = 0;
// setHasFinished(true);
debugStatus = "STOPPED";
setHasFinished(true);
return;
}
}

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
View File

@@ -71,7 +71,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
}
/* ............................................................................ grab */
if(engine.gamepad1.left_stick_x>0.5 || engine.gamepad1.right_stick_x<-0.5 ||
if(engine.gamepad1.left_stick_x < -0.5 || engine.gamepad1.right_stick_x < -0.5 ||
engine.gamepad2.x){ // in
robot.pServoGrab.setPosition(0.9);
}
@@ -79,7 +79,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
engine.gamepad2.back){ // small out
robot.pServoGrab.setPosition(0.50);
}
if(engine.gamepad1.right_stick_x>0.5 || engine.gamepad1.right_stick_x>0.5 ||
if(engine.gamepad1.left_stick_x > 0.5 || engine.gamepad1.right_stick_x > 0.5 ||
engine.gamepad2.b){ // big out
robot.pServoGrab.setPosition(0.0);
}