mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 20:12:35 +00:00
Merge remote-tracking branch 'origin/master'
This commit is contained in:
Sodi
@@ -8,7 +8,8 @@ import org.timecrafters.Autonomous.States.CollectorDistanceState;
|
||||
import org.timecrafters.Autonomous.States.CollectorState;
|
||||
import org.timecrafters.Autonomous.States.ConeIdentification;
|
||||
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
|
||||
import org.timecrafters.Autonomous.States.JunctionAllignmentState;
|
||||
import org.timecrafters.Autonomous.States.JunctionAllignmentAngleState;
|
||||
import org.timecrafters.Autonomous.States.JunctionAllignmentDistanceState;
|
||||
import org.timecrafters.Autonomous.States.PathDecision;
|
||||
import org.timecrafters.Autonomous.States.RotationState;
|
||||
import org.timecrafters.Autonomous.States.ServoCameraRotate;
|
||||
@@ -47,7 +48,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
|
||||
|
||||
// 6-3 align to junction with rotation or skip if it looks like it won't be able to
|
||||
addState(new JunctionAllignmentState(robot, "RightFourCone", "06-3"));
|
||||
addState(new JunctionAllignmentDistanceState(robot, "RightFourCone", "06-3"));
|
||||
addState(new JunctionAllignmentAngleState(robot, "RightFourCone", "06-4"));
|
||||
|
||||
//pause
|
||||
addState(new BottomArm(robot, "RightFourCone", "06-2"));
|
||||
@@ -55,16 +57,22 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
||||
|
||||
// 7 Drop bottom arm down on the junction to place cone
|
||||
addState(new BottomArm(robot, "RightFourCone", "07-0"));
|
||||
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "07-1"));
|
||||
|
||||
// 7-1 drive back slightly
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-1"));
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "07-2"));
|
||||
|
||||
// 8 Drop cone as soon as arm is in position
|
||||
addState(new CollectorState(robot, "RightFourCone", "08-0"));
|
||||
// 8-1 drive back to lose contact
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "08-1"));
|
||||
// 8-1 realign to old angle
|
||||
addState(new RotationState(robot, "RightFourCone", "08-2"));
|
||||
|
||||
// 9 Raise bottom arm to clear junction
|
||||
addState(new BottomArm(robot, "RightFourCone", "09-0"));
|
||||
//
|
||||
addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "09-0"));
|
||||
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "09-1"));
|
||||
|
||||
// // 10 Back up and bring lower arm down (parallel state)
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "10-0"));
|
||||
addParallelStateToLastState(new BottomArm(robot, "RightFourCone", "10-1"));
|
||||
|
||||
@@ -6,7 +6,7 @@ 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 {
|
||||
public class JunctionAllignmentAngleState extends CyberarmState {
|
||||
private final boolean stateDisabled;
|
||||
PhoenixBot1 robot;
|
||||
private final double drivePower;
|
||||
@@ -25,7 +25,7 @@ public class JunctionAllignmentState extends CyberarmState {
|
||||
private boolean finishedEnabled;
|
||||
|
||||
|
||||
public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
public JunctionAllignmentAngleState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
|
||||
this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
|
||||
@@ -114,6 +114,11 @@ public class JunctionAllignmentState extends CyberarmState {
|
||||
loopsCurrent = loopsCurrent + 1;
|
||||
|
||||
if (rightInRange && leftInRange){
|
||||
|
||||
robot.frontRightDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
setHasFinished(finishedEnabled);
|
||||
}
|
||||
|
||||
@@ -188,30 +193,32 @@ public class JunctionAllignmentState extends CyberarmState {
|
||||
|
||||
}
|
||||
|
||||
if (!rightInRange && !leftInRange && driveLoops < 2){
|
||||
if (!rightInRange && !leftInRange /*&& driveLoops < 2*/){
|
||||
|
||||
if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
// if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
|
||||
// robot.backLeftDrive.setPower(drivePower);
|
||||
// robot.backRightDrive.setPower(drivePower);
|
||||
// robot.frontLeftDrive.setPower(drivePower);
|
||||
// robot.frontRightDrive.setPower(drivePower);
|
||||
//
|
||||
// driveLoops += 1;
|
||||
// }
|
||||
//
|
||||
// } else {
|
||||
//
|
||||
// robot.backLeftDrive.setPower(0);
|
||||
// robot.backRightDrive.setPower(0);
|
||||
// robot.frontLeftDrive.setPower(0);
|
||||
// robot.frontRightDrive.setPower(0);
|
||||
//
|
||||
// robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
// robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
//
|
||||
// if (driveLoops == 2) {
|
||||
// loopsCurrent = loopsTotal;
|
||||
// }
|
||||
|
||||
driveLoops += 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
|
||||
robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
if (driveLoops == 2) {
|
||||
loopsCurrent = loopsTotal;
|
||||
}
|
||||
setHasFinished(true);
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,144 @@
|
||||
package org.timecrafters.Autonomous.States;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||
|
||||
public class JunctionAllignmentDistanceState extends CyberarmState {
|
||||
private final boolean stateDisabled;
|
||||
PhoenixBot1 robot;
|
||||
private final double drivePower;
|
||||
private int loopsTotal;
|
||||
private float slop;
|
||||
private double checkTime = 250;
|
||||
private int traveledDistance;
|
||||
private int loopsCurrent;
|
||||
private double minDistance;
|
||||
private double maxDistance;
|
||||
private String whereAmI = "init";
|
||||
private int driveLoops;
|
||||
private boolean finishedEnabled;
|
||||
double leftDistance;
|
||||
double rightDistance;
|
||||
boolean rightInRange;
|
||||
boolean leftInRange;
|
||||
|
||||
|
||||
public JunctionAllignmentDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
|
||||
this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
|
||||
this.slop = robot.configuration.variable(groupName, actionName, "slop").value();
|
||||
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveled distance").value();
|
||||
this.minDistance = robot.configuration.variable(groupName, actionName, "minDistance").value();
|
||||
this.maxDistance = robot.configuration.variable(groupName, actionName, "maxDistance").value();
|
||||
this.finishedEnabled = robot.configuration.variable(groupName, actionName, "finishedEnabled").value();
|
||||
|
||||
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("right sensor distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
|
||||
engine.telemetry.addData("left sensor distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
|
||||
engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
|
||||
engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
|
||||
engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
|
||||
engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
|
||||
engine.telemetry.addData("drive power", drivePower);
|
||||
engine.telemetry.addData("traveled distance", traveledDistance);
|
||||
engine.telemetry.addData("Loops", loopsCurrent);
|
||||
engine.telemetry.addData("where am i??", whereAmI);
|
||||
engine.telemetry.addData("time", System.currentTimeMillis() - checkTime);
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
|
||||
// robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
// robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
// robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
// robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
// robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
//
|
||||
//
|
||||
// robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
// robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
// robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
// robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
// robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
checkTime = System.currentTimeMillis() - 500;
|
||||
|
||||
|
||||
loopsCurrent = 0;
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
|
||||
if (stateDisabled) {
|
||||
setHasFinished(true);
|
||||
} else {
|
||||
|
||||
if (loopsCurrent >= loopsTotal ) {
|
||||
|
||||
whereAmI = "exceeded loops";
|
||||
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
setHasFinished(finishedEnabled);
|
||||
|
||||
} else if (System.currentTimeMillis() - checkTime >= 100) {
|
||||
|
||||
checkTime = System.currentTimeMillis();
|
||||
|
||||
loopsCurrent += 1;
|
||||
|
||||
leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
|
||||
rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM);
|
||||
rightInRange = (rightDistance >= minDistance) && (rightDistance <= maxDistance);
|
||||
leftInRange = (leftDistance >= minDistance) && (leftDistance <= maxDistance);
|
||||
|
||||
if (rightInRange || leftInRange) {
|
||||
|
||||
whereAmI = "in range";
|
||||
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
setHasFinished(finishedEnabled);
|
||||
|
||||
}
|
||||
|
||||
else if (!rightInRange && !leftInRange) {
|
||||
|
||||
whereAmI = "Out of range";
|
||||
|
||||
// if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user