TimeCrafters/UltimateGoal
1
0
Fork 0
mirror of https://github.com/TimeCrafters/UltimateGoal.git synced 2025-12-13 05:02:34 +00:00
Code Issues Projects Releases Wiki Activity

Full Autonomous First pass

Browse Source
This commit is contained in:
Nathaniel Palme
2021-02-13 21:11:04 -06:00
parent bf9c4c8d86
commit 55602bb617
20 changed files with 361 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java
View File

@@ -72,7 +72,11 @@ public abstract class CyberarmState implements Runnable {
Log.i(TAG, "Adding " + state.getClass() + " to " + this.getClass());
children.add(state);
if (isRunning()) { state.init(); engine.runState(state); }
if (isRunning()) {
state.init();
engine.runState(state);
Log.i(TAG, "Started " + state.getClass() + " in " + this.getClass());
}
return state;
}

1
TeamCode/src/main/java/org/samples/ConceptTensorFlowObjectDetectionWebcam.java
View File

@@ -49,6 +49,7 @@ import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
* IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
* is explained below.
*/
@Disabled
@TeleOp(name = "Concept: TensorFlow Object Detection Webcam", group = "Concept")
public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode {

2
TeamCode/src/main/java/org/samples/ConceptVuMarkIdentificationWebcam.java
View File

@@ -66,7 +66,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
* IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
* is explained in {@link ConceptVuforiaNavigationWebcam}.
*/
@Disabled
@TeleOp(name="Concept: VuMark Id Webcam", group ="Concept")
public class ConceptVuMarkIdentificationWebcam extends LinearOpMode {

2
TeamCode/src/main/java/org/samples/ConceptVuforiaUltimateGoalNavigationWebcam.java
View File

@@ -84,7 +84,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocaliz
* is explained below.
*/
@Disabled
@TeleOp(name="ULTIMATEGOAL Vuforia Nav Webcam", group ="Concept")
public class ConceptVuforiaUltimateGoalNavigationWebcam extends LinearOpMode {

2
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrationEngine.java
View File

@@ -19,6 +19,6 @@ public class CalibrationEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new CalibrateRingBeltLoop(robot));
addState(new OdometryCalibration(robot));
}
}

17
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/OdometryCalibration.java
View File

@@ -20,6 +20,8 @@ public class OdometryCalibration extends CyberarmState {
@Override
public void exec() {
robot.updateLocation();
long timeCurrent = System.currentTimeMillis();
timeChange = timeCurrent - timePrevious;
@@ -32,7 +34,7 @@ public class OdometryCalibration extends CyberarmState {
rotation += robot.getRelativeAngle(imu, rotationPrev);
rotationPrev = imu;
currentTick = (robot.encoderRight.getCurrentPosition() + robot.encoderLeft.getCurrentPosition()) / 2;
currentTick = (robot.encoderRight.getCurrentPosition() - robot.encoderLeft.getCurrentPosition()) / 2;
if (engine.gamepad1.x) {
robot.setDrivePower(power, -power, power, -power);
@@ -55,9 +57,18 @@ public class OdometryCalibration extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("degrees", rotation);
engine.telemetry.addData("ticks", currentTick);
engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
engine.telemetry.addData("Rotation ", robot.getRotation());
//
// engine.telemetry.addData("degrees", rotation);
engine.telemetry.addData("ticks", robot.encoderRight.getCurrentPosition() );
engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise);
engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise);
// engine.telemetry.addData("forward", robot.forwardVector);
// engine.telemetry.addData("sideways", robot.sidewaysVector);
}
private float round(double number,double unit) {
return (float) (Math.floor(number/unit) * unit);
}
}

75
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java
View File

