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

autonomous work

Browse Source
This commit is contained in:
SpencerPiha
2023-01-10 20:34:23 -06:00
parent 0df3e27392
commit 808c371296
3 changed files with 111 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java
View File

@@ -34,8 +34,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0"));
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
// addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
addState(new TopArm(robot, "RightFourCone", "04-1"));
// 6 Raise lower arm while slowly driving at the junction (parallel state)
addState(new BottomArm(robot, "RightFourCone", "05-0"));
@@ -96,23 +96,23 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
//
// // 16 Rotate and use sensor to find junction
// addState(new RotationState(robot, "RightFourCone", "16-0"));
//
// // 17 Drive Towards Junction (This is optional, idk if this is needed atm)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
// 16 Rotate and use sensor to find junction
addState(new RotationState(robot, "RightFourCone", "16-0"));
//
// 17 Drive Towards Junction (This is optional, idk if this is needed atm)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
// // 18 Bring upper arm down
// addState(new TopArm(robot, "RightFourCone", "18-0"));
addState(new TopArm(robot, "RightFourCone", "18-0"));
//
// // 19 Drop cone
// addState(new CollectorState(robot, "RightFourCone", "19-0"));
addState(new CollectorState(robot, "RightFourCone", "19-0"));
//
// // 20 Bring upper arm up
// addState(new TopArm(robot, "RightFourCone", "20-0"));
//
// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
//
// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
// addState(new TopArm(robot, "RightFourCone", "22-0"));
@@ -146,7 +146,7 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
//
// // 32 Rotate towards Stack of cones
// addState(new RotationState(robot, "RightFourCone", "32-0"));
addState(new RotationState(robot, "RightFourCone", "32-0"));
//
// // 33 Decide which path after scanning image from earlier
// addState(new PathDecision(robot, "RightFourCone", "33-0"));

83
TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java
View File

@@ -1,5 +1,7 @@
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;
@@ -14,7 +16,13 @@ public class JunctionAllignmentState extends CyberarmState {
private float targetAngle;
private float slop;
private double checkTime = 250;
private int traveledDistance;
private int loopsCurrent = 0;
private double minDistance;
private double maxDistance;
private String whereAmI = "init";
private int driveLoops;
private boolean finishedEnabled;
public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -23,6 +31,11 @@ public class JunctionAllignmentState extends CyberarmState {
this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
this.rotationAmount = robot.configuration.variable(groupName, actionName, "rotationAmount").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;
}
@@ -38,6 +51,11 @@ public class JunctionAllignmentState extends CyberarmState {
engine.telemetry.addData("target angle", targetAngle);
engine.telemetry.addData("current angle", currentAngle);
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", runTime() - checkTime);
@@ -48,6 +66,23 @@ public class JunctionAllignmentState extends CyberarmState {
@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() - 250;
driveLoops = 0;
}
@Override
@@ -62,24 +97,24 @@ public class JunctionAllignmentState extends CyberarmState {
double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM);
boolean rightInRange = rightDistance > 170 && rightDistance < 200;
boolean leftInRange = leftDistance > 170 && leftDistance < 200;
boolean rightInRange = rightDistance > minDistance && rightDistance < maxDistance;
boolean leftInRange = leftDistance > minDistance && leftDistance < maxDistance;
// The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0
if (loopsTotal >= 5){
setHasFinished(true);
} else if (runTime() - checkTime >= 250 ) {
if (loopsCurrent >= loopsTotal){
setHasFinished(finishedEnabled);
} else if (System.currentTimeMillis() - checkTime >= 250 ) {
whereAmI = "post 250 ms";
checkTime = runTime();
loopsTotal = loopsTotal + 1;
} else {
loopsCurrent = loopsCurrent + 1;
if (rightInRange && leftInRange){
setHasFinished(true);
setHasFinished(finishedEnabled);
}
if (rightInRange && !leftInRange) {
@@ -95,6 +130,8 @@ public class JunctionAllignmentState extends CyberarmState {
robot.backLeftDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
setHasFinished(finishedEnabled);
} else {
if (targetAngle - slop < currentAngle) {
@@ -151,9 +188,31 @@ public class JunctionAllignmentState extends CyberarmState {
}
if (!rightInRange && !leftInRange){
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);
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);
}
}

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

@@ -73,8 +73,36 @@ public class PhoenixBot1 {
setupRobot();
}
private void initVuforia(){
/*
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
*/
int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
parameters.vuforiaLicenseKey = VUFORIA_KEY;
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
// Instantiate the Vuforia engine
vuforia = ClassFactory.getInstance().createVuforia(parameters);
}
private void initTfod() {
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
tfodParameters.minResultConfidence = 0.75f;
tfodParameters.isModelTensorFlow2 = true;
tfodParameters.inputSize = 300;
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
}
private void setupRobot () {
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
@@ -170,34 +198,10 @@ public class PhoenixBot1 {
HighRiserLeft.setPosition(0.45);
HighRiserRight.setPosition(0.45);
CameraServo.setPosition(0.8);
CameraServo.setPosition(0.775);
}
private void initVuforia(){
/*
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
*/
int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
parameters.vuforiaLicenseKey = VUFORIA_KEY;
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
// Instantiate the Vuforia engine
vuforia = ClassFactory.getInstance().createVuforia(parameters);
}
private void initTfod() {
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
tfodParameters.minResultConfidence = 0.75f;
tfodParameters.isModelTensorFlow2 = true;
tfodParameters.inputSize = 300;
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
}
}