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

Shenanigans and Navigation

Browse Source
This commit is contained in:
Nathaniel Palme
2020-11-14 12:17:34 -06:00
parent 921660660d
commit 167955166e
8 changed files with 216 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -106,6 +106,8 @@ public abstract class CyberarmEngine extends OpMode {
for (CyberarmState state: cyberarmStates) {
stopState(state);
}
}
/**

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

@@ -145,18 +145,17 @@ public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode {
* Initialize the Vuforia localization engine.
*/
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 = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
parameters.vuforiaLicenseKey = VUFORIA_KEY;
parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");;
parameters.useExtendedTracking = false;
// Instantiate the Vuforia engine
vuforia = ClassFactory.getInstance().createVuforia(parameters);
// Loading trackables is not necessary for the TensorFlow Object Detection engine.
}
@@ -170,5 +169,6 @@ public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode {
tfodParameters.minResultConfidence = 0.8f;
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
}
}

2
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java
View File

@@ -21,7 +21,7 @@ public class TestingEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new LauncherTest());
addState(new WelcomeToJankyTown());
}

30
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/WelcomeToJankyTown.java Normal file
View File

@@ -0,0 +1,30 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
public class WelcomeToJankyTown extends CyberarmState {
DcMotor left;
DcMotor right;
@Override
public void init() {
left = engine.hardwareMap.dcMotor.get("left");
right = engine.hardwareMap.dcMotor.get("right");
}
@Override
public void exec() {
left.setPower(-engine.gamepad1.left_stick_y);
right.setPower(engine.gamepad1.right_stick_y);
}
@Override
public void telemetry() {
engine.telemetry.addLine("Welcome to Janky Town!");
}
}

66
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java Normal file
View File

@@ -0,0 +1,66 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
public class DriveToPosition extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private double targetX;
private double targetY;
private double translation;
private double tolerance;
private double power;
public DriveToPosition(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
}
@Override
public void init() {
// translation = robot.inchesToTicks(4);
targetX = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"x").value());
targetY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"y").value());
tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"tolerance").value());
power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
}
@Override
public void start() {
}
@Override
public void exec() {
robot.updateLocation();
robot.record();
double distanceRemaining = Math.hypot(targetX - robot.getLocationX(), targetY - robot.getLocationY());
//Math.pow(3, distanceRemaining - translation) + 0.3;
// if (power > 0.65) {
// power = 0.65;
// }
if (distanceRemaining < tolerance) {
robot.setDrivePower(0, 0);
setHasFinished(true);
} else {
robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);
}
}
@Override
public void telemetry() {
engine.telemetry.addData("Target Visible", robot.trackableVisible);
engine.telemetry.addData("Odo X", robot.ticksToInches(robot.getLocationX()));
engine.telemetry.addData("Odo Y", robot.ticksToInches(robot.getLocationY()));
engine.telemetry.addData(" Rotation", robot.getRotation());
}
}

9
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java
View File

@@ -21,6 +21,15 @@ public class LocalizerTestingEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new VuforiaNavTesting(robot));
// addState(new DriveToPosition(robot, "DtoP", "d1"));
// addState(new DriveToPosition(robot, "DtoP", "d2"));
}
@Override
public void stop() {
super.stop();
robot.saveRecording();
robot.deactivateVuforia();
}
}

33
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java
View File

@@ -10,28 +10,36 @@ public class VuforiaNavTesting extends CyberarmState {
private float angle;
private double X;
private double Y;
private double targetX;
private double targetY;
private float joystickDegrees;
private double power;
public VuforiaNavTesting(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
targetX = robot.inchesToTicks(24);
targetY = robot.inchesToTicks(24);
}
@Override
public void exec() {
robot.setDrivePower(-0.5 * engine.gamepad1.left_stick_y, -0.5 * engine.gamepad1.right_stick_y);
if (engine.gamepad1.x) {
robot.syncWithVuforia();
}
robot.updateLocation();
robot.record();
double joystickX = engine.gamepad1.right_stick_x;
double joystickY = engine.gamepad1.right_stick_y;
joystickDegrees = (float) Math.toDegrees(Math.atan2(joystickX, -joystickY));
power = 0.3 * Math.hypot(joystickX, joystickY);
robot.driveAtAngle(joystickDegrees, power);
X = robot.getLocationX();
Y = robot.getLocationY();
angle = robot.getRotation();
}
@Override
@@ -40,8 +48,11 @@ public class VuforiaNavTesting extends CyberarmState {
engine.telemetry.addData("Odo X", robot.ticksToInches(X));
engine.telemetry.addData("Odo Y", robot.ticksToInches(Y));
engine.telemetry.addData("Joystick", joystickDegrees);
// engine.telemetry.addData("Vis X", robot.ticksToInches(robot.visionX));
// engine.telemetry.addData("Vis Y", robot.ticksToInches(robot.visionY));
engine.telemetry.addData("Robot Angle", angle);
engine.telemetry.addData("Raw Angle", robot.rawAngle);
engine.telemetry.addData("Angle to Target", robot.getAngleToPosition(targetX,targetY));
}
}

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