@@ -3,20 +3,54 @@ package org.timecrafters.UltimateGoal.Competition.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.WobbleArm;
import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
import java.util.ArrayList;
@Autonomous (name = "Autonomous")
public class AutoEngine extends CyberarmEngine {
private Robot robot;
private TensorFlowCheck tensorFlowCheck;
private double launchTolerance;
private double launchPower;
private long launchBrakeTime;
private float scoreAFaceAngle;
private double scoreATolerance;
private double scoreAPower;
private long scoreABrakeTime;
float scoreBFaceAngle;
double scoreBTolerance;
double scoreBPower;
long scoreBBrakeTime;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN);
launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();
// scoreAFaceAngle = robot.stateConfiguration.variable("auto","05_0","face").value();
// scoreATolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value());
// scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value();
// scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","brakeMS").value();
//
// scoreBFaceAngle = robot.stateConfiguration.variable("auto","10_0","face").value();
// scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","10_0","tolPos").value());
// scoreBPower = robot.stateConfiguration.variable("auto","10_0","power").value();
// scoreBBrakeTime = robot.stateConfiguration.variable("auto","10_0","brakeMS").value();
super.init();
}
@@ -25,6 +59,9 @@ public class AutoEngine extends CyberarmEngine {
//drive to view
addState(new DriveToCoordinates(robot, "auto", "01_0"));
//face ring
addState(new Face(robot, "auto", "01_1"));
//select scoring area
tensorFlowCheck = new TensorFlowCheck(robot, "auto", "02_0");
addState(tensorFlowCheck);
@@ -33,28 +70,30 @@ public class AutoEngine extends CyberarmEngine {
addState(new DriveToCoordinates(robot, "auto", "03_0"));
//drive to launch position
double launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
double launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
long launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","bakeMS").value();
addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime));
//
addState(new Face(robot, "auto", "04_1"));
//launch rings
addState(new Launch(robot));
addState(new Launch(robot, "auto", "04_2"));
//drive to scoring area
float scoreAFaceAngle = robot.stateConfiguration.variable("auto","05_0","faceAngle").value();
double scoreATolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value());
double scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value();
long scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","bakeMS").value();
addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime));
addState(new DriveToCoordinates(robot, "auto", "05_0", true));
// addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime));
//turn arm towards scoreing area.
addState(new Face(robot, "auto", "05_1"));
ArrayList<CyberarmState> threadStates = new ArrayList<>();
threadStates.add(new Face(robot, "auto", "05_1"));
threadStates.add(new WobbleArm(robot, "auto", "05_2",false));
addState(new ThreadStates(threadStates));
//open the wobble grab arm
addState(new WobbleGrab(robot, "auto", "06_0", true));
//drive to second wobble
addState(new DriveToCoordinates(robot, "auto","06_1"));
addState(new DriveToCoordinates(robot, "auto", "07_0"));
addState(new DriveWithColorSensor(robot, "auto", "08_0"));
@@ -62,16 +101,18 @@ public class AutoEngine extends CyberarmEngine {
addState(new WobbleGrab(robot, "auto", "09_0", false));
//drive to scoring area
float scoreBFaceAngle = robot.stateConfiguration.variable("auto","05_0","faceAngle").value();
double scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value());
double scoreBPower = robot.stateConfiguration.variable("auto","05_0","power").value();
long scoreBBrakeTime = robot.stateConfiguration.variable("auto","05_0","bakeMS").value();
addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime));
// addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime));
addState(new DriveToCoordinates(robot, "auto", "10_0", true));
//release wobble goal 2
addState(new WobbleGrab(robot, "auto", "10_0", true));
addState(new Face(robot, "auto", "11_0"));
addState(new WobbleGrab(robot, "auto", "12_0", true));
//drive to park
addState(new DriveToCoordinates(robot, "auto", "11_0"));
ArrayList<CyberarmState> threadStatesB = new ArrayList<>();
threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false));
threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true));
threadStatesB.add(new DriveToCoordinates(robot, "auto", "13_0"));
addState(new ThreadStates(threadStatesB));
}
}

32
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToCoordinates.java
View File

