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

Merge remote-tracking branch 'origin/master'

Browse Source
This commit is contained in:
Sodi
2022-11-08 18:41:52 -06:00
parent 84b2b45fc1 d21441eec5
commit 83a3cbcff8
14 changed files with 667 additions and 111 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";

96
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java Normal file
View File

@@ -0,0 +1,96 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverState;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.testing.states.PrototypeBot1;
@Autonomous (name = "Left Side")
public class LeftSideAutonomousEngine extends CyberarmEngine {
PrototypeBot1 robot;
@Override
public void setup() {
robot = new PrototypeBot1(this);
addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-0"));
//drive to high pole
addState(new DriverState(robot, "LeftSideAutonomous", "01-0"));
//turn towards high pole
addState(new RotationState(robot, "LeftSideAutonomous", "02-0"));
//lift the upper arm
addState(new TopArm(robot, "LeftSideAutonomous", "03-0"));
//lift the lower arm
addState(new BottomArm(robot, "LeftSideAutonomous", "04-0"));
//drive forward to junction
addState(new DriverState(robot, "LeftSideAutonomous", "05-0"));
//lower the bottom arm to get closer
addState(new TopArm(robot, "LeftSideAutonomous", "06-0"));
//make collector release the cone
addState(new CollectorState(robot, "LeftSideAutonomous", "07-0"));
//lift the lower arm to clear the pole
addState(new TopArm(robot, "LeftSideAutonomous", "08-0"));
//Drive Backwards away from junction
addState(new DriverState(robot, "LeftSideAutonomous", "09-0"));
// Rotate to either set up for cones to grab or park somewhere
addState(new RotationState(robot, "LeftSideAutonomous", "10-0"));
// lower the bottom arm so we dont fall over
addState(new BottomArm(robot, "LeftSideAutonomous", "11-0"));
// lower the upper arm so we dont fall over
addState(new TopArm(robot, "LeftSideAutonomous", "12-0"));;
//adjust arm height to cone.
addState(new TopArm(robot, "LeftSideAutonomous", "13-0"));
//drive towards stack of cones while collecting
addState(new CollectorDistanceState(robot, "LeftSideAutonomous", "14-0"));
//drive slightly back
addState(new DriverState(robot, "LeftSideAutonomous", "15-0"));
// face to -90
addState(new RotationState(robot, "LeftSideAutonomous", "15-1"));
//lift arm up
addState(new TopArm(robot, "LeftSideAutonomous", "16-0"));
// drive backwards too position
addState(new DriverState(robot, "LeftSideAutonomous", "17-0"));
// face low junction
addState(new RotationState(robot, "LeftSideAutonomous", "18-0"));
// drive up to junction
addState(new DriverState(robot, "LeftSideAutonomous", "19-0"));
// bring arm down.
addState(new TopArm(robot, "LeftSideAutonomous", "20-0"));
// get rid of cone
addState(new CollectorState(robot, "LeftSideAutonomous", "21-0"));
// lift arm up to clear
addState(new TopArm(robot, "LeftSideAutonomous", "22-0"));
// rotate towards stack of cones
addState(new RotationState(robot, "LeftSideAutonomous", "23-0"));
// Choose Parking Spot
addState(new PathDecision(robot, "LeftSideAutonomous", "24-0"));
// case 1 drive forward
addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-1"));
// case 2 drive forward
addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-2"));
// case 3 drive forward
addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-3"));
// zero out
addState(new RotationState(robot, "LeftSideAutonomous", "25-0"));
// Top Arm Down
addState(new TopArm(robot, "LeftSideAutonomous", "25-1"));
//Drive Backwards
addState(new DriverState(robot, "LeftSideAutonomous", "26-0"));
}
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
}
}

98
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java Normal file
View File

