mirror of
https://github.com/TimeCrafters/UltimateGoal.git
synced 2025-12-13 05:02:34 +00:00
added second direction to the third odometer. Set up DriveToCoordinates, ThreadStates, and TeleOp. Cleaned up some obsolete states
This commit is contained in:
Nathaniel Palme
@@ -0,0 +1,72 @@
|
||||
package org.timecrafters.UltimateGoal.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Robot;
|
||||
|
||||
public class DriveToCoordinates extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName = "manual";
|
||||
private String actionName;
|
||||
private double xTarget;
|
||||
private double yTarget;
|
||||
private float faceAngle;
|
||||
private double tolerance;
|
||||
private double power;
|
||||
private boolean braking;
|
||||
private long breakStartTime;
|
||||
private long breakTime;
|
||||
|
||||
public DriveToCoordinates(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
public DriveToCoordinates(double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) {
|
||||
this.xTarget = xTarget;
|
||||
this.yTarget = yTarget;
|
||||
this.faceAngle = faceAngle;
|
||||
this.tolerance = tolerance;
|
||||
this.power = power;
|
||||
this.breakTime = breakTime;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
if (!groupName.equals("manual")) {
|
||||
xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "x").value());
|
||||
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "y").value());
|
||||
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
|
||||
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
|
||||
tolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tol").value());
|
||||
breakTime = robot.stateConfiguration.variable(groupName, actionName, "breakMS").value();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
|
||||
|
||||
if (distanceToTarget > tolerance) {
|
||||
double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
|
||||
robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
|
||||
braking = false;
|
||||
} else {
|
||||
if (!braking) {
|
||||
breakStartTime = System.currentTimeMillis();
|
||||
robot.setBrakePosAll();
|
||||
braking = true;
|
||||
}
|
||||
robot.brakeAll();
|
||||
setHasFinished(System.currentTimeMillis() - breakStartTime > breakTime);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
package org.timecrafters.UltimateGoal.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class ThreadStates extends CyberarmState {
|
||||
|
||||
private ArrayList<CyberarmState> states = new ArrayList<>();
|
||||
|
||||
public ThreadStates(ArrayList<CyberarmState> states) {
|
||||
this.states = states;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
for (CyberarmState state : states) {
|
||||
addParallelState(state);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
int finishedStates = 0;
|
||||
for (CyberarmState state : states) {
|
||||
if (state.getHasFinished()) {
|
||||
finishedStates += 1;
|
||||
}
|
||||
}
|
||||
setHasFinished(finishedStates == states.size());
|
||||
|
||||
}
|
||||
}
|
||||
@@ -3,14 +3,13 @@ package org.timecrafters.UltimateGoal.Calibration;
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class OdometryCalibration extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private float rotation;
|
||||
private int currentTick;
|
||||
private double ticksPerDegree;
|
||||
private double ticksPerDegreeClockwise;
|
||||
private double ticksPerDegreeCounterClockwise;
|
||||
|
||||
public OdometryCalibration(Robot robot) {
|
||||
this.robot = robot;
|
||||
@@ -19,21 +18,27 @@ public class OdometryCalibration extends CyberarmState {
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
double power = 0.1;
|
||||
rotation = -robot.imu.getAngularOrientation().firstAngle;
|
||||
currentTick = robot.encoderBack.getCurrentPosition();
|
||||
|
||||
if (engine.gamepad1.x) {
|
||||
double power = 0.1;
|
||||
robot.setDrivePower(power, -power, power, -power);
|
||||
currentTick = robot.encoderBack.getCurrentPosition();
|
||||
rotation = -robot.imu.getAngularOrientation().firstAngle;
|
||||
ticksPerDegree = currentTick/rotation;
|
||||
ticksPerDegreeClockwise = currentTick/rotation;
|
||||
} else if(engine.gamepad1.y) {
|
||||
robot.setDrivePower(-power, power, -power, power);
|
||||
ticksPerDegreeCounterClockwise = currentTick/rotation;
|
||||
} else {
|
||||
robot.setDrivePower(0,0,0,0);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("degrees", rotation);
|
||||
engine.telemetry.addData("ticks", currentTick);
|
||||
engine.telemetry.addData("ticks per degree", ticksPerDegree);
|
||||
engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise);
|
||||
engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -23,7 +23,7 @@ public class StalPowerCalibrator extends CyberarmState {
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
|
||||
// Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
|
||||
Powers = new double[] {0,0,0,0};
|
||||
|
||||
for (DcMotor motor : Motors) {
|
||||
|
||||
@@ -1,27 +1,78 @@
|
||||
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;
|
||||
|
||||
|
||||
public class ControlHubTest extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private float angle = 0;
|
||||
|
||||
private Acceleration acceleration;
|
||||
private double Vx = 0;
|
||||
private double Vy = 0;
|
||||
private double Vz = 0;
|
||||
private double aVx = 0;
|
||||
private double aVy = 0;
|
||||
private double aVz = 0;
|
||||
|
||||
|
||||
public ControlHubTest(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
Velocity startVelocity = new Velocity();
|
||||
startVelocity.xVeloc = 0;
|
||||
startVelocity.yVeloc = 0;
|
||||
startVelocity.zVeloc = 0;
|
||||
|
||||
Position startPosition = new Position();
|
||||
startPosition.x = 0;
|
||||
startPosition.y = 0;
|
||||
startPosition.z = 0;
|
||||
|
||||
robot.imu.startAccelerationIntegration(startPosition,startVelocity, 200);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
angle = robot.imu.getAngularOrientation().firstAngle;
|
||||
acceleration = robot.imu.getAcceleration();
|
||||
|
||||
Vx = robot.imu.getVelocity().xVeloc;
|
||||
Vy = robot.imu.getVelocity().yVeloc;
|
||||
Vz = robot.imu.getVelocity().zVeloc;
|
||||
|
||||
aVx = robot.imu.getAngularVelocity().xRotationRate;
|
||||
aVy = robot.imu.getAngularVelocity().yRotationRate;
|
||||
aVz = robot.imu.getAngularVelocity().zRotationRate;
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addLine("Greetings");
|
||||
engine.telemetry.addData("Angle", angle);
|
||||
engine.telemetry.addLine("Acceleration");
|
||||
engine.telemetry.addData("x", acceleration.xAccel);
|
||||
engine.telemetry.addData("y", acceleration.yAccel);
|
||||
engine.telemetry.addData("z", acceleration.zAccel);
|
||||
|
||||
engine.telemetry.addLine("Velocity");
|
||||
engine.telemetry.addData("X", Vx);
|
||||
engine.telemetry.addData("Y", Vy);
|
||||
engine.telemetry.addData("Z", Vz);
|
||||
|
||||
engine.telemetry.addLine("Angular Velocity");
|
||||
engine.telemetry.addData("X", aVx);
|
||||
engine.telemetry.addData("Y", aVy);
|
||||
engine.telemetry.addData("Z", aVz);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,66 +0,0 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Robot;
|
||||
|
||||
|
||||
public class ControlerTest extends CyberarmState {
|
||||
|
||||
private float leftJoystickDegrees;
|
||||
private double leftJoystickMagnitude;
|
||||
private float rightJoystickDegrees;
|
||||
private double rightJoystickMagnitude;
|
||||
private double leftJoystickX;
|
||||
private double leftJoystickY;
|
||||
private double rightJoystickX;
|
||||
private double rightJoystickY;
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
leftJoystickX = engine.gamepad1.left_stick_x;
|
||||
leftJoystickY = engine.gamepad1.left_stick_y;
|
||||
|
||||
leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
|
||||
leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
|
||||
|
||||
rightJoystickX = engine.gamepad1.left_stick_x;
|
||||
rightJoystickY = engine.gamepad1.left_stick_y;
|
||||
|
||||
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
|
||||
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
|
||||
engine.telemetry.addLine("Left Joystick");
|
||||
engine.telemetry.addData("Pos", "("+leftJoystickX+","+leftJoystickY+")");
|
||||
engine.telemetry.addData("Angle", leftJoystickDegrees);
|
||||
engine.telemetry.addData("Mag", leftJoystickMagnitude);
|
||||
|
||||
engine.telemetry.addLine("Right Joystick");
|
||||
engine.telemetry.addData("Pos", "("+rightJoystickX+","+rightJoystickY+")");
|
||||
engine.telemetry.addData("Angle", leftJoystickDegrees);
|
||||
engine.telemetry.addData("Mag", leftJoystickMagnitude);
|
||||
|
||||
engine.telemetry.addLine("Buttons");
|
||||
engine.telemetry.addData("a", engine.gamepad1.a);
|
||||
engine.telemetry.addData("b", engine.gamepad1.b);
|
||||
engine.telemetry.addData("x", engine.gamepad1.x);
|
||||
engine.telemetry.addData("y", engine.gamepad1.y);
|
||||
|
||||
engine.telemetry.addLine("Top");
|
||||
engine.telemetry.addData("Left Bump", engine.gamepad1.left_bumper);
|
||||
engine.telemetry.addData("Right Bump", engine.gamepad1.right_bumper);
|
||||
engine.telemetry.addData("Left Trigger", engine.gamepad1.left_trigger);
|
||||
engine.telemetry.addData("Right Trigger", engine.gamepad1.right_trigger);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -16,7 +16,5 @@ public class EncoderTest extends CyberarmState {
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
robot.setDrivePower(0.1, 0.1,0.1,0.1);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Robot;
|
||||
|
||||
|
||||
public class LauncherTest extends CyberarmState {
|
||||
|
||||
DcMotor LaunchMotor;
|
||||
DcMotor SuckMotor;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
LaunchMotor = engine.hardwareMap.dcMotor.get("launcher");
|
||||
SuckMotor = engine.hardwareMap.dcMotor.get("collector");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
LaunchMotor.setPower(1);
|
||||
SuckMotor.setPower(1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,9 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
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;
|
||||
|
||||
|
||||
@@ -16,15 +19,43 @@ public class MecanumFunctionTest extends CyberarmState {
|
||||
private double powerBackLeft=0;
|
||||
private double powerBackRight=0;
|
||||
|
||||
private static double TURN_POWER_SCALE = 0.5;
|
||||
private double aVx = 0;
|
||||
private double aVy = 0;
|
||||
private double aVz = 0;
|
||||
|
||||
private int powerStep = 5;
|
||||
private double POWER_SCALE = 0.5 ;
|
||||
private boolean toggleSpeedInput = false;
|
||||
|
||||
public MecanumFunctionTest(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
Velocity startVelocity = new Velocity();
|
||||
startVelocity.xVeloc = 0;
|
||||
startVelocity.yVeloc = 0;
|
||||
startVelocity.zVeloc = 0;
|
||||
|
||||
Position startPosition = new Position();
|
||||
startPosition.x = 0;
|
||||
startPosition.y = 0;
|
||||
startPosition.z = 0;
|
||||
|
||||
robot.imu.startAccelerationIntegration(startPosition,startVelocity, 10);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.updateLocation();
|
||||
robot.record();
|
||||
|
||||
AngularVelocity angularVelocity = robot.imu.getAngularVelocity();
|
||||
|
||||
aVx = angularVelocity.xRotationRate;
|
||||
aVy = angularVelocity.yRotationRate;
|
||||
aVz = angularVelocity.zRotationRate;
|
||||
|
||||
double leftJoystickX = engine.gamepad1.left_stick_x;
|
||||
double leftJoystickY = engine.gamepad1.left_stick_y;
|
||||
@@ -37,29 +68,45 @@ public class MecanumFunctionTest extends CyberarmState {
|
||||
|
||||
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
|
||||
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
|
||||
//
|
||||
|
||||
powerFrontLeft = 0;
|
||||
powerFrontRight = 0;
|
||||
powerBackLeft = 0;
|
||||
powerBackRight = 0;
|
||||
|
||||
boolean a = engine.gamepad1.a;
|
||||
boolean b = engine.gamepad1.b;
|
||||
|
||||
if (a && !toggleSpeedInput && POWER_SCALE < 1) {
|
||||
powerStep += 1;
|
||||
POWER_SCALE = powerStep * 0.1;
|
||||
}
|
||||
|
||||
if (b && !toggleSpeedInput && POWER_SCALE > 0.1) {
|
||||
powerStep -= 1;
|
||||
POWER_SCALE = powerStep * 0.1;
|
||||
}
|
||||
|
||||
toggleSpeedInput = a || b;
|
||||
|
||||
if (rightJoystickMagnitude == 0) {
|
||||
if (leftJoystickMagnitude !=0) {
|
||||
double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, leftJoystickDegrees);
|
||||
double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, leftJoystickDegrees);
|
||||
powerFrontLeft = powers[0];
|
||||
powerFrontRight = powers[1];
|
||||
powerBackLeft = powers[2];
|
||||
powerBackRight = powers[3];
|
||||
|
||||
}
|
||||
} else {
|
||||
if (leftJoystickMagnitude == 0) {
|
||||
double[] powers = robot.getFacePowers(rightJoystickDegrees, rightJoystickMagnitude);
|
||||
powerFrontLeft = TURN_POWER_SCALE * powers[0];
|
||||
powerFrontRight = TURN_POWER_SCALE * powers[1];
|
||||
powerBackLeft = TURN_POWER_SCALE * powers[0];
|
||||
powerBackRight = TURN_POWER_SCALE * powers[1];
|
||||
double[] powers = robot.getFacePowers(rightJoystickDegrees, POWER_SCALE * rightJoystickMagnitude);
|
||||
powerFrontLeft = powers[0];
|
||||
powerFrontRight = powers[1];
|
||||
powerBackLeft = powers[0];
|
||||
powerBackRight = powers[1];
|
||||
} else {
|
||||
double[] powers = robot.getMecanumPowers(leftJoystickDegrees, leftJoystickMagnitude, rightJoystickDegrees);
|
||||
double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, rightJoystickDegrees);
|
||||
powerFrontLeft = powers[0];
|
||||
powerFrontRight = powers[1];
|
||||
powerBackLeft = powers[2];
|
||||
@@ -67,22 +114,24 @@ public class MecanumFunctionTest extends CyberarmState {
|
||||
}
|
||||
}
|
||||
|
||||
// robot.record(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
|
||||
|
||||
robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Scale", POWER_SCALE);
|
||||
|
||||
engine.telemetry.addLine("Left Joystick");
|
||||
engine.telemetry.addData("Angle", leftJoystickDegrees);
|
||||
engine.telemetry.addData("Mag", leftJoystickMagnitude);
|
||||
|
||||
engine.telemetry.addLine("Right Joystick");
|
||||
engine.telemetry.addData("Angle", rightJoystickDegrees);
|
||||
engine.telemetry.addData("Mag", rightJoystickMagnitude);
|
||||
engine.telemetry.addLine("Angular Velocity");
|
||||
engine.telemetry.addData("X", aVx);
|
||||
engine.telemetry.addData("Y", aVy);
|
||||
engine.telemetry.addData("Z", aVz);
|
||||
|
||||
engine.telemetry.addLine("Powers");
|
||||
engine.telemetry.addData("FL", robot.driveFrontLeft.motor.getPower());
|
||||
engine.telemetry.addData("FR", robot.driveFrontRight.motor.getPower());
|
||||
engine.telemetry.addData("BL", robot.driveBackLeft.motor.getPower());
|
||||
engine.telemetry.addData("BR", robot.driveBackRight.motor.getPower());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.UltimateGoal.Robot;
|
||||
|
||||
@TeleOp (name = "Encoder test", group = "test")
|
||||
@TeleOp (name = "Performance test", group = "test")
|
||||
public class TestingEngine extends CyberarmEngine {
|
||||
|
||||
private Robot robot;
|
||||
@@ -24,9 +24,9 @@ public class TestingEngine extends CyberarmEngine {
|
||||
addState(new MecanumFunctionTest(robot));
|
||||
}
|
||||
|
||||
// @Override
|
||||
// public void stop() {
|
||||
// robot.saveRecording();
|
||||
// super.stop();
|
||||
// }
|
||||
@Override
|
||||
public void stop() {
|
||||
robot.saveRecording();
|
||||
super.stop();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,30 +0,0 @@
|
||||
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!");
|
||||
|
||||
}
|
||||
}
|
||||
@@ -38,7 +38,7 @@ public class IMUDrive extends CyberarmState {
|
||||
}
|
||||
|
||||
angleTarget=robot.getRotation();
|
||||
tickStart = robot.driveFrontRight.getCurrentPosition();
|
||||
tickStart = robot.driveFrontRight.motor.getCurrentPosition();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -46,7 +46,7 @@ public class IMUDrive extends CyberarmState {
|
||||
|
||||
robot.updateLocation();
|
||||
|
||||
int ticksTraveled = Math.abs( robot.driveFrontRight.getCurrentPosition()-tickStart);
|
||||
int ticksTraveled = Math.abs( robot.driveFrontRight.motor.getCurrentPosition()-tickStart);
|
||||
if (ticksTraveled > tickTarget) {
|
||||
// robot.setDrivePower(0,0);
|
||||
sleep(finishDelay);
|
||||
|
||||
@@ -14,6 +14,8 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
|
||||
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
|
||||
@@ -57,25 +59,22 @@ public class Robot {
|
||||
public DcMotor encoderRight;
|
||||
public DcMotor encoderBack;
|
||||
|
||||
private int brakePosFrontLeft;
|
||||
private int brakePosFrontRight;
|
||||
private int brakePosBackLeft;
|
||||
private int brakePosBackRight;
|
||||
public DcMotor launcher;
|
||||
|
||||
static final double BIAS_FRONT_LEFT = 1;
|
||||
static final double BIAS_FRONT_RIGHT = 1;
|
||||
static final double BIAS_BACK_LEFT = 1;
|
||||
static final double BIAS_BACK_RIGHT = 1;
|
||||
|
||||
static final double FINE_CORRECTION = 0.01;
|
||||
static final double LARGE_CORRECTION = 0.01;
|
||||
//Steering Constants
|
||||
static final double FINE_CORRECTION = 0.05 ;
|
||||
static final double LARGE_CORRECTION = 0.002;
|
||||
static final double MOMENTUM_CORRECTION = 1.015;
|
||||
static final double MOMENTUM_MAX_CORRECTION = 1.35;
|
||||
static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
|
||||
|
||||
//Conversion Constants
|
||||
static final double ENCODER_CIRCUMFERENCE = Math.PI * 4;
|
||||
static final int COUNTS_PER_REVOLUTION = 8192;
|
||||
static final float mmPerInch = 25.4f;
|
||||
//todo make/run calibration code to find this exact value
|
||||
static final double TICKS_PER_ROBOT_DEGREE = Math.PI * 1000;
|
||||
static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = Math.PI * 1000;
|
||||
static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = Math.PI * 1000;
|
||||
|
||||
// Inches Forward of axis of rotation
|
||||
static final float CAMERA_FORWARD_DISPLACEMENT = 13f;
|
||||
@@ -89,28 +88,48 @@ public class Robot {
|
||||
public double locationY;
|
||||
private float rotation;
|
||||
|
||||
private int encoderLeftPrevious = 0;
|
||||
private int encoderBackPrevious = 0;
|
||||
private int encoderRightPrevious = 0;
|
||||
private float rotationPrevious = 0;
|
||||
public float angularVelocity;
|
||||
|
||||
//Launcher
|
||||
public DcMotor launchMotor;
|
||||
|
||||
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;
|
||||
public static final double LAUNCH_TOLERANCE_POS = 50;
|
||||
public static final double LAUNCH_TOLERANCE_FACE = 0.5;
|
||||
|
||||
//Ring Belt
|
||||
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;
|
||||
|
||||
//Debugging
|
||||
public double visionX;
|
||||
public double visionY;
|
||||
public float rawAngle;
|
||||
// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
|
||||
private String TestingRecord = "Rotation";
|
||||
private String TestingRecord = "AngularVelocity";
|
||||
|
||||
public double traveledLeft;
|
||||
public double traveledRight;
|
||||
|
||||
|
||||
private int encoderLeftPrevious = 0;
|
||||
private int encoderBackPrevious = 0;
|
||||
private int encoderRightPrevious = 0;
|
||||
private float rotationPrevious = 0;
|
||||
|
||||
//vuforia navigation
|
||||
public boolean trackableVisible;
|
||||
private VuforiaTrackables targetsUltimateGoal;
|
||||
private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
|
||||
private OpenGLMatrix lastConfirmendLocation;
|
||||
|
||||
private long timeStartZeroVelocity = 0;
|
||||
private static final long MIN_CHECK_DURATION_MS = 500;
|
||||
private static final double MIN_CHECK_VELOCITY = 0.005;
|
||||
|
||||
//TensorFlow Object Detection
|
||||
public TFObjectDetector tfObjectDetector;
|
||||
private static final float MINIMUM_CONFIDENCE = 0.8f;
|
||||
@@ -126,16 +145,24 @@ public class Robot {
|
||||
MotorConfigurationType motorType = dcMotor.getMotorType();
|
||||
motorType.setGearing(5);
|
||||
motorType.setTicksPerRev(140);
|
||||
motorType.setMaxRPM(1200);
|
||||
motorType.setMaxRPM(800);
|
||||
|
||||
driveFrontLeft = new DriveMotor(dcMotor, motorType, DcMotorSimple.Direction.FORWARD);
|
||||
driveFrontRight = new DriveMotor(hardwareMap.dcMotor.get("driveFrontRight"), motorType, DcMotorSimple.Direction.REVERSE);
|
||||
driveBackLeft = new DriveMotor(hardwareMap.dcMotor.get("driveBackLeft"), motorType, DcMotorSimple.Direction.FORWARD);
|
||||
driveBackRight = new DriveMotor(hardwareMap.dcMotor.get("driveBackRight"), motorType, DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
driveFrontLeft.init();
|
||||
driveFrontRight.init();
|
||||
driveBackLeft.init();
|
||||
driveBackRight.init();
|
||||
|
||||
//todo add these when they exist
|
||||
encoderLeft = hardwareMap.dcMotor.get("odoLeft");
|
||||
encoderRight = hardwareMap.dcMotor.get("odoRight");
|
||||
encoderBack = hardwareMap.dcMotor.get("odoBack");
|
||||
// encoderRight = hardwareMap.dcMotor.get("odoRight");
|
||||
// encoderBack = hardwareMap.dcMotor.get("odoBack");
|
||||
//
|
||||
// launcher = hardwareMap.dcMotor.get("launcher");
|
||||
|
||||
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||
|
||||
@@ -146,6 +173,18 @@ public class Robot {
|
||||
|
||||
imu.initialize(parameters);
|
||||
|
||||
Velocity startVelocity = new Velocity();
|
||||
startVelocity.xVeloc = 0;
|
||||
startVelocity.yVeloc = 0;
|
||||
startVelocity.zVeloc = 0;
|
||||
|
||||
Position startPosition = new Position();
|
||||
startPosition.x = 0;
|
||||
startPosition.y = 0;
|
||||
startPosition.z = 0;
|
||||
|
||||
imu.startAccelerationIntegration(startPosition,startVelocity, 10);
|
||||
|
||||
// initVuforia();
|
||||
|
||||
rotation = stateConfiguration.variable("system", "startPos", "direction").value();
|
||||
@@ -216,10 +255,12 @@ public class Robot {
|
||||
|
||||
//run this in every exec to track the robot's location.
|
||||
public void updateLocation(){
|
||||
|
||||
// orientation is inverted to have clockwise be positive.
|
||||
float imuAngle = -imu.getAngularOrientation().firstAngle;
|
||||
double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
|
||||
|
||||
//todo add this when encoders exist.
|
||||
int encoderLeftCurrent = encoderLeft.getCurrentPosition();
|
||||
int encoderRightCurrent = encoderRight.getCurrentPosition();
|
||||
int encoderBackCurrent = encoderBack.getCurrentPosition();
|
||||
@@ -241,7 +282,16 @@ public class Robot {
|
||||
//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 sidewaysVector = encoderBackChange + (rotationChange*TICKS_PER_ROBOT_DEGREE);
|
||||
|
||||
double ticksPerDegree;
|
||||
|
||||
if(rotationChange < 0) {
|
||||
ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
|
||||
} else {
|
||||
ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
|
||||
}
|
||||
//todo: check if direction of the back encoder gives expected values
|
||||
double sidewaysVector = encoderBackChange - (rotationChange* ticksPerDegree);
|
||||
|
||||
double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
|
||||
double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
|
||||
@@ -251,9 +301,26 @@ public class Robot {
|
||||
|
||||
locationX += xChange;
|
||||
locationY += yChange;
|
||||
|
||||
rotation += rotationChange;
|
||||
|
||||
|
||||
double totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightCurrent) + Math.abs(encoderBackChange);
|
||||
|
||||
if (totalV < MIN_CHECK_VELOCITY) {
|
||||
long timeCurrent = System.currentTimeMillis();
|
||||
|
||||
if (timeStartZeroVelocity == 0) {
|
||||
timeStartZeroVelocity = timeCurrent;
|
||||
} else if (timeCurrent - timeStartZeroVelocity >= MIN_CHECK_DURATION_MS) {
|
||||
syncWithVuforia();
|
||||
}
|
||||
|
||||
} else {
|
||||
timeStartZeroVelocity = 0;
|
||||
}
|
||||
|
||||
|
||||
if (rotation > 180) {
|
||||
rotation -= 360;
|
||||
}
|
||||
@@ -263,6 +330,15 @@ public class Robot {
|
||||
|
||||
}
|
||||
|
||||
public void checkVuforiaSync() {
|
||||
|
||||
double V1 = driveFrontLeft.motor.getPower();
|
||||
double V2 = driveFrontRight.motor.getPower();
|
||||
double V3 = driveBackLeft.motor.getPower();
|
||||
double V4 = driveBackRight.motor.getPower();
|
||||
|
||||
}
|
||||
|
||||
public void syncWithVuforia() {
|
||||
trackableVisible = false;
|
||||
for (VuforiaTrackable trackable : trackables) {
|
||||
@@ -354,10 +430,10 @@ public class Robot {
|
||||
|
||||
//Drive Functions
|
||||
public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){
|
||||
driveFrontLeft.setPower(powerFrontLeft * BIAS_FRONT_LEFT);
|
||||
driveFrontRight.setPower(powerFrontRight * BIAS_FRONT_RIGHT);
|
||||
driveBackLeft.setPower(powerBackLeft * BIAS_BACK_LEFT);
|
||||
driveBackRight.setPower(powerBackRight * BIAS_BACK_RIGHT);
|
||||
driveFrontLeft.setPower(powerFrontLeft);
|
||||
driveFrontRight.setPower(powerFrontRight);
|
||||
driveBackLeft.setPower(powerBackLeft);
|
||||
driveBackRight.setPower(powerBackRight);
|
||||
}
|
||||
|
||||
//returns an array of the powers necessary to execute the provided motion. "degreesDirectionMotion"
|
||||
@@ -365,25 +441,52 @@ public class Robot {
|
||||
//the angle the robot should face relative to the field. The order of the output powers is
|
||||
//is ForwardLeft, ForwardRight, BackLeft, BackRight
|
||||
public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) {
|
||||
angularVelocity = imu.getAngularVelocity().xRotationRate;
|
||||
|
||||
//calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion
|
||||
//once it is pointed towards the degreesDirectionFace
|
||||
double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion));
|
||||
double y = scalar * Math.cos(rad);
|
||||
double x = scalar * Math.sin(rad);
|
||||
double y = Math.cos(rad);
|
||||
double x = Math.sin(rad);
|
||||
|
||||
double p = y + x;
|
||||
double q = y - x;
|
||||
|
||||
//calculating correction needed to steer the robot towards the degreesDirectionFace
|
||||
float relativeRotation = getRelativeAngle(degreesDirectionFace, rotation);
|
||||
double turnCorrection = Math.pow(LARGE_CORRECTION * relativeRotation, 3) + FINE_CORRECTION * relativeRotation;
|
||||
|
||||
double powerForwardRight = q + turnCorrection;
|
||||
double powerForwardLeft = p - turnCorrection;
|
||||
double powerBackRight = p + turnCorrection;
|
||||
double powerBackLeft = q - turnCorrection;
|
||||
double powerForwardRight = scalar * (q + turnCorrection);
|
||||
double powerForwardLeft = scalar * (p - turnCorrection);
|
||||
double powerBackRight = scalar * (p + turnCorrection);
|
||||
double powerBackLeft = scalar * (q - turnCorrection);
|
||||
|
||||
//the turnCorrection often results in powers with magnitudes significantly larger than the
|
||||
//scalar. The scaleRatio mitigates this without altering the quality of the motion by making
|
||||
//it so that the average of the four magnitudes is equal to the scalar magnitude.
|
||||
double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) +
|
||||
Math.abs(powerBackRight) + Math.abs(powerBackLeft);
|
||||
double scaleRatio = (4 * Math.abs(scalar))/powerSum;
|
||||
|
||||
powerForwardRight *= scaleRatio;
|
||||
powerForwardLeft *= scaleRatio;
|
||||
powerBackRight *= scaleRatio;
|
||||
powerBackLeft *= scaleRatio;
|
||||
|
||||
|
||||
if (relativeRotation != 0) {
|
||||
double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation));
|
||||
double exponential = Math.pow(MOMENTUM_CORRECTION, -momentumRelative);
|
||||
double momentumCorrection = (2*exponential)/(1+exponential);
|
||||
powerForwardRight *= momentumCorrection;
|
||||
powerForwardLeft *= momentumCorrection;
|
||||
powerBackRight *= momentumCorrection;
|
||||
powerBackLeft *= momentumCorrection;
|
||||
}
|
||||
|
||||
// The "extreme" is the power value that is furthest from zero. When this values exceed the
|
||||
// -1 to 1 power range, dividing the powers by the "extreme" scales everything back into the
|
||||
// workable range without altering the final motion vector;
|
||||
// workable range without altering the final motion vector.
|
||||
|
||||
double extreme = Math.max(
|
||||
Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)),
|
||||
@@ -396,6 +499,15 @@ public class Robot {
|
||||
powerBackLeft = powerBackLeft/extreme;
|
||||
}
|
||||
|
||||
// double powerControlThreshold = 0.6 * ;
|
||||
//
|
||||
// if (Math.min(extreme, 1) > powerControlThreshold) {
|
||||
// powerForwardLeft *= powerControlThreshold;
|
||||
// powerForwardRight *= powerControlThreshold;
|
||||
// powerBackLeft *= powerControlThreshold;
|
||||
// powerBackRight *= powerControlThreshold;
|
||||
// }
|
||||
|
||||
double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
|
||||
|
||||
return powers;
|
||||
@@ -409,7 +521,7 @@ public class Robot {
|
||||
}
|
||||
|
||||
//sets the position the motors will lock to when braking to the motor's current position.
|
||||
public void setAllBrakePos() {
|
||||
public void setBrakePosAll() {
|
||||
driveFrontLeft.setBrakePosition();
|
||||
driveFrontRight.setBrakePosition();
|
||||
driveBackLeft.setBrakePosition();
|
||||
@@ -418,17 +530,21 @@ public class Robot {
|
||||
|
||||
//Outputs the power necessary to turn and face a provided direction
|
||||
public double[] getFacePowers(float direction, double power) {
|
||||
angularVelocity = imu.getAngularVelocity().xRotationRate;
|
||||
double relativeAngle = getRelativeAngle(direction, rotation);
|
||||
double scaler = Math.pow(LARGE_CORRECTION * relativeAngle, 3) + FINE_CORRECTION * relativeAngle;
|
||||
|
||||
if (relativeAngle != 0) {
|
||||
double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle));
|
||||
double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
|
||||
double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
|
||||
|
||||
scaler *= momentumCorrection;
|
||||
}
|
||||
|
||||
double left = -power * scaler;
|
||||
double right = power *scaler;
|
||||
|
||||
|
||||
// if (relativeAngle > 0) {
|
||||
// left *= -1;
|
||||
// right *= -1;
|
||||
// }
|
||||
double[] powers = {left,right};
|
||||
return powers;
|
||||
}
|
||||
@@ -442,7 +558,7 @@ public class Robot {
|
||||
//towards the target angle
|
||||
//--------------------------------------------------------------------------------------
|
||||
|
||||
double turnPowerCorrection = Math.pow(0.03 * relativeAngle, 3) + 0.02 * relativeAngle;
|
||||
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;
|
||||
@@ -463,7 +579,7 @@ public class Robot {
|
||||
// }
|
||||
|
||||
public void record() {
|
||||
TestingRecord+="\n"+rotation;
|
||||
TestingRecord+="\n"+angularVelocity;
|
||||
}
|
||||
|
||||
public void saveRecording() {
|
||||
|
||||
@@ -1,4 +1,10 @@
|
||||
package org.timecrafters.UltimateGoal;
|
||||
|
||||
public class TeleOpEngine {
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
public class TeleOpEngine extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,4 +1,83 @@
|
||||
package org.timecrafters.UltimateGoal;
|
||||
|
||||
public class TeleOpState {
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Autonomous.DriveToCoordinates;
|
||||
|
||||
public class TeleOpState extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private float leftJoystickDegrees;
|
||||
private double leftJoystickMagnitude;
|
||||
private float rightJoystickDegrees;
|
||||
private double rightJoystickMagnitude;
|
||||
|
||||
private double POWER_SPRINT = 0.4;
|
||||
private double POWER_NORMAL = 0.2;
|
||||
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 exec() {
|
||||
robot.updateLocation();
|
||||
|
||||
double leftJoystickX = engine.gamepad1.left_stick_x;
|
||||
double leftJoystickY = engine.gamepad1.left_stick_y;
|
||||
|
||||
leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
|
||||
leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
|
||||
|
||||
double rightJoystickX = engine.gamepad1.right_stick_x;
|
||||
double rightJoystickY = engine.gamepad1.right_stick_y;
|
||||
|
||||
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
|
||||
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
|
||||
|
||||
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) {
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user