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

Teleop and Auto

Browse Source
This commit is contained in:
Nathaniel Palme
2021-02-11 20:44:37 -06:00
parent 1810e70eb1
commit f75b05693f
12 changed files with 226 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.idea/compiler.xml generated Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CompilerConfiguration">
<bytecodeTargetLevel target="1.8" />
</component>
</project>

2
.idea/misc.xml generated
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<project version="4"> <project version="4">
<component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" project-jdk-name="1.8" project-jdk-type="JavaSDK"> <component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" default="true" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<output url="file://$PROJECT_DIR$/build/classes" /> <output url="file://$PROJECT_DIR$/build/classes" />
</component> </component>
<component name="ProjectType"> <component name="ProjectType">

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

@@ -32,7 +32,7 @@ public class OdometryCalibration extends CyberarmState {
rotation += robot.getRelativeAngle(imu, rotationPrev); rotation += robot.getRelativeAngle(imu, rotationPrev);
rotationPrev = imu; rotationPrev = imu;
currentTick = robot.encoderBack.getCurrentPosition(); currentTick = (robot.encoderRight.getCurrentPosition() + robot.encoderLeft.getCurrentPosition()) / 2;
if (engine.gamepad1.x) { if (engine.gamepad1.x) {
robot.setDrivePower(power, -power, power, -power); robot.setDrivePower(power, -power, power, -power);

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

@@ -3,12 +3,15 @@ package org.timecrafters.UltimateGoal.Competition.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine; import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.Robot; import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
@Autonomous (name = "Autonomous") @Autonomous (name = "Autonomous")
public class AutoEngine extends CyberarmEngine { public class AutoEngine extends CyberarmEngine {
private Robot robot; private Robot robot;
private TensorFlowCheck tensorFlowCheck;
@Override @Override
public void init() { public void init() {
@@ -20,8 +23,55 @@ public class AutoEngine extends CyberarmEngine {
@Override @Override
public void setup() { public void setup() {
//drive to view //drive to view
addState(new DriveToCoordinates(robot, "auto", "001_0")); addState(new DriveToCoordinates(robot, "auto", "01_0"));
addState(new DriveToCoordinates(robot, "auto", "001_0"));
//select scoring area
tensorFlowCheck = new TensorFlowCheck(robot, "auto", "02_0");
addState(tensorFlowCheck);
//drive around ring stack
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));
//launch rings
addState(new Launch(robot));
//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));
//turn arm towards scoreing area.
addState(new Face(robot, "auto", "05_1"));
//open the wobble grab arm
addState(new WobbleGrab(robot, "auto", "06_0", true));
//drive to second wobble
addState(new DriveToCoordinates(robot, "auto", "07_0"));
addState(new DriveWithColorSensor(robot, "auto", "08_0"));
//close grabber
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));
//release wobble goal 2
addState(new WobbleGrab(robot, "auto", "10_0", true));
//drive to park
addState(new DriveToCoordinates(robot, "auto", "11_0"));
} }
} }

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

@@ -15,7 +15,7 @@ public class DriveToCoordinates extends CyberarmState {
private double power; private double power;
private boolean braking; private boolean braking;
private long breakStartTime; private long breakStartTime;
private long breakTime; private long brakeTime;
private boolean autoFace; private boolean autoFace;
public DriveToCoordinates(Robot robot, String groupName, String actionName) { public DriveToCoordinates(Robot robot, String groupName, String actionName) {
@@ -24,14 +24,14 @@ public class DriveToCoordinates extends CyberarmState {
this.actionName = actionName; this.actionName = actionName;
} }
public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long breakStartTime) { public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime) {
this.robot = robot; this.robot = robot;
this.xTarget = xTarget; this.xTarget = xTarget;
this.yTarget = yTarget; this.yTarget = yTarget;
this.faceAngle = faceAngle; this.faceAngle = faceAngle;
this.tolerancePos = tolerance; this.tolerancePos = tolerance;
this.power = power; this.power = power;
this.breakTime = breakTime; this.brakeTime = brakeTime;
} }
@Override @Override
@@ -41,7 +41,7 @@ public class DriveToCoordinates extends CyberarmState {
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value()); yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
power = robot.stateConfiguration.variable(groupName, actionName, "power").value(); power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value()); tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value(); brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
try { try {
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value(); faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
@@ -53,6 +53,11 @@ public class DriveToCoordinates extends CyberarmState {
@Override @Override
public void start() { public void start() {
if (robot.stateConfiguration.action(groupName,actionName).enabled) {
setHasFinished(true);
}
if (autoFace) { if (autoFace) {
faceAngle = robot.getAngleToPosition(xTarget,yTarget); faceAngle = robot.getAngleToPosition(xTarget,yTarget);
} }
@@ -73,7 +78,7 @@ public class DriveToCoordinates extends CyberarmState {
breakStartTime = currentTime; breakStartTime = currentTime;
braking = true; braking = true;
} }
setHasFinished(currentTime - breakStartTime >= breakTime); setHasFinished(currentTime - breakStartTime >= brakeTime);
} }
} }

41
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveWithColorSensor.java Normal file
View File

@@ -0,0 +1,41 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class DriveWithColorSensor extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private double power;
private double minimum;
private double maximum;
public DriveWithColorSensor(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
}
@Override
public void init() {
power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
minimum = robot.stateConfiguration.variable(groupName,actionName,"min").value();
maximum = robot.stateConfiguration.variable(groupName,actionName,"max").value();
}
@Override
public void exec() {
double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
if (sensorValue < minimum) {
robot.getMecanumPowers(180,power,0);
} else if (sensorValue > maximum) {
robot.getMecanumPowers(0,power,0);
} else {
robot.setDrivePower(0,0,0,0);
setHasFinished(true);
}
}
}

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