@@ -0,0 +1,98 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
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.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverState;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.testing.states.PrototypeBot1;
@Autonomous (name = "Right Side")
public class RightSideAutonomousEngine extends CyberarmEngine {
PrototypeBot1 robot;
@Override
public void setup() {
robot = new PrototypeBot1(this);
addState(new ConeIdentification(robot, "RightSideAutonomous", "00-0"));
//drive to high pole
addState(new DriverState(robot, "RightSideAutonomous", "01-0"));
//turn towards high pole
addState(new RotationState(robot, "RightSideAutonomous", "02-0"));
//lift the upper arm
addState(new TopArm(robot, "RightSideAutonomous", "03-0"));
//lift the lower arm
addState(new BottomArm(robot, "RightSideAutonomous", "04-0"));
//drive forward
addState(new DriverState(robot, "RightSideAutonomous", "05-0"));
// drive backward
addState(new DriverState(robot, "RightSideAutonomous", "05-1"));
//lower the bottom arm to get closer
addState(new TopArm(robot, "RightSideAutonomous", "06-0"));
//make collector release the cone
addState(new CollectorState(robot, "RightSideAutonomous", "07-0"));
//lift the lower arm to clear the pole
addState(new TopArm(robot, "RightSideAutonomous", "08-0"));
//Drive Backwards
addState(new DriverState(robot, "RightSideAutonomous", "09-0"));
// lower the bottom arm so we dont fall over
addState(new BottomArm(robot, "RightSideAutonomous", "10-0"));
// lower the upper arm so we dont fall over
addState(new TopArm(robot, "RightSideAutonomous", "11-0"));;
// Rotate to either set up for cones to grab or park somewhere
addState(new RotationState(robot, "RightSideAutonomous", "12-0"));
//adjust arm height to cone.
addState(new TopArm(robot, "RightSideAutonomous", "13-0"));
//drive towards stack of cones while collecting
addState(new CollectorDistanceState(robot, "RightSideAutonomous", "14-0"));
//drive slightly back
addState(new DriverState(robot, "RightSideAutonomous", "15-0"));
// face to -90
addState(new RotationState(robot, "RightSideAutonomous", "15-1"));
//lift arm up
addState(new TopArm(robot, "RightSideAutonomous", "16-0"));
// drive backwards too position
addState(new DriverState(robot, "RightSideAutonomous", "17-0"));
// rotate
addState(new RotationState(robot, "RightSideAutonomous", "18-0"));
// drive slightly towards junction
addState(new DriverState(robot, "RightSideAutonomous", "18-1"));
// drive away from junction
addState(new DriverState(robot, "RightSideAutonomous", "18-2"));
// bring arm down.
addState(new TopArm(robot, "RightSideAutonomous", "19-0"));
// get rid of cone
addState(new CollectorState(robot, "RightSideAutonomous", "20-0"));
// lift arm up to clear
addState(new TopArm(robot, "RightSideAutonomous", "21-0"));
// rotate towards stack of cones
addState(new RotationState(robot, "RightSideAutonomous", "22-0"));
// Choose Parking Spot
addState(new PathDecision(robot, "RightSideAutonomous", "23-0"));
// case 1 drive forward
addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-1"));
// case 2 drive forward
addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-2"));
// case 3 drive forward
addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-3"));
// zero out
addState(new RotationState(robot, "RightSideAutonomous", "24-0"));
// Top Arm Down
addState(new TopArm(robot, "RightSideAutonomous", "25-0"));
}
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboard.get("parkPlace"));
}
}

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

@@ -1,66 +0,0 @@
package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
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.DriverState;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.testing.states.PrototypeBot1;
@Autonomous (name = "Autonomous Test")
public class TestAutonomousEngine extends CyberarmEngine {
PrototypeBot1 robot;
@Override
public void setup() {
robot = new PrototypeBot1(this);
//drive to high pole
addState(new DriverState(robot, "TestAutonomous", "01-0"));
//turn towards high pole
addState(new RotationState(robot, "TestAutonomous", "02-0"));
//lift the upper arm
addState(new TopArm(robot, "TestAutonomous", "03-0"));
//lift the lower arm
addState(new BottomArm(robot, "TestAutonomous", "04-0"));
//drive forward
addState(new DriverState(robot, "TestAutonomous", "05-0"));
//lower the bottom arm to get closer
addState(new TopArm(robot, "TestAutonomous", "06-0"));
//make collector release the cone
addState(new CollectorState(robot, "TestAutonomous", "07-0"));
//lift the lower arm to clear the pole
addState(new TopArm(robot, "TestAutonomous", "08-0"));
//Drive Backwards
addState(new DriverState(robot, "TestAutonomous", "09-0"));
// lower the bottom arm so we dont fall over
addState(new BottomArm(robot, "TestAutonomous", "10-0"));
// lower the upper arm so we dont fall over
addState(new TopArm(robot, "TestAutonomous", "11-0"));;
// Rotate to either set up for cones to grab or park somewhere
addState(new RotationState(robot, "TestAutonomous", "12-0"));
//adjust arm height to cone.
addState(new TopArm(robot, "TestAutonomous", "13-0"));
//drive towards stack of cones
addState(new CollectorDistanceState(robot, "", ""));
// DriverState driverState = new DriverState(robot, "TestAutonomous", "14-0");
// addState(driverState);
// //collect next cone
// driverState.addParallelState(new CollectorState(robot, "TestAutonomous", "15-0"));
}
}

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);
}
}
}
}

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