@@ -1,6 +1,8 @@
package org.timecrafters.UltimateGoal;
import android.app.Activity;
import android.os.Environment;
import android.util.Log;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.DcMotor;
@@ -19,7 +21,12 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefau
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
import org.timecrafters.TimeCraftersConfigurationTool.backend.Backend;
import org.timecrafters.TimeCraftersConfigurationTool.backend.TAC;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
@@ -57,20 +64,21 @@ public class Robot {
static final float mmPerInch = 25.4f;
// Inches Forward of axis of rotation
static final float CAMERA_FORWARD_DISPLACEMENT = 4.25f;
static final float CAMERA_FORWARD_DISPLACEMENT = 13f;
// Inches above Ground
static final float CAMERA_VERTICAL_DISPLACEMENT = 4.5f;
// Inches Left of axis of rotation
static final float CAMERA_LEFT_DISPLACEMENT = 2f;
//Robot Localization
private double locationX;
private double locationY;
public double locationX;
public double locationY;
private float rotation;
public double visionX;
public double visionY;
public float rawAngle;
private String TestingRecord = "X,Y,Angle";
public double traveledLeft;
@@ -122,6 +130,10 @@ public class Robot {
imu.initialize(parameters);
initVuforia();
rotation = stateConfiguration.variable("system", "startPos", "direction").value();
locationX = stateConfiguration.variable("system", "startPos", "x").value();
locationY = stateConfiguration.variable("system", "startPos", "y").value();
}
private void initVuforia() {
@@ -192,7 +204,7 @@ public class Robot {
public void updateLocation(){
// orientation is inverted to have clockwise be positive.
float imuAngle = -imu.getAngularOrientation().firstAngle;
float rotationChange = imuAngle - rotationPrevious;
double rotationChange = imuAngle - rotationPrevious;
if (rotationChange > 180) {
rotationChange -= 360;
@@ -222,17 +234,22 @@ public class Robot {
locationX += xChange;
locationY += yChange;
//TODO : add separate odometer and vision coordinates.
//TODO : make Odometer Coordinates set to vision coordinates on button push.
if (rotation > 180) {
rotation -= 360;
}
if (rotation < -180) {
rotation += 360;
}
}
public void syncWithVuforia() {
trackableVisible = false;
for (VuforiaTrackable trackable : trackables) {
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
//this is used for debugging purposes.
trackableVisible = true;
@@ -244,8 +261,7 @@ public class Robot {
VectorF translation = lastConfirmendLocation.getTranslation();
locationX = -inchesToTicks(translation.get(1) / mmPerInch);
locationY = inchesToTicks( translation.get(0) / mmPerInch);
// visionX = -translation.get(1) / mmPerInch;
// visionY = translation.get(0) / mmPerInch;
//For our tournament, it makes sense to make zero degrees towards the goal.
@@ -260,14 +276,6 @@ public class Robot {
break;
}
}
if (rotation > 180) {
rotation -= 360;
}
if (rotation < -180) {
rotation += 360;
}
}
public float getRotation() {
@@ -282,6 +290,12 @@ public class Robot {
return locationY;
}
public void setCurrentPosition(float rotation, double x, double y) {
this.rotation = rotation;
locationX = x;
locationY = y;
}
public float getAngleToPosition (double x, double y) {
double differenceX = x- getLocationX();
double differenceY = y- getLocationY();
@@ -313,7 +327,53 @@ public class Robot {
return relative;
}
public void deactivateVuforia() {
public void driveAtAngle(float angle, double power) {
double relativeAngle = getRelativeAngle(angle, getRotation());
//calculate how the power of each motor should be adjusted to make the robot curve
//towards the target angle
//--------------------------------------------------------------------------------------
double turnPowerCorrection = Math.pow(0.03 * relativeAngle, 3) + 0.02 * relativeAngle;
//Adjusts power based on degrees off from target.
double leftPower = power - turnPowerCorrection;
double rightPower = power + turnPowerCorrection;
//--------------------------------------------------------------------------------------
//calculates speed adjuster that slows the motors to be closer to the BasePower while
// maintaining the power ratio nesesary to execute the turn.
double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
setDrivePower(leftPower * powerAdjust, rightPower * powerAdjust);
}
public void record() {
TestingRecord+="\n"+locationX+","+locationY+","+rotation;
}
public void saveRecording() {
writeToFile(Environment.getExternalStorageDirectory().getAbsolutePath()+File.separator+"TimeCrafters_TestingRecord"+File.separator+"RobotTestingRecord.txt", TestingRecord);
}
public boolean writeToFile(String filePath, String content) {
try {
FileWriter writer = new FileWriter(filePath);
writer.write(content);
writer.close();
return true;
} catch (IOException e) {
Log.e("RecordTest", e.getLocalizedMessage());
return false;
}
}
public void deactivateVuforia() {
targetsUltimateGoal.deactivate();
}
}