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

Adding run-to on arm heights and telemetry

Browse Source
This commit is contained in:
Sodi
2022-12-08 20:29:41 -06:00
parent 4c6d289a18
commit 9e5292cf9c
2 changed files with 71 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixBot1.java
View File

@@ -37,7 +37,7 @@ public class PhoenixBot1 {
public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo;
private final CyberarmEngine engine;
public Rev2mDistanceSensor collectorDistance;
public Rev2mDistanceSensor collectorDistance, downSensor, leftPoleDistance, rightPoleDistance;
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive;
@@ -60,6 +60,8 @@ public class PhoenixBot1 {
private void setupRobot () {
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
rightPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Right Pole Distance");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();

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

@@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class PhoenixTeleOPState extends CyberarmState {
@@ -46,6 +47,9 @@ public class PhoenixTeleOPState extends CyberarmState {
engine.telemetry.addData("IMU", robot.imu.getAngularOrientation().firstAngle);
engine.telemetry.addData("Drive Power", drivePower);
engine.telemetry.addData("Delta Rotation", DeltaRotation);
engine.telemetry.addData("Collector Height", robot.downSensor.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Left Pole Distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Right Pole Distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
}
@Override
@@ -389,15 +393,31 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (!engine.gamepad2.left_bumper) {
if (robot.HighRiserLeft.getPosition() < 0.73) {
if (robot.HighRiserLeft.getPosition() > 0.77 && robot.LowRiserLeft.getPosition() < 0.47) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
if (robot.HighRiserLeft.getPosition() > 0.77 && robot.LowRiserLeft.getPosition() > 0.47) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(0.73);
robot.HighRiserRight.setPosition(0.73);
robot.HighRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05);
}
}
}
// if (!engine.gamepad2.left_bumper) {
// if (robot.HighRiserLeft.getPosition() < 0.73) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(0.73);
// robot.HighRiserRight.setPosition(0.73);
// }
// }
// }
}
if (engine.gamepad2.right_bumper) {
@@ -416,31 +436,8 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() > 0.8)
if (robot.HighRiserLeft.getPosition() > 0.87 && robot.LowRiserLeft.getPosition() < 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
}
}
if (!engine.gamepad2.right_bumper) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < 0.6 && robot.HighRiserLeft.getPosition() >= 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (robot.HighRiserLeft.getPosition() > 0.87) {
if (robot.LowRiserLeft.getPosition() > 0.8) {
if (robot.HighRiserLeft.getPosition() > 0.87 && robot.LowRiserLeft.getPosition() < 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
@@ -448,9 +445,33 @@ public class PhoenixTeleOPState extends CyberarmState {
}
}
}
// if (!engine.gamepad2.right_bumper) {
// if (robot.HighRiserLeft.getPosition() < 0.85) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
// }
// }
//
// if (robot.LowRiserLeft.getPosition() < 0.6 && robot.HighRiserLeft.getPosition() >= 0.85) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
// }
// }
// if (robot.HighRiserLeft.getPosition() > 0.87) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05);
// }
// }
// }
}
if (engine.gamepad2.right_stick_button) {
if (engine.gamepad2.x) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
@@ -466,23 +487,23 @@ public class PhoenixTeleOPState extends CyberarmState {
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
if (!engine.gamepad2.right_stick_button) {
if (robot.HighRiserLeft.getPosition() < 0.85) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
}
}
if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
}
}
}
// if (!engine.gamepad2.x) {
// if (robot.HighRiserLeft.getPosition() < 0.85) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
// robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05);
// }
// }
//
// if (robot.LowRiserLeft.getPosition() < 0.75 && robot.HighRiserLeft.getPosition() > 0.7) {
// if (System.currentTimeMillis() - lastStepTime >= 150) {
// lastStepTime = System.currentTimeMillis();
// robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05);
// robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05);
// }
// }
// }
}