@@ -9,6 +9,56 @@ import org.timecrafters.testing.states.PrototypeBot1;
public class CollectorDistanceState extends CyberarmState {
private final PrototypeBot1 robot;
private int traveledDistance;
private int RampUpDistance;
private int RampDownDistance;
private double drivePower;
private double targetDrivePower;
private double lastMeasuredTime;
private double currentDistance;
private double LastDistanceRead;
private double distanceDelta;
private double debugRunTime;
private String debugStatus = "?";
public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
}
@Override
public void telemetry() {
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
engine.telemetry.addLine();
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addLine();
engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Current Distance", currentDistance);
engine.telemetry.addData("last Distance", LastDistanceRead);
engine.telemetry.addLine();
engine.telemetry.addData("distanceDelta", distanceDelta);
engine.telemetry.addData("DEBUG RunTime", debugRunTime);
engine.telemetry.addData("DEBUG Status", debugStatus);
}
@Override
public void start() {
@@ -26,17 +76,72 @@ 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 (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
robot.frontRightDrive.setPower(0.15);
robot.frontLeftDrive.setPower(0.15);
robot.backRightDrive.setPower(0.15);
robot.backLeftDrive.setPower(0.15);
if (System.currentTimeMillis() - lastMeasuredTime > 150) {
// checking to see if time is greater than 150 milliseconds
lastMeasuredTime = System.currentTimeMillis();
currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM);
distanceDelta = LastDistanceRead - currentDistance;
if (distanceDelta >= 0.0 || currentDistance > 500) {
// I am moving forward
// and im close too my target.
LastDistanceRead = currentDistance;
debugRunTime = runTime();
debugStatus = "Driving Towards Cone";
} else {
// I have stopped
debugStatus = "Nothing Collected";
robot.collectorLeft.setPower(0);
robot.collectorRight.setPower(0);
setHasFinished(true);
return;
}
}
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) {
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
// ramping up
drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15;
} else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.15);
} else {
// middle ground
drivePower = targetDrivePower;
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
@@ -46,12 +151,9 @@ public class CollectorDistanceState extends CyberarmState {
robot.collectorLeft.setPower(0);
setHasFinished(true);
}
}
}
}
public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
}
}

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);
}
}
}

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

@@ -0,0 +1,108 @@
package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.testing.states.PrototypeBot1;
public class DriverParkPlaceState extends CyberarmState {
private final boolean stateDisabled;
PrototypeBot1 robot;
private int RampUpDistance;
private int RampDownDistance;
private String intendedPlacement;
public DriverParkPlaceState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
@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.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);
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
}
String placement = engine.blackboard.get("parkPlace");
if (placement != null) {
if (!placement.equals(intendedPlacement)){
setHasFinished(true);
}
if (placement.equals(intendedPlacement)) {
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) {
// ramping up
drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
} else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
// middle ground
drivePower = targetDrivePower;
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) {
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
} else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
setHasFinished(true);
} // else ending
} // placement equals if statement
// setHasFinished(true);
} // end of placement doesn't equal null
} // end of exec
@Override
public void telemetry() {
engine.telemetry.addData("Position", intendedPlacement);
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
} // end of telemetry
} // end of class

48
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java
View File

@@ -8,14 +8,18 @@ import org.timecrafters.testing.states.PrototypeBot1;
public class DriverState extends CyberarmState {
private final boolean stateDisabled;
PrototypeBot1 robot;
private int RampUpDistance;
private int RampDownDistance;
public DriverState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower;
private double drivePower, targetDrivePower;
private int traveledDistance;
@Override
@@ -38,6 +42,29 @@ public class DriverState extends CyberarmState {
return;
}
double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition());
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){
// ramping up
drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25;
}
else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){
// ramping down
drivePower = ((delta / RampDownDistance) + 0.25);
} else {
// middle ground
drivePower = targetDrivePower;
}
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
@@ -51,5 +78,22 @@ public class DriverState extends CyberarmState {
setHasFinished(true);
}
}
@Override
public void telemetry() {
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
}
}

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

