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

So many things

Browse Source
This commit is contained in:
Nathaniel Palme
2021-01-16 11:46:05 -06:00
parent 9df096031e
commit 91d443d84f
26 changed files with 187 additions and 396 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,4 +0,0 @@
package org.timecrafters.UltimateGoal;
public class AutoEngine {
}

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

@@ -1,11 +1,9 @@
package org.timecrafters.UltimateGoal.Calibration;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.HardwareTesting.MecanumFunctionTest;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
@TeleOp (name = "Calibration", group = "test")
public class CalibrationEngine extends CyberarmEngine {

3
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MechanumBiasCalibrator.java
View File

@@ -1,9 +1,8 @@
package org.timecrafters.UltimateGoal.Calibration;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.lang.reflect.Array;
import java.util.ArrayList;
public class MechanumBiasCalibrator extends CyberarmState {

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

@@ -1,7 +1,7 @@
package org.timecrafters.UltimateGoal.Calibration;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class OdometryCalibration extends CyberarmState {

4
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/PerformanceTest.java
View File

@@ -1,9 +1,7 @@
package org.timecrafters.UltimateGoal.Calibration;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.util.ArrayList;

4
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/StalPowerCalibrator.java
View File

@@ -3,9 +3,7 @@ package org.timecrafters.UltimateGoal.Calibration;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
import java.util.ArrayList;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class StalPowerCalibrator extends CyberarmState {

7
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/VelocityTest.java
View File

@@ -1,11 +1,8 @@
package org.timecrafters.UltimateGoal.Calibration;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.DriveMotor;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.DriveMotor;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class VelocityTest extends CyberarmState {

4
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/AutoEngine.java Normal file
View File

@@ -0,0 +1,4 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
public class AutoEngine {
}

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

@@ -1,7 +1,7 @@
package org.timecrafters.UltimateGoal.Autonomous;
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class DriveToCoordinates extends CyberarmState {

6
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/DriveToPosition.java → TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class DriveToPosition extends CyberarmState {
@@ -51,7 +51,7 @@ public class DriveToPosition extends CyberarmState {
robot.setDrivePower(0, 0, 0, 0);
setHasFinished(true);
} else {
robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);
// robot.driveAtAngle(robot.getAngleToPosition(targetX, targetY), power);
}
}

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

@@ -1,4 +1,4 @@
package org.timecrafters.UltimateGoal.Autonomous;
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;

2
TeamCode/src/main/java/org/timecrafters/UltimateGoal/DriveMotor.java → TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/DriveMotor.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.UltimateGoal;
package org.timecrafters.UltimateGoal.Competition;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

4
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java Normal file
View File

@@ -0,0 +1,4 @@
package org.timecrafters.UltimateGoal.Competition;
public class Launch {
}

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

@@ -1,4 +1,4 @@
package org.timecrafters.UltimateGoal;
package org.timecrafters.UltimateGoal.Competition;
import android.os.Environment;
import android.util.Log;
@@ -6,6 +6,7 @@ import android.util.Log;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
@@ -59,8 +60,6 @@ public class Robot {
public DcMotor encoderRight;
public DcMotor encoderBack;
public DcMotor launcher;
//Steering Constants
static final double FINE_CORRECTION = 0.05 ;
static final double LARGE_CORRECTION = 0.002;
@@ -97,6 +96,7 @@ public class Robot {
//Launcher
public DcMotor launchMotor;
private static final long LAUNCH_ACCEL_TIME = 3000;
public static final double LAUNCH_POSITION_X = 28 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
public static final double LAUNCH_POSITION_Y = 0 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
public static final float LAUNCH_ROTATION = 0;
@@ -104,10 +104,18 @@ public class Robot {
public static final double LAUNCH_TOLERANCE_FACE = 0.5;
//Ring Belt
public DcMotor collectionMotor;
public DcMotor ringBeltMotor;
private DigitalChannel limitSwitch;
//todo: tune these when they exist
public static final int RING_BELT_LOOP_TICKS = 1000;
public static final int RING_BELT_POS_1 = 200;
public static final int RING_BELT_POS_2 = 400;
public static final int RING_BELT_POS_3 = 600;
public static final int RING_BELT_GAP = 200;
public static final int RING_BELT_RECEIVE_POS = 100;
public static final double RING_DETECT_DISTANCE = 100;
public static final double RING_DETECT_DELAY = 1000;
//Debugging
public double visionX;
@@ -139,6 +147,7 @@ public class Robot {
// webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
imu = hardwareMap.get(BNO055IMU.class, "imu");
// limitSwitch.setMode(DigitalChannel.Mode.INPUT);
DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft");
@@ -157,12 +166,14 @@ public class Robot {
driveBackLeft.init();
driveBackRight.init();
//todo add these when they exist
encoderLeft = hardwareMap.dcMotor.get("odoLeft");
// encoderLeft = hardwareMap.dcMotor.get("odoLeft");
// encoderRight = hardwareMap.dcMotor.get("odoRight");
// encoderBack = hardwareMap.dcMotor.get("odoBack");
//
// launcher = hardwareMap.dcMotor.get("launcher");
launchMotor = hardwareMap.dcMotor.get("launcher");
collectionMotor = hardwareMap.dcMotor.get("collect");
collectionMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
ringBeltMotor = hardwareMap.dcMotor.get("belt");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
@@ -190,6 +201,14 @@ public class Robot {
rotation = stateConfiguration.variable("system", "startPos", "direction").value();
locationX = stateConfiguration.variable("system", "startPos", "x").value();
locationY = stateConfiguration.variable("system", "startPos", "y").value();
// double launcherPower = 0;
// long launchAccelStart = System.currentTimeMillis();
// while (launcherPower < 1) {
// launcherPower = (double) (System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME;
// launchMotor.setPower(launcherPower);
// }
}
private void initVuforia() {
@@ -549,34 +568,17 @@ public class Robot {
return powers;
}
//This function should not be used
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.abs(power) * (Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * 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 int getBeltPos(){
return loopPos(ringBeltMotor.getCurrentPosition());
}
//Data Recording
// public void record(double frontLeft, double frontRight, double backLeft, double backRight) {
// TestingRecord+="\n"+frontLeft+","+frontRight+","+backLeft+","+backRight;
// }
public int loopPos(int pos) {
if (pos < 0) {
pos += RING_BELT_LOOP_TICKS;
}
pos %= RING_BELT_LOOP_TICKS;
return pos;
}
public void record() {
TestingRecord+="\n"+angularVelocity;

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

@@ -1,4 +1,4 @@
package org.timecrafters.UltimateGoal;
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;

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

@@ -1,7 +1,8 @@
package org.timecrafters.UltimateGoal;
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Autonomous.DriveToCoordinates;
import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class TeleOpState extends CyberarmState {
@@ -16,13 +17,20 @@ public class TeleOpState extends CyberarmState {
private double powerScale = 0.2 ;
private boolean toggleSpeedInput = false;
private boolean launchInput = false;
private CyberarmState launchDriveState = new DriveToCoordinates(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y, robot.LAUNCH_ROTATION, 10, POWER_NORMAL, 50);
public TeleOpState(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
}
@Override
public void exec() {
robot.updateLocation();
@@ -41,43 +49,44 @@ public class TeleOpState extends CyberarmState {
double[] powers = {0,0,0,0};
boolean joystickButton = engine.gamepad1.left_stick_button;
if (joystickButton && !toggleSpeedInput) {
if (powerScale == POWER_NORMAL) {
powerScale = POWER_SPRINT;
} else {
powerScale = POWER_NORMAL;
}
}
toggleSpeedInput = joystickButton;
if (rightJoystickMagnitude == 0) {
if (leftJoystickMagnitude !=0) {
powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees);
}
} else {
if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {
powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees);
}
}
robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
if (engine.gamepad1.y) {
double distanceToTarget = Math.hypot(robot.LAUNCH_POSITION_X - robot.getLocationX(), robot.LAUNCH_POSITION_Y - robot.getLocationY());
if (distanceToTarget > robot.LAUNCH_TOLERANCE_POS) {
powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.LAUNCH_POSITION_X, robot.LAUNCH_POSITION_Y), POWER_NORMAL, robot.LAUNCH_ROTATION);
} else if (robot.getRelativeAngle(robot.LAUNCH_ROTATION, robot.getRotation()) > robot.LAUNCH_TOLERANCE_FACE) {
//todo add launch sequence
}
} else {
boolean joystickButton = engine.gamepad1.left_stick_button;
if (joystickButton && !toggleSpeedInput) {
if (powerScale == POWER_NORMAL) {
powerScale = POWER_SPRINT;
} else {
powerScale = POWER_NORMAL;
}
}
toggleSpeedInput = joystickButton;
if (rightJoystickMagnitude == 0) {
if (leftJoystickMagnitude !=0) {
powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees);
}
} else {
if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {
powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees);
}
}
}
robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
}
}

4
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ControlHubTest.java
View File

@@ -1,12 +1,10 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.hardware.bosch.BNO055IMU;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class ControlHubTest extends CyberarmState {

37
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/DistanceSensorTest.java Normal file
View File

@@ -0,0 +1,37 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.hardware.ColorRangeSensor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class DistanceSensorTest extends CyberarmState {
private Rev2mDistanceSensor distanceSensor;
// private RevColorSensorV3 colorSensor;
private ColorRangeSensor colorSensor;
private double distance = 0;
private double color = 0;
@Override
public void init() {
distanceSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distance");
colorSensor = engine.hardwareMap.get(ColorRangeSensor.class, "color");
// colorSensor = engine.hardwareMap.get(RevColorSensorV3.class, "color");
}
@Override
public void exec() {
distance = distanceSensor.getDistance(DistanceUnit.MM);
color = colorSensor.getDistance(DistanceUnit.MM);
}
@Override
public void telemetry() {
engine.telemetry.addData("distance", distance);
engine.telemetry.addData("color", color);
}
}

20
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/EncoderTest.java
View File

@@ -1,20 +0,0 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
public class EncoderTest extends CyberarmState {
private Robot robot;
public EncoderTest(Robot robot) {
this.robot = robot;
}
@Override
public void exec() {
}
}

39
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/FullTest.java Normal file
View File

@@ -0,0 +1,39 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class FullTest extends CyberarmState {
private Robot robot;
public FullTest(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
}
@Override
public void exec() {
double p = 0;
if (engine.gamepad1.x) {
p = 0.1;
}
robot.setDrivePower(p,p,p,p);
robot.collectionMotor.setPower(0.5);
robot.ringBeltMotor.setPower(p);
robot.launchMotor.setPower(p);
}
@Override
public void telemetry() {
}
}

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

@@ -4,7 +4,7 @@ import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class MecanumFunctionTest extends CyberarmState {

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

@@ -3,13 +3,12 @@ package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Robot;
import org.timecrafters.UltimateGoal.Competition.Robot;
@TeleOp (name = "Performance test", group = "test")
@TeleOp (name = "Hardware test", group = "test")
public class TestingEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
@@ -17,16 +16,14 @@ public class TestingEngine extends CyberarmEngine {
super.init();
}
@Override
public void setup() {
addState(new MecanumFunctionTest(robot));
}
@Override
public void stop() {
robot.saveRecording();
super.stop();
addState(new FullTest(robot));
}
//
// @Override
// public void stop() {
// robot.saveRecording();
// super.stop();
// }
}

82
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUDrive.java
View File

@@ -1,82 +0,0 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
public class IMUDrive extends CyberarmState {
private Robot robot;
private String actionName;
private String groupName;
private double power;
private int tickTarget;
private float angleRelative;
private float angleTarget;
private int tickStart;
private long finishDelay;
public IMUDrive(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.actionName = actionName;
this.groupName = groupName;
}
@Override
public void init() {
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
double inchesTarget = robot.stateConfiguration.variable(groupName, actionName, "inches").value();
tickTarget = (int) robot.inchesToTicks(inchesTarget);
angleTarget = robot.stateConfiguration.variable(groupName, actionName, "angle").value();
finishDelay = robot.stateConfiguration.variable(groupName, actionName, "delay").value();
}
@Override
public void start() {
if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
setHasFinished(true);
}
angleTarget=robot.getRotation();
tickStart = robot.driveFrontRight.motor.getCurrentPosition();
}
@Override
public void exec() {
robot.updateLocation();
int ticksTraveled = Math.abs( robot.driveFrontRight.motor.getCurrentPosition()-tickStart);
if (ticksTraveled > tickTarget) {
// robot.setDrivePower(0,0);
sleep(finishDelay);
setHasFinished(true);
} else {
angleRelative = robot.getRelativeAngle(angleTarget, robot.getRotation());
double turnPowerCorrection = Math.pow(0.03 * angleRelative, 3) + 0.01 * angleRelative;
double rightPower = power + turnPowerCorrection;
double leftPower = power - turnPowerCorrection;
double powerAdjust = ((2 * power) / (Math.abs(leftPower) + Math.abs(rightPower)));
// robot.setDrivePower(powerAdjust*leftPower, powerAdjust*rightPower);
}
}
@Override
public void telemetry() {
engine.telemetry.addLine("Measured Values");
engine.telemetry.addData("Y", robot.ticksToInches(robot.getLocationY()));
engine.telemetry.addData("X", robot.ticksToInches(robot.getLocationX()));
engine.telemetry.addLine();
engine.telemetry.addData("Rotation", robot.getRotation());
engine.telemetry.addLine();
engine.telemetry.addLine("Total Travel");
engine.telemetry.addData("Left", robot.ticksToInches(robot.traveledLeft));
engine.telemetry.addData("Right", robot.ticksToInches(robot.traveledRight));
}
}

90
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/IMUTurn.java
View File

@@ -1,90 +0,0 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
public class IMUTurn extends CyberarmState {
private Robot robot;
private String actionName;
private String groupName;
private double power;
private float angleTarget;
private int turnDirection;
private float angleAllowance;
private boolean useOptimalDirection;
public IMUTurn(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.actionName = actionName;
this.groupName = groupName;
}
@Override
public void init() {
power = robot.stateConfiguration.variable(groupName,actionName, "power").value();
angleTarget = robot.stateConfiguration.variable(groupName,actionName, "angle").value();
angleAllowance = robot.stateConfiguration.variable(groupName,actionName, "allowance").value();
// turnDirection = robot.stateConfiguration.variable(groupName, actionName, "direction").value();
turnDirection = 0;
useOptimalDirection = (turnDirection == 0);
}
@Override
public void start() {
if (!robot.stateConfiguration.action(groupName,actionName).enabled) {
setHasFinished(true);
}
}
@Override
public void exec() {
robot.updateLocation();
int OptimalDirection = getOptimalDirection(angleTarget, robot.getRotation());
if (!useOptimalDirection && OptimalDirection == turnDirection){
useOptimalDirection = true;
}
if (useOptimalDirection){
turnDirection = OptimalDirection;
}
// robot.setDrivePower(power * turnDirection,-power * turnDirection);
if (robot.getRotation() > angleTarget - angleAllowance && robot.getRotation() < angleTarget + angleAllowance ) {
// robot.setDrivePower(0,0);
setHasFinished(true);
}
}
private int getOptimalDirection(float angleTarget, float angleCurrent){
if (angleCurrent < 0) {
angleCurrent += 360;
}
if (angleTarget < 0) {
angleTarget += 360;
}
float degreeDifferance = angleTarget - angleCurrent;
if (degreeDifferance > 180 || (degreeDifferance < 0 && degreeDifferance > -180)) {
return -1;
} else {
return 1;
}
}
@Override
public void telemetry() {
engine.telemetry.addData("rotation", robot.getRotation());
}
}

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

@@ -1,35 +0,0 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Robot;
@TeleOp(name = "Localizer Test")
public class LocalizerTestingEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
super.init();
}
@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();
}
}

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

@@ -1,58 +0,0 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
public class VuforiaNavTesting extends CyberarmState {
private Robot robot;
private float angle;
private double X;
private double Y;
private float joystickDegrees;
private double power;
public VuforiaNavTesting(Robot robot) {
this.robot = robot;
}
@Override
public void exec() {
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
public void telemetry() {
engine.telemetry.addData("Target Visible", robot.trackableVisible);
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);
}
}