@@ -17,12 +17,18 @@ public class TensorFlowCheck extends CyberarmState {
private long checkTime; private long checkTime;
public double wobblePosX; public double wobblePosX;
public double wobblePosY; public double wobblePosY;
private int manualPath;
public TensorFlowCheck(Robot robot, String group, String action) {
this.robot = robot;
this.group = group;
this.action = action;
}
@Override @Override
public void init() { public void init() {
checkTime = robot.stateConfiguration.variable(group,action,"time").value(); checkTime = robot.stateConfiguration.variable(group,action,"time").value();
manualPath = robot.stateConfiguration.variable(group,action,"path").value();
} }
@Override @Override
@@ -32,27 +38,41 @@ public class TensorFlowCheck extends CyberarmState {
@Override @Override
public void exec() { public void exec() {
recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
if (recognitions != null) { if (manualPath == -1) {
recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
if (recognitions.size() == 1) { if (recognitions != null) {
Recognition recognition = recognitions.get(0);
String label = recognition.getLabel(); if (recognitions.size() == 1) {
if (label.equals("Single")) { Recognition recognition = recognitions.get(0);
path = 1; String label = recognition.getLabel();
} else if (label.equals("Quad")) { if (label.equals("Single")) {
path = 2; path = 1;
} else if (label.equals("Quad")) {
path = 2;
}
} else if (recognitions.size() > 1) {
path = 1;
} }
} else if (recognitions.size() > 1) {
path = 1;
} }
} else {
path = manualPath;
} }
if (runTime() >= checkTime) { if (runTime() >= checkTime) {
if (checkTime == 0) { if (path == 0) {
wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value(); wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos0","x").value();
wobblePosY = 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();
}
if (path == 2) {
wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos2","x").value();
wobblePosY = robot.stateConfiguration.variable("auto", "_goalPos2","y").value();
}
} }
} }

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

@@ -27,6 +27,11 @@ public class Launch extends CyberarmState {
if (hasCycled) { if (hasCycled) {
robot.ringBeltMotor.setPower(0); robot.ringBeltMotor.setPower(0);
robot.ringBeltStage = 0; robot.ringBeltStage = 0;
if (!robot.initLauncher) {
robot.launchMotor.setPower(0);
}
setHasFinished(true); setHasFinished(true);
} else { } else {
hasCycled = true; hasCycled = true;
@@ -34,6 +39,5 @@ public class Launch extends CyberarmState {
} }
detectedPass = detectingPass; detectedPass = detectingPass;
// if (robot.getBeltPos() > robot.loopPos(Robot.RING_BELT_GAP * 3) && hasCycled);
} }
} }

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

@@ -6,17 +6,12 @@ public class ProgressRingBelt extends CyberarmState {
private Robot robot; private Robot robot;
private int targetPos; private int targetPos;
private boolean useLaunchPrep; private boolean prepLaunch;
public ProgressRingBelt(Robot robot) { public ProgressRingBelt(Robot robot) {
this.robot = robot; this.robot = robot;
} }
@Override
public void init() {
useLaunchPrep = (robot.launchMotor.getPower() == 0);
}
@Override @Override
public void start() { public void start() {
int currentPos = robot.getBeltPos(); int currentPos = robot.getBeltPos();
@@ -28,6 +23,7 @@ public class ProgressRingBelt extends CyberarmState {
targetPos = robot.loopPos(currentPos + 160); targetPos = robot.loopPos(currentPos + 160);
robot.ringBeltOn(); robot.ringBeltOn();
robot.ringBeltStage += 1; robot.ringBeltStage += 1;
prepLaunch = !robot.initLauncher;
} }
} }
@@ -38,7 +34,7 @@ public class ProgressRingBelt extends CyberarmState {
if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) { if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) {
robot.ringBeltMotor.setPower(0); robot.ringBeltMotor.setPower(0);
if(useLaunchPrep) { if(prepLaunch) {
robot.launchMotor.setPower(Robot.LAUNCH_POWER); robot.launchMotor.setPower(Robot.LAUNCH_POWER);
} }

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

@@ -5,6 +5,7 @@ import android.util.Log;
import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.hardware.rev.RevTouchSensor; import com.qualcomm.hardware.rev.RevTouchSensor;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -60,9 +61,9 @@ public class Robot {
//Steering Constants //Steering Constants
static final double FINE_CORRECTION = 0.055 ; static final double FINE_CORRECTION = 0.055 ;
static final double LARGE_CORRECTION = 0.025 ; static final double LARGE_CORRECTION = 0.025;
static final double MOMENTUM_CORRECTION = 1.02; static final double MOMENTUM_CORRECTION = 1.045;
static final double MOMENTUM_MAX_CORRECTION = 1.3; static final double MOMENTUM_MAX_CORRECTION = 1.4;
static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION)); static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
//Conversion Constants //Conversion Constants
@@ -95,13 +96,13 @@ public class Robot {
public static final double LAUNCH_POWER = 0.75; public static final double LAUNCH_POWER = 0.75;
private static final long LAUNCH_ACCEL_TIME = 500; private static final long LAUNCH_ACCEL_TIME = 500;
public static final double LAUNCH_POSITION_X = 36 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); public double launchPositionX;
public static final double LAUNCH_POSITION_Y = -8 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); public double launchPositionY;
public static final float LAUNCH_ROTATION = 0; public float launchRotation;
public static final double LAUNCH_TOLERANCE_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE); public static final double LAUNCH_TOLERANCE_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
public static final double LAUNCH_TOLERANCE_FACE = 0.5; public static final double LAUNCH_TOLERANCE_FACE = 0.5;
public boolean initLauncher;
//Ring Intake //Ring Intake
public DcMotor collectionMotor; public DcMotor collectionMotor;
@@ -123,8 +124,8 @@ public class Robot {
public DcMotor wobbleArmMotor; public DcMotor wobbleArmMotor;
public Servo wobbleGrabServo; public Servo wobbleGrabServo;
public static final int WOBBLE_ARM_DOWN = -710; public static final int WOBBLE_ARM_DOWN = -710;
public static final double WOBBLE_SERVO_MAX = 0.3; public static final double WOBBLE_SERVO_MAX = 0.3;
public RevColorSensorV3 wobbleColorSensor;
//vuforia navigation //vuforia navigation
private WebcamName webcam; private WebcamName webcam;
@@ -249,8 +250,11 @@ public class Robot {
launchMotor = launcher; launchMotor = launcher;
launchMotor.setMotorType(launchMotorType); launchMotor.setMotorType(launchMotorType);
launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
if (stateConfiguration.action("system","initLauncher").enabled) { initLauncher = stateConfiguration.action("system","initLauncher").enabled;
if (initLauncher) {
double launcherPower = 0; double launcherPower = 0;
long launchAccelStart = System.currentTimeMillis(); long launchAccelStart = System.currentTimeMillis();
while (launcherPower < LAUNCH_POWER) { while (launcherPower < LAUNCH_POWER) {
@@ -258,6 +262,10 @@ public class Robot {
launchMotor.setPower(launcherPower); launchMotor.setPower(launcherPower);
} }
} }
//
launchPositionX = stateConfiguration.variable("system", "launchPos","x").value();
launchPositionX = stateConfiguration.variable("system", "launchPos","y").value();
launchRotation = stateConfiguration.variable("system", "launchPos","rot").value();
} }
@@ -353,7 +361,7 @@ public class Robot {
double ticksPerDegree; double ticksPerDegree;
if(rotationChange < 0) { if (rotationChange < 0) {
ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE; ticksPerDegree = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
} else { } else {
ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE; ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;

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

@@ -14,11 +14,12 @@ public class TeleOpState extends CyberarmState {
private double leftJoystickMagnitude; private double leftJoystickMagnitude;
private float rightJoystickDegrees; private float rightJoystickDegrees;
private double rightJoystickMagnitude; private double rightJoystickMagnitude;
private float cardinalSnapping;
private float pairSnapping;
private double[] powers = {0,0,0,0}; private double[] powers = {0,0,0,0};
private double drivePower = 1; private double drivePower = 1;
private static final double TURN_POWER = 1; private static final double TURN_POWER = 1;
private boolean toggleSpeedInput = false;
private Launch launchState; private Launch launchState;
private boolean launching; private boolean launching;
private ProgressRingBelt ringBeltState; private ProgressRingBelt ringBeltState;
@@ -27,11 +28,12 @@ public class TeleOpState extends CyberarmState {
private boolean yPrev; private boolean yPrev;
private boolean aPrev; private boolean aPrev;
private boolean bPrev; private boolean bPrev;
private boolean rbPrev; private boolean lbPrev;
private boolean wobbleArmUp = true; private boolean wobbleArmUp = true;
private boolean wobbleGrabOpen = false; private boolean wobbleGrabOpen = false;
private boolean launchInput = false; private boolean launchInput = false;
@@ -40,8 +42,9 @@ public class TeleOpState extends CyberarmState {
} }
@Override @Override
public void start() { public void init() {
cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
} }
@Override @Override
@@ -57,14 +60,19 @@ public class TeleOpState extends CyberarmState {
double rightJoystickX = engine.gamepad1.right_stick_x; double rightJoystickX = engine.gamepad1.right_stick_x;
double rightJoystickY = engine.gamepad1.right_stick_y; double rightJoystickY = engine.gamepad1.right_stick_y;
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)); rightJoystickDegrees = snapToCardinal((float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)),cardinalSnapping,0);
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY); rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
if (leftJoystickMagnitude > 0.66) {
drivePower = 1 ; boolean lb = engine.gamepad1.left_stick_button;
} else { if (lb && !lbPrev) {
drivePower = 0.6; if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
} }
lbPrev = lb;
double[] powers = {0,0,0,0}; double[] powers = {0,0,0,0};
@@ -73,18 +81,18 @@ public class TeleOpState extends CyberarmState {
//Launch Sequence //Launch Sequence
double distanceToTarget = Math.hypot(Robot.LAUNCH_POSITION_X - robot.getLocationX(), Robot.LAUNCH_POSITION_Y - robot.getLocationY()); double distanceToTarget = Math.hypot(robot.launchPositionX - robot.getLocationX(), robot.launchPositionY - robot.getLocationY());
if (distanceToTarget > Robot.LAUNCH_TOLERANCE_POS) { if (distanceToTarget > Robot.LAUNCH_TOLERANCE_POS) {
powers = robot.getMecanumPowers(robot.getAngleToPosition(Robot.LAUNCH_POSITION_X, Robot.LAUNCH_POSITION_Y), drivePower, Robot.LAUNCH_ROTATION); powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.launchPositionX, robot.launchPositionY), drivePower, robot.launchRotation);
} else if (robot.getRelativeAngle(Robot.LAUNCH_ROTATION, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) { } else if (robot.getRelativeAngle(robot.launchRotation, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) {
launchState = new Launch(robot); launchState = new Launch(robot);
addParallelState(launchState); addParallelState(launchState);
} else { } else {
double[] facePowers = robot.getFacePowers(Robot.LAUNCH_ROTATION, TURN_POWER); double[] facePowers = robot.getFacePowers(robot.launchRotation, TURN_POWER);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} }
@@ -97,14 +105,16 @@ public class TeleOpState extends CyberarmState {
if (rightJoystickMagnitude == 0) { if (rightJoystickMagnitude == 0) {
if (leftJoystickMagnitude !=0) { if (leftJoystickMagnitude !=0) {
powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, leftJoystickDegrees); float direction = snapToCardinal(leftJoystickDegrees,cardinalSnapping,0);
powers = robot.getMecanumPowers(direction, drivePower, direction);
} }
} else { } else {
if (leftJoystickMagnitude == 0) { if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude); double[] facePowers = robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]}; powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else { } else {
powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, rightJoystickDegrees);
powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees,pairSnapping,rightJoystickDegrees), drivePower, rightJoystickDegrees);
} }
} }
} }
@@ -113,16 +123,15 @@ public class TeleOpState extends CyberarmState {
this.powers = powers; this.powers = powers;
if (childrenHaveFinished()) {
robot.collectionMotor.setPower(engine.gamepad2.right_trigger);
} else {
robot.collectionMotor.setPower(0);
}
boolean x = engine.gamepad2.x; boolean x = engine.gamepad2.x;
if (x && !xPrev && childrenHaveFinished()) { if (x && !xPrev && childrenHaveFinished()) {
if (CollectorOn) { addParallelState(new ProgressRingBelt(robot));
robot.collectionMotor.setPower(0);
CollectorOn = false;
} else {
robot.collectionMotor.setPower(1);
CollectorOn = true;
}
} }
xPrev = x; xPrev = x;
@@ -135,41 +144,48 @@ public class TeleOpState extends CyberarmState {
boolean a = engine.gamepad2.a; boolean a = engine.gamepad2.a;
if (a && !aPrev && childrenHaveFinished()) { if (a && !aPrev && childrenHaveFinished()) {
addParallelState(new ProgressRingBelt(robot)); wobbleGrabOpen = !wobbleGrabOpen;
addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100));
} }
aPrev = a; aPrev = a;
boolean b = engine.gamepad2.b; boolean b = engine.gamepad2.b;
if (b && !bPrev) { if (b && !bPrev && childrenHaveFinished()) {
wobbleArmUp = !wobbleArmUp; wobbleArmUp = !wobbleArmUp;
addParallelState(new WobbleArm(robot, wobbleArmUp, 100)); addParallelState(new WobbleArm(robot, wobbleArmUp, 100));
} }
bPrev = b; bPrev = b;
boolean rb = engine.gamepad2.right_bumper;
if (rb && !rbPrev) {
wobbleGrabOpen = !wobbleGrabOpen ;
addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100));
}
rbPrev = rb;
} }
@Override @Override
public void telemetry() { public void telemetry() {
engine.telemetry.addLine("Powers");
for (double power : powers) {
engine.telemetry.addData(" ", power);
}
engine.telemetry.addData("childrenHaveFinished", childrenHaveFinished());
for (CyberarmState state : children) {
engine.telemetry.addLine(""+state.getClass());
}
engine.telemetry.addLine("Location"); engine.telemetry.addLine("Location");
engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()))+","+round(robot.ticksToInches(robot.getLocationY()))+")"); 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("Rotation ", robot.getRotation());
engine.telemetry.addData("totalV", robot.totalV); engine.telemetry.addData("totalV", robot.totalV);
} }
private float round(double number) { private float round(double number,double unit) {
return ((float) Math.floor(number*100)) / 100; return (float) (Math.floor(number/unit) * unit);
}
private float snapToCardinal(float angle, float snapping, float offset) {
int o = (int) offset + 180;
o %= 90;
for (int cardinal = o-180; (cardinal <= 180+o && cardinal != angle); cardinal += 90) {
if (angle >= cardinal-snapping && angle <= cardinal+snapping) {
angle = cardinal;
}
}
return angle;
} }

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

@@ -17,9 +17,9 @@ public class WobbleGrab extends CyberarmState {
this.open = open; this.open = open;
} }
public WobbleGrab(Robot robot, boolean armUp, long waitTime) { public WobbleGrab(Robot robot, boolean open, long waitTime) {
this.robot = robot; this.robot = robot;
this.open = armUp; this.open = open;
this.waitTime = waitTime; this.waitTime = waitTime;
} }