@@ -17,6 +17,7 @@ public class DriveToCoordinates extends CyberarmState {
private long breakStartTime;
private long brakeTime;
private boolean autoFace;
private boolean scoringArea;
public DriveToCoordinates(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -24,6 +25,13 @@ public class DriveToCoordinates extends CyberarmState {
this.actionName = actionName;
}
public DriveToCoordinates(Robot robot, String groupName, String actionName, boolean scoringArea) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.scoringArea = scoringArea;
}
public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime) {
this.robot = robot;
this.xTarget = xTarget;
@@ -37,8 +45,6 @@ public class DriveToCoordinates extends CyberarmState {
@Override
public void init() {
if (!groupName.equals("manual")) {
xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
@@ -53,9 +59,16 @@ public class DriveToCoordinates extends CyberarmState {
@Override
public void start() {
if (!groupName.equals("manual")) {
setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
if (robot.stateConfiguration.action(groupName,actionName).enabled) {
setHasFinished(true);
if (!scoringArea) {
xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
} else {
xTarget = robot.wobbleScoreX;
yTarget = robot.wobbleScoreY;
}
}
if (autoFace) {
@@ -65,12 +78,14 @@ public class DriveToCoordinates extends CyberarmState {
@Override
public void exec() {
robot.updateLocation();
double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
if (distanceToTarget > tolerancePos) {
double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
braking = false;
} else {
long currentTime = System.currentTimeMillis();
@@ -78,7 +93,10 @@ public class DriveToCoordinates extends CyberarmState {
breakStartTime = currentTime;
braking = true;
}
setHasFinished(currentTime - breakStartTime >= brakeTime);
if (currentTime - breakStartTime >= brakeTime) {
robot.setDrivePower(0,0,0,0);
setHasFinished(true);
}
}
}

5
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java
View File

@@ -26,6 +26,11 @@ public class DriveWithColorSensor extends CyberarmState {
maximum = robot.stateConfiguration.variable(groupName,actionName,"max").value();
}
@Override
public void start() {
setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
}
@Override
public void exec() {
double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);

40
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java
View File

@@ -9,7 +9,7 @@ public class Face extends CyberarmState {
private String groupName = "manual";
private String actionName;
private float faceAngle;
private double toleranceFace;
private float toleranceFace;
private double power;
private boolean braking;
private long breakStartTime;
@@ -22,7 +22,7 @@ public class Face extends CyberarmState {
this.actionName = actionName;
}
public Face(Robot robot, float faceAngle, double toleranceFace, double power, long breakTime) {
public Face(Robot robot, float faceAngle, float toleranceFace, double power, long breakTime) {
this.robot = robot;
this.faceAngle = faceAngle;
this.toleranceFace = toleranceFace;
@@ -32,14 +32,15 @@ public class Face extends CyberarmState {
@Override
public void init() {
if (!groupName.equals("manual")) {
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
breakTime = robot.stateConfiguration.variable(groupName, actionName, "brake MS").value();
breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
try {
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value();
} catch (NullPointerException e) {
} catch (RuntimeException e) {
autoFace = true;
}
}
@@ -47,19 +48,25 @@ public class Face extends CyberarmState {
@Override
public void start() {
if (autoFace) {
double x = robot.stateConfiguration.variable(groupName, actionName, "faceX").value();
double y = robot.stateConfiguration.variable(groupName, actionName, "faceY").value();
faceAngle = robot.getAngleToPosition(x,y);
if (!groupName.equals("manual")) {
setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
if (autoFace) {
double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value());
double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
faceAngle = robot.getAngleToPosition(x, y);
}
}
}
@Override
public void exec() {
robot.updateLocation();
double[] powers = robot.getFacePowers(faceAngle,power);
robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]);
if (Math.abs(robot.getRelativeAngle(faceAngle,robot.getRotation())) > toleranceFace) {
double[] powers = robot.getFacePowers(faceAngle,power);
robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]);
braking = false;
} else {
long currentTime = System.currentTimeMillis();
@@ -67,7 +74,16 @@ public class Face extends CyberarmState {
breakStartTime = currentTime;
braking = true;
}
setHasFinished(currentTime - breakStartTime >= breakTime);
if (currentTime - breakStartTime >= breakTime) {
setHasFinished(true);
robot.setDrivePower(0,0,0,0);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("target angle", faceAngle);
engine.telemetry.addData("current angle", robot.getRotation());
}
}

56
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FaceAndDrive.java Normal file
View File

@@ -0,0 +1,56 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.util.ArrayList;
public class FaceAndDrive extends CyberarmState {
private Robot robot;
private String groupName = "manual";
private String actionName;
private double xTarget;
private double yTarget;
private float faceAngle;
private double tolerancePos;
private double power;
private long brakeTime;
private float toleranceFace;
public FaceAndDrive(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
}
@Override
public void init() {
xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
try {
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
} catch (NullPointerException e) {
faceAngle = robot.getAngleToPosition(xTarget,yTarget);
}
}
@Override
public void start() {
addState(new Face(robot, faceAngle, toleranceFace, power, brakeTime));
addState(new DriveToCoordinates(robot,xTarget,yTarget,faceAngle,tolerancePos,power,brakeTime));
}
@Override
public void exec() {
setHasFinished(true);
}
}

69
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java
View File

@@ -18,6 +18,7 @@ public class TensorFlowCheck extends CyberarmState {
public double wobblePosX;
public double wobblePosY;
private int manualPath;
private String status;
public TensorFlowCheck(Robot robot, String group, String action) {
this.robot = robot;
@@ -33,47 +34,61 @@ public class TensorFlowCheck extends CyberarmState {
@Override
public void start() {
if (!robot.stateConfiguration.action(group,action).enabled) {
manualPath = -1;
} else {
robot.tfObjectDetector.activate();
}
}
@Override
public void exec() {
if (manualPath == -1) {
recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
if (runTime() < checkTime) {
if (manualPath != -1) {
recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
if (recognitions != null) {
if (recognitions != null) {
if (recognitions.size() == 1) {
String label = recognitions.get(0).getLabel();
if (label.equals("Single")) {
path = 1;
} else if (label.equals("Quad")) {
path = 2;
}
} else if (recognitions.size() > 1) {
path = 1;
}
}
if (recognitions.size() == 1) {
Recognition recognition = recognitions.get(0);
String label = recognition.getLabel();
if (label.equals("Single")) {
path = 1;
} else if (label.equals("Quad")) {
path = 2;
}
} else if (recognitions.size() > 1) {
path = 1;
}
}
} else {
path = manualPath;
}
} else {
path = manualPath;
}
} else {
robot.tfObjectDetector.deactivate();
if (runTime() >= checkTime) {
if (path == 0) {
wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos0","x").value();
wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos0","y").value();
robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","x").value());
robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","y").value());
}
if (path == 1) {
wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value();
wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos1","y").value();
robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","x").value());
robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","y").value());
}
if (path == 2) {
wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos2","x").value();
wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos2","y").value();
robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","x").value());
robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","y").value());
}
}
// make the servo look up once we're done using tensorFlow
robot.webCamServo.setPosition(0);
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
setHasFinished(true);
}
}
@Override
public void telemetry() {
engine.telemetry.addData("Chosen Path", path);
}
}

13
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/ThreadStates.java
View File

@@ -6,7 +6,7 @@ import java.util.ArrayList;
public class ThreadStates extends CyberarmState {
private ArrayList<CyberarmState> states = new ArrayList<>();
private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
public ThreadStates(ArrayList<CyberarmState> states) {
this.states = states;
@@ -30,4 +30,15 @@ public class ThreadStates extends CyberarmState {
setHasFinished(finishedStates == states.size());
}
@Override
public void telemetry() {
engine.telemetry.addLine("Threaded States");
for (CyberarmState child: children) {
engine.telemetry.addLine(""+child.getClass());
if (child.getHasFinished()) {
engine.telemetry.addLine("finished");
}
}
}
}

18
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java
View File

@@ -5,6 +5,8 @@ import org.cyberarm.engine.V2.CyberarmState;
public class Launch extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
boolean hasCycled;
boolean detectedPass;
@@ -12,9 +14,23 @@ public class Launch extends CyberarmState {
this.robot = robot;
}
public Launch(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
}
@Override
public void start() {
robot.ringBeltOn();
try {
if (robot.stateConfiguration.action(groupName, actionName).enabled) {
robot.ringBeltOn();
} else {
setHasFinished(true);
}
} catch (NullPointerException e){
robot.ringBeltOn();
}
}
@Override

3
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java
View File

@@ -24,8 +24,11 @@ public class ProgressRingBelt extends CyberarmState {
robot.ringBeltOn();
robot.ringBeltStage += 1;
prepLaunch = !robot.initLauncher;
} else if (robot.ringBeltStage > 2) {
setHasFinished(true);
}
}
@Override

75
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java
View File

@@ -62,7 +62,7 @@ public class Robot {
//Steering Constants
static final double FINE_CORRECTION = 0.055 ;
static final double LARGE_CORRECTION = 0.025;
static final double MOMENTUM_CORRECTION = 1.045;
static final double MOMENTUM_CORRECTION = 1.05;
static final double MOMENTUM_MAX_CORRECTION = 1.4;
static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
@@ -70,15 +70,20 @@ public class Robot {
static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
static final int COUNTS_PER_REVOLUTION = 8192;
static final float mmPerInch = 25.4f;
static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD = 12.3;
static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD = 18.8;
static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4;
static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6;
// Inches Forward of axis of rotation
static final float CAMERA_FORWARD_DISPLACEMENT = 13f;
static final float CAMERA_FORWARD_DISPLACEMENT = 8f;
// Inches above Ground
static final float CAMERA_VERTICAL_DISPLACEMENT = 4.5f;
static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
// Inches Left of axis of rotation
static final float CAMERA_LEFT_DISPLACEMENT = 2f;
static final float CAMERA_LEFT_DISPLACEMENT = 4f;
static final double CAMERA_DISPLACEMENT_MAG = Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
//Robot Localization
public double locationX;
@@ -93,7 +98,7 @@ public class Robot {
//Launcher
public DcMotor launchMotor;
public static final double LAUNCH_POWER = 0.75;
public static final double LAUNCH_POWER = 0.7;
private static final long LAUNCH_ACCEL_TIME = 500;
public double launchPositionX;
@@ -126,13 +131,14 @@ public class Robot {
public static final int WOBBLE_ARM_DOWN = -710;
public static final double WOBBLE_SERVO_MAX = 0.3;
public RevColorSensorV3 wobbleColorSensor;
public double wobbleScoreX;
public double wobbleScoreY;
//vuforia navigation
private WebcamName webcam;
private VuforiaLocalizer vuforia;
public Servo webCamServo;
public static final double CAM_SERVO_UP = 0.2;
public static final double CAM_SERVO_Down = 0.2;
public static final double CAM_SERVO_DOWN = 0.15;
public boolean trackableVisible;
private VuforiaTrackables targetsUltimateGoal;
@@ -151,11 +157,15 @@ public class Robot {
public double totalV;
public double visionX;
public double visionY;
public double visionZ;
public float rawAngle;
// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
private String TestingRecord = "TicksPerDegree";
private String TestingRecord = "x,y";
public double traveledLeft;
public double forwardVector;
public double sidewaysVector;
public double traveledForward = 0;
public double traveledRight;
public void initHardware() {
@@ -192,6 +202,8 @@ public class Robot {
wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
//init ring belt
collectionMotor = hardwareMap.dcMotor.get("collect");
collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -233,8 +245,8 @@ public class Robot {
webCamServo.setDirection(Servo.Direction.REVERSE );
rotation = stateConfiguration.variable("system", "startPos", "direction").value();
locationX = stateConfiguration.variable("system", "startPos", "x").value();
locationY = stateConfiguration.variable("system", "startPos", "y").value();
locationX = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "x").value());
locationY = inchesToTicks((double) stateConfiguration.variable("system", "startPos", "y").value());
minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value();
minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value();
@@ -263,10 +275,11 @@ public class Robot {
}
}
//
launchPositionX = stateConfiguration.variable("system", "launchPos","x").value();
launchPositionX = stateConfiguration.variable("system", "launchPos","y").value();
launchPositionX = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","x").value());
launchPositionY = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","y").value());
launchRotation = stateConfiguration.variable("system", "launchPos","rot").value();
initTensorFlow();
}
private void initVuforia() {
@@ -338,7 +351,7 @@ public class Robot {
double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
int encoderLeftCurrent = -encoderLeft.getCurrentPosition();
int encoderRightCurrent = -encoderRight.getCurrentPosition();
int encoderRightCurrent = encoderRight.getCurrentPosition();
int encoderBackCurrent = encoderBack.getCurrentPosition();
double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
@@ -352,22 +365,27 @@ public class Robot {
//The forward Vector has the luxury of having an odometer on both sides of the robot.
//This allows us to eliminate the unwanted influence of turning the robot by averaging
//the two. This meathod doesn't require any prior calibration.
double forwardVector = (encoderLeftChange+encoderRightChange)/2;
//the two.
//Since there isn't a second wheel to remove the influence of robot rotation, we have to
//instead do this by approximating the number of ticks that were removed due to rotation
//based on a separate calibration program.
double ticksPerDegree;
double ticksPerDegreeForward;
double ticksPerDegreeSideways;
if (rotationChange < 0) {
ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD;
} else {
ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD;
}
double sidewaysVector = encoderBackChange + (rotationChange* ticksPerDegree);
forwardVector = ((encoderLeftChange+encoderRightChange)/2) - (rotationChange* ticksPerDegreeForward);
traveledForward += forwardVector;
sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
@@ -420,13 +438,6 @@ public class Robot {
lastConfirmendLocation = robotLocation;
}
VectorF translation = lastConfirmendLocation.getTranslation();
locationX = -inchesToTicks(translation.get(1) / mmPerInch);
locationY = inchesToTicks( translation.get(0) / mmPerInch);
//For our tournament, it makes sense to make zero degrees towards the goal.
//Orientation is inverted to have clockwise be positive.
Orientation rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
@@ -436,6 +447,16 @@ public class Robot {
this.rotation -= -180;
}
VectorF translation = lastConfirmendLocation.getTranslation();
double camX = -translation.get(1) / mmPerInch;
double camY = translation.get(0) / mmPerInch;
double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(this.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
locationX = inchesToTicks(camX - displaceX);
locationY = inchesToTicks(camY - displaceY);
break;
}
}

1
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpEngine.java
View File

@@ -27,6 +27,7 @@ public class TeleOpEngine extends CyberarmEngine {
@Override
public void stop() {
robot.deactivateVuforia();
robot.saveRecording();
super.stop();
}
}

47
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/TeleOpState.java
View File

@@ -14,9 +14,16 @@ public class TeleOpState extends CyberarmState {
private double leftJoystickMagnitude;
private float rightJoystickDegrees;
private double rightJoystickMagnitude;
private double rightJoystickMagnitudePrevious;
private float faceDirection = 0;
private double faceControlThreshold;
private float cardinalSnapping;
private float pairSnapping;
private double[] powers = {0,0,0,0};
private double drivePower = 1;
private static final double TURN_POWER = 1;
@@ -33,7 +40,6 @@ public class TeleOpState extends CyberarmState {
private boolean wobbleGrabOpen = false;
private boolean launchInput = false;
@@ -45,11 +51,13 @@ public class TeleOpState extends CyberarmState {
public void init() {
cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
}
@Override
public void exec() {
robot.updateLocation();
robot.record(""+robot.ticksToInches(robot.getLocationX())+", "+robot.ticksToInches(robot.getLocationY()));
double leftJoystickX = engine.gamepad1.left_stick_x;
double leftJoystickY = engine.gamepad1.left_stick_y;
@@ -60,9 +68,15 @@ public class TeleOpState extends CyberarmState {
double rightJoystickX = engine.gamepad1.right_stick_x;
double rightJoystickY = engine.gamepad1.right_stick_y;
rightJoystickDegrees = snapToCardinal((float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)),cardinalSnapping,0);
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
//if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
//if the joystick is close to one of the cardinal directions, it is set to exactly the cardinal direction
faceDirection = snapToCardinal(rightJoystickDegrees,cardinalSnapping,0);
}
rightJoystickMagnitudePrevious = rightJoystickMagnitude;
boolean lb = engine.gamepad1.left_stick_button;
if (lb && !lbPrev) {
@@ -103,20 +117,14 @@ public class TeleOpState extends CyberarmState {
//Normal Driver Controls
if (rightJoystickMagnitude == 0) {
if (leftJoystickMagnitude !=0) {
float direction = snapToCardinal(leftJoystickDegrees,cardinalSnapping,0);
powers = robot.getMecanumPowers(direction, drivePower, direction);
}
if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(faceDirection, TURN_POWER);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {
if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {
powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees,pairSnapping,rightJoystickDegrees), drivePower, rightJoystickDegrees);
}
powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees,pairSnapping,faceDirection), drivePower, faceDirection);
}
}
robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
@@ -157,6 +165,10 @@ public class TeleOpState extends CyberarmState {
bPrev = b;
if (engine.gamepad2.right_bumper && engine.gamepad2.left_bumper) {
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
}
}
@Override
@@ -164,13 +176,18 @@ public class TeleOpState extends CyberarmState {
engine.telemetry.addData("childrenHaveFinished", childrenHaveFinished());
for (CyberarmState state : children) {
engine.telemetry.addLine(""+state.getClass());
if (!state.getHasFinished()) {
engine.telemetry.addLine("" + state.getClass());
}
}
engine.telemetry.addData("Vision Z", robot.visionZ);
engine.telemetry.addData("belt stage", robot.ringBeltStage);
engine.telemetry.addLine("Location");
engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
engine.telemetry.addData("Rotation ", robot.getRotation());
engine.telemetry.addData("totalV", robot.totalV);
}
private float round(double number,double unit) {

4
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java
View File

@@ -45,4 +45,8 @@ public class WobbleArm extends CyberarmState {
setHasFinished(runTime() > waitTime);
}
@Override
public void telemetry() {
engine.telemetry.addData("runTime", runTime());
}
}

10
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java
View File

@@ -32,10 +32,14 @@ public class WobbleGrab extends CyberarmState {
@Override
public void start() {
if (open) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
if (robot.stateConfiguration.action(groupName, actionName).enabled) {
if (open) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
} else {
robot.wobbleGrabServo.setPosition(0);
}
} else {
robot.wobbleGrabServo.setPosition(0);
setHasFinished(true);
}
}