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

Made a Junction alignment state with the angled sensors for autonomous but must be reviewed and tested on tuesday

Browse Source
This commit is contained in:
SpencerPiha
2022-12-11 19:46:13 -06:00
parent 020bec7f87
commit eb7b940707
1 changed files with 108 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java Normal file
View File

@@ -0,0 +1,108 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
public class JunctionAllignmentState extends CyberarmState {
private final boolean stateDisabled;
PhoenixBot1 robot;
private double TargetSensorDistance;
private final String targetedJunction;
private final double drivePower;
public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
this.targetedJunction = robot.configuration.variable(groupName, actionName, "targetedJunction").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@Override
public void exec() {
if (stateDisabled){
setHasFinished(true);
} else {
double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM);
switch (targetedJunction) {
case "low":
TargetSensorDistance = 90.0;
break;
case "mid":
TargetSensorDistance = 135.0;
break;
case "high":
TargetSensorDistance = 200.0;
break;
}
// the state is finished if the distance sensors are at the correct distance.
if (leftDistance == TargetSensorDistance && rightDistance == TargetSensorDistance) {
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
setHasFinished(true);
}
// the robot is lined up but needs to drive forward till the robot is at the specified distance
else if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
}
// the robot is lined up but needs to drive backward till the robot is at the specified distance
else if (leftDistance < TargetSensorDistance && rightDistance < TargetSensorDistance){
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower);
}
// the robot is going to rotate CCW until a distance is met
else if (leftDistance > TargetSensorDistance && rightDistance < TargetSensorDistance) {
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
}
// the robot is going to rotate CW until a distance is met
else if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) {
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
}
// The right sensor is aligned but the robot must rotate rotate CW with only the left side powered
else if (leftDistance < TargetSensorDistance && rightDistance == TargetSensorDistance) {
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(0);
}
// The right sensor is aligned but the robot must rotate rotate CCW with only the left side powered
else if (leftDistance > TargetSensorDistance && rightDistance == TargetSensorDistance) {
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(0);
}
// The left sensor is aligned but the robot must rotate rotate CW with only the right side powered
else if (leftDistance == TargetSensorDistance && rightDistance < TargetSensorDistance) {
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(-drivePower);
}
// The left sensor is aligned but the robot must rotate CCW with only the right side powered
else if (leftDistance == TargetSensorDistance && rightDistance > TargetSensorDistance) {
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(drivePower);
}
}
}
}