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

autnomous work added

Browse Source
This commit is contained in:
SpencerPiha
2022-11-05 16:26:31 -05:00
parent 863ef69690
commit ddba73b6cb
7 changed files with 224 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java
View File

@@ -5,6 +5,8 @@ import android.util.Log;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.Gamepad;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.concurrent.CopyOnWriteArrayList;
/**
@@ -17,7 +19,8 @@ public abstract class CyberarmEngine extends OpMode {
public static CyberarmEngine instance;
//Array To Hold States
final private CopyOnWriteArrayList<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
private int activeStateIndex = 0;
public HashMap<String, String> blackboard = new HashMap<>();
private int activeStateIndex = 0;
private boolean isRunning;
private final static String TAG = "PROGRAM.ENGINE";

26
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java
View File

@@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverState;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.RotationState;
@@ -20,7 +21,7 @@ public class TestAutonomousEngine extends CyberarmEngine {
@Override
public void setup() {
robot = new PrototypeBot1(this);
addState(new ConeIdentification(robot, "TestAutonomous", "00-0"));
//drive to high pole
addState(new DriverState(robot, "TestAutonomous", "01-0"));
//turn towards high pole
@@ -67,15 +68,22 @@ public class TestAutonomousEngine extends CyberarmEngine {
addState(new TopArm(robot, "TestAutonomous", "22-0"));
// get rid of cone
addState(new CollectorState(robot, "TestAutonomous", "23-0"));
// lift arm up to clear
addState(new TopArm(robot, "TestAutonomous", "24-0"));
// drive back
addState(new DriverState(robot, "TestAutonomous", "25-0"));
// bring bottom arm down
addState(new BottomArm(robot, "TestAutonomous", "26-0"));
// bring top arm down
addState(new TopArm(robot, "TestAutonomous", "27-0"));
// rotate towards stack of cones
addState(new RotationState(robot, "TestAutonomous", "28-0"));
}
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
}
}

37
TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java
View File

@@ -11,6 +11,7 @@ public class BottomArm extends CyberarmState {
long time;
long lastStepTime = 0;
boolean up;
boolean directPosition;
public BottomArm(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
@@ -18,6 +19,7 @@ public class BottomArm extends CyberarmState {
this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value();
this.time = robot.configuration.variable(groupName, actionName, "time").value();
this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
this.directPosition = robot.configuration.variable(groupName, actionName, "directPosition").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
@@ -35,22 +37,31 @@ public class BottomArm extends CyberarmState {
return;
}
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();
if (directPosition) {
robot.LowRiserLeft.setPosition(LowerRiserLeftPos);
robot.LowRiserRight.setPosition(LowerRiserLeftPos);
if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance);
} else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
} else {
if (runTime() >= time){
setHasFinished(true);
}
} else {
if (System.currentTimeMillis() - lastStepTime >= time) {
lastStepTime = System.currentTimeMillis();
if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance);
} else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) {
robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance);
robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance);
} else {
setHasFinished(true);
}
}
}
}

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

@@ -14,6 +14,10 @@ public class CollectorDistanceState extends CyberarmState {
private int RampDownDistance;
private double drivePower;
private double targetDrivePower;
private double lastMeasuredTime;
private double currentDistance;
private double LastDistanceRead;
public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
@@ -38,6 +42,8 @@ public class CollectorDistanceState extends CyberarmState {
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
engine.telemetry.addData("Current Distance", currentDistance);
engine.telemetry.addData("last Distance", LastDistanceRead);
}
@@ -57,12 +63,37 @@ public class CollectorDistanceState extends CyberarmState {
robot.collectorLeft.setPower(1);
robot.collectorRight.setPower(1);
lastMeasuredTime = System.currentTimeMillis();
LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM);
}
@Override
public void exec() {
if (System.currentTimeMillis() - lastMeasuredTime > 150) {
// checking to see if time is greater than 150 milliseconds
lastMeasuredTime = System.currentTimeMillis();
currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM);
if (LastDistanceRead - currentDistance >= 0.0 || currentDistance > 500) {
// I am moving forward
// and im close too my target.
LastDistanceRead = currentDistance;
} else {
// I have stopped
targetDrivePower = 0;
// setHasFinished(true);
}
}
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());

98
TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java Normal file
View File

@@ -0,0 +1,98 @@
package org.timecrafters.Autonomous.States;
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.timecrafters.testing.states.PrototypeBot1;
import java.util.List;
public class ConeIdentification extends CyberarmState {
PrototypeBot1 robot;
private int time;
private float minimumConfidence;
private int ParkPlace;
public ConeIdentification(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
minimumConfidence = robot.configuration.variable(groupName, actionName, "Minimum Confidence").value();
time = robot.configuration.variable(groupName, actionName, "time").value();
}
@Override
public void init() {
engine.blackboard.put("parkPlace", "1");
robot.tfod.activate();
}
@Override
public void telemetry() {
if (robot.tfod != null) {
// getUpdatedRecognitions() will return null if no new information is available since
// the last time that call was made.
List<Recognition> updatedRecognitions = robot.tfod.getUpdatedRecognitions();
if (updatedRecognitions != null) {
engine.telemetry.addData("# Objects Detected", updatedRecognitions.size());
// step through the list of recognitions and display image position/size information for each one
// Note: "Image number" refers to the randomized image orientation/number
for (Recognition recognition : updatedRecognitions) {
double col = (recognition.getLeft() + recognition.getRight()) / 2 ;
double row = (recognition.getTop() + recognition.getBottom()) / 2 ;
double width = Math.abs(recognition.getRight() - recognition.getLeft()) ;
double height = Math.abs(recognition.getTop() - recognition.getBottom()) ;
engine.telemetry.addData("Label", recognition.getLabel());
engine.telemetry.addData(""," ");
engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 );
engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col);
engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height);
if (recognition.getLabel().equals("2 Bulb")) {
engine.telemetry.addData("2 Bulb", engine.blackboard.put("parkPlace", "2"));
} else if (recognition.getLabel().equals("3 Panel")) {
engine.telemetry.addData("3 Panel",engine.blackboard.put("parkPlace", "3"));
} else {
engine.telemetry.addData("1 Bolt", engine.blackboard.put("parkPlace", "1"));
}
}
}
}
}
@Override
public void exec() {
if (robot.tfod != null) {
// getUpdatedRecognitions() will return null if no new information is available since
// the last time that call was made.
List<Recognition> updatedRecognitions = robot.tfod.getUpdatedRecognitions();
if (updatedRecognitions != null) {
// step through the list of recognitions and display image position/size information for each one
// Note: "Image number" refers to the randomized image orientation/number
float bestConfidence = 0;
for (Recognition recognition : updatedRecognitions) {
if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) {
bestConfidence = recognition.getConfidence();
if (recognition.getLabel().equals("2 Bulb")) {
engine.blackboard.put("parkPlace", "2");
} else if (recognition.getLabel().equals("3 Panel")) {
engine.blackboard.put("parkPlace", "3");
} else {
engine.blackboard.put("parkPlace", "1");
}
}
}
}
}
if (runTime() >= time){
setHasFinished(true);
}
}
}

39
TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java Normal file
View File

@@ -0,0 +1,39 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class PathDecision extends CyberarmState {
PrototypeBot1 robot;
private String groupName;
public PathDecision (PrototypeBot1 robot, String groupName, String actionName){
this.robot = robot;
this.groupName = groupName;
}
@Override
public void exec() {
String placement = engine.blackboard.get("parkPlace");
if (placement != null) {
if (placement.equals("2")) {
engine.insertState(this, new DriverState(robot, groupName, "29-1"));
} else if (placement.equals("3")) {
engine.insertState(this, new DriverState(robot, groupName, "29-2"));
}
}
else {
engine.insertState(this, new DriverState(robot, groupName, "29-0"));
}
}
}

15
TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java
View File

@@ -29,11 +29,11 @@ public class PrototypeBot1 {
private static final String VUFORIA_KEY =
"Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
private VuforiaLocalizer vuforia;
public VuforiaLocalizer vuforia;
private TFObjectDetector tfod;
public TFObjectDetector tfod;
public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight;
public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo;
private final CyberarmEngine engine;
public Rev2mDistanceSensor collectorDistance;
@@ -80,6 +80,9 @@ public class PrototypeBot1 {
// servo configuration
//Camera Servo
CameraServo = engine.hardwareMap.servo.get("Camera Servo");
// Collector
collectorLeft = engine.hardwareMap.crservo.get("Collector Left");
collectorRight = engine.hardwareMap.crservo.get("Collector Right");
@@ -122,17 +125,21 @@ public class PrototypeBot1 {
HighRiserLeft.setPosition(0.45);
HighRiserRight.setPosition(0.45);
CameraServo.setPosition(0);
}
private void initVuforia(){
/*
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
*/
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
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);
}