@@ -0,0 +1,29 @@
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");
setHasFinished(true);
}
}

42
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
View File

@@ -8,8 +8,9 @@ public class RotationState extends CyberarmState {
PrototypeBot1 robot;
public RotationState(PrototypeBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value();
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
drivePowerVariable = drivePower;
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
@@ -18,20 +19,41 @@ public class RotationState extends CyberarmState {
private double drivePower;
private float targetRotation;
float RobotRotation;
private double RotationTarget;
private double RotationDirectionMinimum;
private String debugStatus = "?";
private double drivePowerVariable;
@Override
public void exec() {
if (stateDisabled){
setHasFinished(true);
return;
}
} // end of if
RobotRotation = robot.imu.getAngularOrientation().firstAngle;
if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) {
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
drivePowerVariable = 0.3 * drivePower;
debugStatus = "Rotate Slow";
} // end of if
else {
drivePowerVariable = drivePower * 0.75;
debugStatus = "Rotate Fast";
}
if (RobotRotation >= targetRotation + 1){
drivePowerVariable = Math.abs(drivePowerVariable);
} else {
drivePowerVariable = -1 * Math.abs(drivePowerVariable);
}
if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) {
robot.backLeftDrive.setPower(drivePowerVariable);
robot.backRightDrive.setPower(-drivePowerVariable);
robot.frontLeftDrive.setPower(drivePowerVariable);
robot.frontRightDrive.setPower(-drivePowerVariable);
} else
{
robot.backLeftDrive.setPower(0);
@@ -45,9 +67,13 @@ public class RotationState extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("DEBUG Status", debugStatus);
engine.telemetry.addLine();
engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
engine.telemetry.addData("Robot Target Rotation", targetRotation);
engine.telemetry.addData("Drive Power", drivePower);
engine.telemetry.addData("Drive Power", drivePowerVariable);
}
}

4
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java
View File

@@ -71,7 +71,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
}
/* ............................................................................ grab */
if(engine.gamepad1.left_stick_x>0.5 || engine.gamepad1.right_stick_x<-0.5 ||
if(engine.gamepad1.left_stick_x < -0.5 || engine.gamepad1.right_stick_x < -0.5 ||
engine.gamepad2.x){ // in
robot.pServoGrab.setPosition(0.9);
}
@@ -79,7 +79,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState {
engine.gamepad2.back){ // small out
robot.pServoGrab.setPosition(0.50);
}
if(engine.gamepad1.right_stick_x>0.5 || engine.gamepad1.right_stick_x>0.5 ||
if(engine.gamepad1.left_stick_x > 0.5 || engine.gamepad1.right_stick_x > 0.5 ||
engine.gamepad2.b){ // big out
robot.pServoGrab.setPosition(0.0);
}

19
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");
@@ -117,22 +120,26 @@ public class PrototypeBot1 {
LowRiserLeft.setDirection(Servo.Direction.FORWARD);
LowRiserRight.setDirection(Servo.Direction.REVERSE);
LowRiserLeft.setPosition(0.45);
LowRiserRight.setPosition(0.45);
LowRiserLeft.setPosition(0.35);
LowRiserRight.setPosition(0.35);
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);
}

4
TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java
View File

@@ -290,7 +290,7 @@ public class PrototypeTeleOPState extends CyberarmState {
}
if (engine.gamepad2.dpad_up) {
if (robot.HighRiserLeft.getPosition() < 0.9) {
if (robot.HighRiserLeft.getPosition() < 1.0) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);
@@ -310,7 +310,7 @@ public class PrototypeTeleOPState extends CyberarmState {
}
if (engine.gamepad2.y) {
if (robot.HighRiserLeft.getPosition() < 0.84) {
if (robot.HighRiserLeft.getPosition() < 0.9) {
if (System.currentTimeMillis() - lastStepTime >= 150) {
lastStepTime = System.currentTimeMillis();
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);