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-02-06 12:15:36 -06:00
parent ef3736440b
commit 1810e70eb1
30 changed files with 997 additions and 459 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/Break.java
View File

@@ -1,43 +0,0 @@
package org.timecrafters.UltimateGoal.Calibration;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import org.cyberarm.engine.V2.CyberarmState;
public class Break extends CyberarmState {
public DcMotor driveBackLeft;
public DcMotor driveFrontLeft;
public DcMotor driveBackRight;
public DcMotor driveFrontRight;
@Override
public void init() {
driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft");
driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight");
driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft");
driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight");
driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
driveFrontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveFrontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveBackLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
driveBackRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
@Override
public void start() {
}
@Override
public void exec() {
driveFrontLeft.setPower(0);
driveFrontRight.setPower(0);
driveBackLeft.setPower(0);
driveBackRight.setPower(0);
}
}

58
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/CalibrateRingBeltLoop.java Normal file
View File

@@ -0,0 +1,58 @@
package org.timecrafters.UltimateGoal.Calibration;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class CalibrateRingBeltLoop extends CyberarmState {
private Robot robot;
private float startingTick;
private int currentTick;
private double ticksPerLoop;
private boolean limit;
private boolean hasSeenLimit;
private boolean limitPrevious;
public CalibrateRingBeltLoop(Robot robot) {
this.robot = robot;
}
@Override
public void exec() {
currentTick = robot.ringBeltMotor.getCurrentPosition();
limit = robot.limitSwitch.isPressed();
if (engine.gamepad1.x || (engine.gamepad1.a && !limit)) {
robot.ringBeltMotor.setPower(0.5);
} else {
robot.ringBeltMotor.setPower(0);
}
if (engine.gamepad1.y) {
robot.ringBeltMotor.setPower(-0.1);
} else {
robot.ringBeltMotor.setPower(0);
}
if (engine.gamepad1.b) {
startingTick = currentTick;
}
if (limit && !limitPrevious){
if (hasSeenLimit) {
ticksPerLoop = currentTick - startingTick;
}
startingTick = currentTick;
hasSeenLimit = true;
}
limitPrevious = limit;
}
@Override
public void telemetry() {
engine.telemetry.addData("ticks", currentTick - startingTick);
engine.telemetry.addData("limit swich", limit );
engine.telemetry.addData("Clockwise", ticksPerLoop);
}
}

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

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

59
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Calibration/MotorCheck.java Normal file
View File

@@ -0,0 +1,59 @@
package org.timecrafters.UltimateGoal.Calibration;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Engagable;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.DriveMotor;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class MotorCheck extends CyberarmState {
private Robot robot;
private double[] Powers;
private DcMotor[] Motors;
private int currentMotor = 0;
private double currentPower = 0.001;
private boolean yPrevious;
public MotorCheck(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
}
@Override
public void start() {
Motors[currentMotor].setPower(0.2);
}
@Override
public void exec() {
if (currentMotor <= 3) {
boolean y = engine.gamepad1.y;
if (y && !yPrevious) {
Motors[currentMotor].setPower(0);
currentMotor += 1;
Motors[currentMotor].setPower(0.2);
sleep(20);
}
yPrevious = y;
}
}
@Override
public void telemetry() {
engine.telemetry.addData("current Motor", Motors[currentMotor].toString());
}
}

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

@@ -6,10 +6,13 @@ import org.timecrafters.UltimateGoal.Competition.Robot;
public class OdometryCalibration extends CyberarmState {
private Robot robot;
private float rotation;
private float rotation = 0;
private float rotationPrev = 0;
private int currentTick;
private double ticksPerDegreeClockwise;
private double ticksPerDegreeCounterClockwise;
private long timePrevious;
private long timeChange;
public OdometryCalibration(Robot robot) {
this.robot = robot;
@@ -17,21 +20,37 @@ public class OdometryCalibration extends CyberarmState {
@Override
public void exec() {
long timeCurrent = System.currentTimeMillis();
timeChange = timeCurrent - timePrevious;
double power = 0.1;
rotation = -robot.imu.getAngularOrientation().firstAngle;
currentTick = robot.encoderBack.getCurrentPosition();
if (timeChange >= 1200) {
timePrevious = timeCurrent;
if (engine.gamepad1.x) {
robot.setDrivePower(power, -power, power, -power);
ticksPerDegreeClockwise = currentTick/rotation;
} else if(engine.gamepad1.y) {
robot.setDrivePower(-power, power, -power, power);
ticksPerDegreeCounterClockwise = currentTick/rotation;
} else {
robot.setDrivePower(0,0,0,0);
double power = 0.25;
float imu = -robot.imu.getAngularOrientation().firstAngle;
rotation += robot.getRelativeAngle(imu, rotationPrev);
rotationPrev = imu;
currentTick = robot.encoderBack.getCurrentPosition();
if (engine.gamepad1.x) {
robot.setDrivePower(power, -power, power, -power);
ticksPerDegreeClockwise = currentTick / rotation;
} else if (engine.gamepad1.y) {
robot.setDrivePower(-power, power, -power, power);
ticksPerDegreeCounterClockwise = currentTick / rotation;
} else {
robot.setDrivePower(0, 0, 0, 0);
}
}
if (engine.gamepad1.b) {
robot.record("Clockwise : "+ticksPerDegreeClockwise);
robot.record("Counter Clockwise : "+ticksPerDegreeCounterClockwise);
robot.saveRecording();
setHasFinished(true);
}
}
@Override

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

@@ -19,7 +19,7 @@ public class PerformanceTest extends CyberarmState {
@Override
public void exec() {
robot.updateLocation();
robot.record();
// robot.record();
if (engine.gamepad1.x) {
double[] powers = robot.getMecanumPowers(0, 1, 0);

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

@@ -1,70 +0,0 @@
package org.timecrafters.UltimateGoal.Calibration;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class StalPowerCalibrator extends CyberarmState {
private Robot robot;
private double[] Powers;
private DcMotor[] Motors;
private int currentMotor = 0;
private double currentPower = 0.001;
public StalPowerCalibrator(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
// Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
Powers = new double[] {0,0,0,0};
for (DcMotor motor : Motors) {
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
}
@Override
public void exec() {
if (currentMotor <= 3) {
if (Math.abs(Motors[currentMotor].getCurrentPosition()) < 10 && currentPower < 1) {
currentPower += 0.001;
Motors[currentMotor].setPower(currentPower);
sleep(20);
} else {
Powers[currentMotor] = currentPower;
Motors[currentMotor].setPower(0);
currentPower = 0.001;
currentMotor += 1;
sleep(1000);
for (DcMotor motor : Motors) {
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("current Power", currentPower);
engine.telemetry.addData("current Motor", currentMotor);
engine.telemetry.addData("FrontLeft", Powers[0]);
engine.telemetry.addData("FrontRight", Powers[1]);
engine.telemetry.addData("BackLeft", Powers[2]);
engine.telemetry.addData("BackRight", Powers[3]);
}
}

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

@@ -1,49 +0,0 @@
package org.timecrafters.UltimateGoal.Calibration;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.DriveMotor;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class VelocityTest extends CyberarmState {
private Robot robot;
private double[] Velocities;
private DriveMotor[] Motors;
private long LastTime = 0;
public VelocityTest(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
Motors = new DriveMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
Velocities = new double[] {0,0,0,0};
}
@Override
public void start() {
for (DriveMotor motor : Motors) {
motor.setPower(0.1);
}
}
@Override
public void exec() {
for (int i = 0; i < 4; i++) {
Velocities[i] = (Motors[i].motor.getCurrentPosition()* 60000) / runTime();
}
}
@Override
public void telemetry() {
engine.telemetry.addData("FrontLeft", Velocities[0]);
engine.telemetry.addData("FrontRight", Velocities[1]);
engine.telemetry.addData("BackLeft", Velocities[2]);
engine.telemetry.addData("BackRight", Velocities[3]);
}
}

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

@@ -1,4 +1,27 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
public class AutoEngine {
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
@Autonomous (name = "Autonomous")
public class AutoEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
super.init();
}
@Override
public void setup() {
//drive to view
addState(new DriveToCoordinates(robot, "auto", "001_0"));
addState(new DriveToCoordinates(robot, "auto", "001_0"));
}
}

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

@@ -11,11 +11,12 @@ public class DriveToCoordinates extends CyberarmState {
private double xTarget;
private double yTarget;
private float faceAngle;
private double tolerance;
private double tolerancePos;
private double power;
private boolean braking;
private long breakStartTime;
private long breakTime;
private boolean autoFace;
public DriveToCoordinates(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -23,11 +24,12 @@ public class DriveToCoordinates extends CyberarmState {
this.actionName = actionName;
}
public DriveToCoordinates(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 breakStartTime) {
this.robot = robot;
this.xTarget = xTarget;
this.yTarget = yTarget;
this.faceAngle = faceAngle;
this.tolerance = tolerance;
this.tolerancePos = tolerance;
this.power = power;
this.breakTime = breakTime;
}
@@ -35,18 +37,25 @@ public class DriveToCoordinates extends CyberarmState {
@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());
xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
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();
tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
try {
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
} catch (NullPointerException e) {
autoFace = true;
}
}
}
@Override
public void start() {
if (autoFace) {
faceAngle = robot.getAngleToPosition(xTarget,yTarget);
}
}
@Override
@@ -54,18 +63,17 @@ public class DriveToCoordinates extends CyberarmState {
double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
if (distanceToTarget > tolerance) {
if (distanceToTarget > tolerancePos) {
double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
braking = false;
} else {
long currentTime = System.currentTimeMillis();
if (!braking) {
breakStartTime = System.currentTimeMillis();
robot.setBrakePosAll();
breakStartTime = currentTime;
braking = true;
}
robot.brakeAll();
setHasFinished(System.currentTimeMillis() - breakStartTime > breakTime);
setHasFinished(currentTime - breakStartTime >= breakTime);
}
}

66
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/DriveToPosition.java
View File

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

73
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Face.java Normal file
View File

@@ -0,0 +1,73 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class Face extends CyberarmState {
private Robot robot;
private String groupName = "manual";
private String actionName;
private float faceAngle;
private double toleranceFace;
private double power;
private boolean braking;
private long breakStartTime;
private long breakTime;
private boolean autoFace;
public Face(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
}
public Face(Robot robot, float faceAngle, double toleranceFace, double power, long breakTime) {
this.robot = robot;
this.faceAngle = faceAngle;
this.toleranceFace = toleranceFace;
this.power = power;
this.breakTime = breakTime;
}
@Override
public void init() {
if (!groupName.equals("manual")) {
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
breakTime = robot.stateConfiguration.variable(groupName, actionName, "brake MS").value();
try {
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value();
} catch (NullPointerException e) {
autoFace = true;
}
}
}
@Override
public void start() {
if (autoFace) {
double x = robot.stateConfiguration.variable(groupName, actionName, "faceX").value();
double y = robot.stateConfiguration.variable(groupName, actionName, "faceY").value();
faceAngle = robot.getAngleToPosition(x,y);
}
}
@Override
public void exec() {
if (Math.abs(robot.getRelativeAngle(faceAngle,robot.getRotation())) > toleranceFace) {
double[] powers = robot.getFacePowers(faceAngle,power);
robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]);
braking = false;
} else {
long currentTime = System.currentTimeMillis();
if (!braking) {
breakStartTime = currentTime;
braking = true;
}
setHasFinished(currentTime - breakStartTime >= breakTime);
}
}
}

59
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/TensorFlowCheck.java Normal file
View File

@@ -0,0 +1,59 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.util.ArrayList;
import java.util.List;
public class TensorFlowCheck extends CyberarmState {
private Robot robot;
private String group;
private String action;
private List<Recognition> recognitions;
private int path = 0;
private long checkTime;
public double wobblePosX;
public double wobblePosY;
@Override
public void init() {
checkTime = robot.stateConfiguration.variable(group,action,"time").value();
}
@Override
public void start() {
}
@Override
public void exec() {
recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
if (recognitions != null) {
if (recognitions.size() == 1) {
Recognition recognition = recognitions.get(0);
String label = recognition.getLabel();
if (label.equals("Single")) {
path = 1;
} else if (label.equals("Quad")) {
path = 2;
}
} else if (recognitions.size() > 1) {
path = 1;
}
}
if (runTime() >= checkTime) {
if (checkTime == 0) {
wobblePosX = robot.stateConfiguration.variable("auto", "_goalPos1","x").value();
}
}
}
}

47
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java Normal file
View File

@@ -0,0 +1,47 @@
package org.timecrafters.UltimateGoal.Competition;
import org.cyberarm.engine.V2.CyberarmState;
public class CamServo extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private boolean open;
private long waitTime = -1;
public CamServo(Robot robot, String groupName, String actionName, boolean open) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.open = open;
}
public CamServo(Robot robot, boolean armUp, long waitTime) {
this.robot = robot;
this.open = armUp;
this.waitTime = waitTime;
}
@Override
public void init() {
if (waitTime == -1) {
waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
}
}
@Override
public void start() {
if (open) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
} else {
robot.wobbleGrabServo.setPosition(0);
}
}
@Override
public void exec() {
setHasFinished(runTime() > waitTime);
}
}

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

@@ -10,7 +10,7 @@ public class DriveMotor {
private int brakePosition;
private MotorConfigurationType motorType;
private DcMotorSimple.Direction direction;
static final double FINE_CORRECTION = 0.02;
static final double FINE_CORRECTION = 0.05;
static final double LARGE_CORRECTION = 0.03;
public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) {
@@ -20,9 +20,9 @@ public class DriveMotor {
}
public void init() {
motor.setMotorType(motorType);
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// motor.setMotorType(motorType);
// motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
motor.setDirection(direction);
}

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

@@ -1,4 +1,39 @@
package org.timecrafters.UltimateGoal.Competition;
public class Launch {
import org.cyberarm.engine.V2.CyberarmState;
public class Launch extends CyberarmState {
private Robot robot;
boolean hasCycled;
boolean detectedPass;
public Launch(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
robot.ringBeltOn();
}
@Override
public void exec() {
//detect when limit switch is initially triggered
boolean detectingPass = robot.limitSwitch.isPressed();
if (detectingPass && !detectedPass) {
//finish once the ring belt has cycled all the way through and then returned to
//the first receiving position.
if (hasCycled) {
robot.ringBeltMotor.setPower(0);
robot.ringBeltStage = 0;
setHasFinished(true);
} else {
hasCycled = true;
}
}
detectedPass = detectingPass;
// if (robot.getBeltPos() > robot.loopPos(Robot.RING_BELT_GAP * 3) && hasCycled);
}
}

26
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java Normal file
View File

@@ -0,0 +1,26 @@
package org.timecrafters.UltimateGoal.Competition.PreInit;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class FindLimitSwitch extends CyberarmState {
private Robot robot;
public FindLimitSwitch(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
robot.ringBeltOn();
}
@Override
public void exec() {
if (robot.limitSwitch.isPressed()) {
robot.ringBeltMotor.setPower(0);
setHasFinished(true);
}
}
}

30
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java Normal file
View File

@@ -0,0 +1,30 @@
package org.timecrafters.UltimateGoal.Competition.PreInit;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class LoadRings extends CyberarmState {
private Robot robot;
private ProgressRingBelt ringBeltState;
private int ringCount = 0;
public LoadRings(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
robot.collectionMotor.setPower(1);
}
@Override
public void exec() {
ringBeltState = new ProgressRingBelt(robot);
if (engine.gamepad1.x && childrenHaveFinished()) {
addParallelState(ringBeltState);
}
}
}

25
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/PreInitEngine.java Normal file
View File

@@ -0,0 +1,25 @@
package org.timecrafters.UltimateGoal.Competition.PreInit;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
@Autonomous (name = "Load Rings")
public class PreInitEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
super.init();
}
@Override
public void setup() {
addState(new FindLimitSwitch(robot));
addState(new LoadRings(robot));
}
}

49
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java Normal file
View File

@@ -0,0 +1,49 @@
package org.timecrafters.UltimateGoal.Competition;
import org.cyberarm.engine.V2.CyberarmState;
public class ProgressRingBelt extends CyberarmState {
private Robot robot;
private int targetPos;
private boolean useLaunchPrep;
public ProgressRingBelt(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
useLaunchPrep = (robot.launchMotor.getPower() == 0);
}
@Override
public void start() {
int currentPos = robot.getBeltPos();
if (robot.ringBeltStage < 2) {
targetPos = robot.loopPos(currentPos + Robot.RING_BELT_GAP);
robot.ringBeltOn();
robot.ringBeltStage += 1;
} else if (robot.ringBeltStage == 2) {
targetPos = robot.loopPos(currentPos + 160);
robot.ringBeltOn();
robot.ringBeltStage += 1;
}
}
@Override
public void exec() {
int currentPos = robot.getBeltPos();
if (currentPos >= targetPos && currentPos < targetPos + Robot.RING_BELT_GAP) {
robot.ringBeltMotor.setPower(0);
if(useLaunchPrep) {
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
}
setHasFinished(true);
}
}
}

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

@@ -4,10 +4,12 @@ import android.os.Environment;
import android.util.Log;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevTouchSensor;
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.Servo;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
@@ -35,7 +37,7 @@ import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
public class Robot {
public class Robot {
private HardwareMap hardwareMap;
@@ -46,34 +48,29 @@ public class Robot {
public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
public BNO055IMU imu;
private WebcamName webcam;
private VuforiaLocalizer vuforia;
//drive system
public DriveMotor driveFrontLeft;
public DriveMotor driveBackLeft;
public DriveMotor driveFrontRight;
public DriveMotor driveBackRight;
public DcMotor driveFrontLeft;
public DcMotor driveBackLeft;
public DcMotor driveFrontRight;
public DcMotor driveBackRight;
public DcMotor encoderLeft;
public DcMotor encoderRight;
public DcMotor encoderBack;
//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 FINE_CORRECTION = 0.055 ;
static final double LARGE_CORRECTION = 0.025 ;
static final double MOMENTUM_CORRECTION = 1.02;
static final double MOMENTUM_MAX_CORRECTION = 1.3;
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 double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
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_CLOCKWISE = Math.PI * 1000;
static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = Math.PI * 1000;
static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4;
static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6;
// Inches Forward of axis of rotation
static final float CAMERA_FORWARD_DISPLACEMENT = 13f;
@@ -95,85 +92,117 @@ public class Robot {
//Launcher
public DcMotor launchMotor;
public static final double LAUNCH_POWER = 0.75;
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);
private static final long LAUNCH_ACCEL_TIME = 500;
public static final double LAUNCH_POSITION_X = 36 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
public static final double LAUNCH_POSITION_Y = -8 * (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_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
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_GAP = 200;
public static final int RING_BELT_RECEIVE_POS = 100;
//Ring Intake
public DcMotor collectionMotor;
public Rev2mDistanceSensor ringIntakeSensor;
public static final double RING_DETECT_DISTANCE = 100;
public static final double RING_DETECT_DELAY = 1000;
//Ring Belt
public DcMotor ringBeltMotor;
public RevTouchSensor limitSwitch;
public int ringBeltStage;
public static final int RING_BELT_LOOP_TICKS = 2544;
public static final int RING_BELT_GAP = 670;
//Debugging
public double visionX;
public double visionY;
public float rawAngle;
// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
private String TestingRecord = "AngularVelocity";
public static final double RING_BELT_POWER = 0.2;
public double traveledLeft;
public double traveledRight;
//Wobble Goal Arm
public DcMotor wobbleArmMotor;
public Servo wobbleGrabServo;
public static final int WOBBLE_ARM_DOWN = -710;
public static final double WOBBLE_SERVO_MAX = 0.3;
//vuforia navigation
private WebcamName webcam;
private VuforiaLocalizer vuforia;
public Servo webCamServo;
public static final double CAM_SERVO_UP = 0.2;
public static final double CAM_SERVO_Down = 0.2;
public 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;
private long minCheckDurationMs = 500;
private int minCheckVelocity = 1;
//TensorFlow Object Detection
public TFObjectDetector tfObjectDetector;
private static final float MINIMUM_CONFIDENCE = 0.8f;
//Debugging
public double totalV;
public double visionX;
public double visionY;
public float rawAngle;
// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
private String TestingRecord = "TicksPerDegree";
public double traveledLeft;
public double traveledRight;
public void initHardware() {
// webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
imu = hardwareMap.get(BNO055IMU.class, "imu");
// limitSwitch.setMode(DigitalChannel.Mode.INPUT);
limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
DcMotor dcMotor = hardwareMap.dcMotor.get("driveFrontLeft");
driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
MotorConfigurationType motorType = dcMotor.getMotorType();
motorType.setGearing(5);
motorType.setTicksPerRev(140);
motorType.setMaxRPM(800);
driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
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);
//Localization encoders
encoderRight = hardwareMap.dcMotor.get("driveFrontRight");
encoderBack = hardwareMap.dcMotor.get("driveFrontLeft");
encoderLeft = hardwareMap.dcMotor.get("collect");
driveFrontLeft.init();
driveFrontRight.init();
driveBackLeft.init();
driveBackRight.init();
encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
// encoderLeft = hardwareMap.dcMotor.get("odoLeft");
// encoderRight = hardwareMap.dcMotor.get("odoRight");
// encoderBack = hardwareMap.dcMotor.get("odoBack");
//init wobble arm
wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm");
wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
wobbleArmMotor.setTargetPosition(0);
wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
launchMotor = hardwareMap.dcMotor.get("launcher");
wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
//init ring belt
collectionMotor = hardwareMap.dcMotor.get("collect");
collectionMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
//init ring belt
ringBeltMotor = hardwareMap.dcMotor.get("belt");
ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
//init IMU
imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
@@ -196,18 +225,39 @@ public class Robot {
imu.startAccelerationIntegration(startPosition,startVelocity, 10);
// initVuforia();
//Init Localization
webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
initVuforia();
webCamServo = hardwareMap.servo.get("look");
webCamServo.setDirection(Servo.Direction.REVERSE );
rotation = stateConfiguration.variable("system", "startPos", "direction").value();
locationX = stateConfiguration.variable("system", "startPos", "x").value();
locationY = stateConfiguration.variable("system", "startPos", "y").value();
// double launcherPower = 0;
// long launchAccelStart = System.currentTimeMillis();
// while (launcherPower < 1) {
// launcherPower = (double) (System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME;
// launchMotor.setPower(launcherPower);
// }
minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value();
minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value();
//Init Launch Motor
DcMotor launcher = hardwareMap.dcMotor.get("launcher");
MotorConfigurationType launchMotorType = launcher.getMotorType();
launchMotorType.setGearing(3);
launchMotorType.setTicksPerRev(84);
launchMotorType.setMaxRPM(2400);
launchMotor = launcher;
launchMotor.setMotorType(launchMotorType);
launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
if (stateConfiguration.action("system","initLauncher").enabled) {
double launcherPower = 0;
long launchAccelStart = System.currentTimeMillis();
while (launcherPower < LAUNCH_POWER) {
launcherPower = (double) ((System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME) * LAUNCH_POWER;
launchMotor.setPower(launcherPower);
}
}
}
@@ -267,7 +317,7 @@ public class Robot {
int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
parameters.minResultConfidence = MINIMUM_CONFIDENCE;
parameters.minResultConfidence = stateConfiguration.variable("system", "tensorFlow", "minConfidence").value();
tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
}
@@ -279,9 +329,8 @@ public class Robot {
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 encoderLeftCurrent = -encoderLeft.getCurrentPosition();
int encoderRightCurrent = -encoderRight.getCurrentPosition();
int encoderBackCurrent = encoderBack.getCurrentPosition();
double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
@@ -309,8 +358,8 @@ public class Robot {
} else {
ticksPerDegree = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
}
//todo: check if direction of the back encoder gives expected values
double sidewaysVector = encoderBackChange - (rotationChange* ticksPerDegree);
double sidewaysVector = encoderBackChange + (rotationChange* ticksPerDegree);
double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
double direction = Math.toRadians(rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
@@ -324,14 +373,15 @@ public class Robot {
rotation += rotationChange;
double totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightCurrent) + Math.abs(encoderBackChange);
totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
if (totalV < MIN_CHECK_VELOCITY) {
if (totalV < minCheckVelocity) {
long timeCurrent = System.currentTimeMillis();
if (timeStartZeroVelocity == 0) {
timeStartZeroVelocity = timeCurrent;
} else if (timeCurrent - timeStartZeroVelocity >= MIN_CHECK_DURATION_MS) {
} else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) {
syncWithVuforia();
}
@@ -349,15 +399,6 @@ 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) {
@@ -480,23 +521,23 @@ public class Robot {
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;
// //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);
double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
powerForwardRight *= momentumCorrection;
powerForwardLeft *= momentumCorrection;
powerBackRight *= momentumCorrection;
@@ -532,21 +573,6 @@ public class Robot {
return powers;
}
public void brakeAll() {
driveFrontLeft.brake();
driveFrontRight.brake();
driveBackLeft.brake();
driveBackRight.brake();
}
//sets the position the motors will lock to when braking to the motor's current position.
public void setBrakePosAll() {
driveFrontLeft.setBrakePosition();
driveFrontRight.setBrakePosition();
driveBackLeft.setBrakePosition();
driveBackRight.setBrakePosition();
}
//Outputs the power necessary to turn and face a provided direction
public double[] getFacePowers(float direction, double power) {
angularVelocity = imu.getAngularVelocity().xRotationRate;
@@ -568,20 +594,21 @@ public class Robot {
return powers;
}
public void ringBeltOn() {
ringBeltMotor.setPower(RING_BELT_POWER);
}
public int getBeltPos(){
return loopPos(ringBeltMotor.getCurrentPosition());
}
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;
public void record(String record) {
TestingRecord+="\n"+record;
}
public void saveRecording() {

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

@@ -1,10 +1,32 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
@TeleOp (name = "TeleOp",group = "comp")
public class TeleOpEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
robot.wobbleGrabServo.setPosition(0);
robot.webCamServo.setPosition(0);
super.init();
}
@Override
public void setup() {
addState(new TeleOpState(robot));
}
@Override
public void stop() {
robot.deactivateVuforia();
super.stop();
}
}

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

@@ -1,8 +1,11 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.WobbleArm;
import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
public class TeleOpState extends CyberarmState {
@@ -12,14 +15,24 @@ public class TeleOpState extends CyberarmState {
private float rightJoystickDegrees;
private double rightJoystickMagnitude;
private double POWER_SPRINT = 0.4;
private double POWER_NORMAL = 0.2;
private double powerScale = 0.2 ;
private double[] powers = {0,0,0,0};
private double drivePower = 1;
private static final double TURN_POWER = 1;
private boolean toggleSpeedInput = false;
private Launch launchState;
private boolean launching;
private ProgressRingBelt ringBeltState;
private boolean CollectorOn;
private boolean xPrev;
private boolean yPrev;
private boolean aPrev;
private boolean bPrev;
private boolean rbPrev;
private boolean wobbleArmUp = true;
private boolean wobbleGrabOpen = 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) {
@@ -47,46 +60,117 @@ public class TeleOpState extends CyberarmState {
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
if (leftJoystickMagnitude > 0.66) {
drivePower = 1 ;
} else {
drivePower = 0.6;
}
double[] powers = {0,0,0,0};
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);
boolean y = engine.gamepad1.y;
if (y) {
} else if (robot.getRelativeAngle(robot.LAUNCH_ROTATION, robot.getRotation()) > robot.LAUNCH_TOLERANCE_FACE) {
//todo add launch sequence
//Launch Sequence
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), drivePower, Robot.LAUNCH_ROTATION);
} else if (robot.getRelativeAngle(Robot.LAUNCH_ROTATION, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) {
launchState = new Launch(robot);
addParallelState(launchState);
} else {
double[] facePowers = robot.getFacePowers(Robot.LAUNCH_ROTATION, TURN_POWER);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
}
} 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 (!y || ( launchState != null && launchState.getHasFinished())) {
//Normal Driver Controls
if (rightJoystickMagnitude == 0) {
if (leftJoystickMagnitude !=0) {
powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, leftJoystickDegrees);
powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, leftJoystickDegrees);
}
} else {
if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(rightJoystickDegrees, powerScale * rightJoystickMagnitude);
double[] facePowers = robot.getFacePowers(rightJoystickDegrees, TURN_POWER * rightJoystickMagnitude);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {
powers = robot.getMecanumPowers(leftJoystickDegrees, powerScale * leftJoystickMagnitude, rightJoystickDegrees);
powers = robot.getMecanumPowers(leftJoystickDegrees, drivePower, rightJoystickDegrees);
}
}
}
robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
this.powers = powers;
boolean x = engine.gamepad2.x;
if (x && !xPrev && childrenHaveFinished()) {
if (CollectorOn) {
robot.collectionMotor.setPower(0);
CollectorOn = false;
} else {
robot.collectionMotor.setPower(1);
CollectorOn = true;
}
}
xPrev = x;
boolean y2 = engine.gamepad2.y;
if (y2 && !yPrev && childrenHaveFinished()) {
launchState = new Launch(robot);
addParallelState(launchState);
}
yPrev = y2;
boolean a = engine.gamepad2.a;
if (a && !aPrev && childrenHaveFinished()) {
addParallelState(new ProgressRingBelt(robot));
}
aPrev = a;
boolean b = engine.gamepad2.b;
if (b && !bPrev) {
wobbleArmUp = !wobbleArmUp;
addParallelState(new WobbleArm(robot, wobbleArmUp, 100));
}
bPrev = b;
boolean rb = engine.gamepad2.right_bumper;
if (rb && !rbPrev) {
wobbleGrabOpen = !wobbleGrabOpen ;
addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100));
}
rbPrev = rb;
}
@Override
public void telemetry() {
engine.telemetry.addLine("Powers");
for (double power : powers) {
engine.telemetry.addData(" ", power);
}
engine.telemetry.addLine("Location");
engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()))+","+round(robot.ticksToInches(robot.getLocationY()))+")");
engine.telemetry.addData("Rotation ", robot.getRotation());
engine.telemetry.addData("totalV", robot.totalV);
}
private float round(double number) {
return ((float) Math.floor(number*100)) / 100;
}
}

48
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleArm.java Normal file
View File

@@ -0,0 +1,48 @@
package org.timecrafters.UltimateGoal.Competition;
import org.cyberarm.engine.V2.CyberarmState;
public class WobbleArm extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private boolean armUp;
private long waitTime = -1;
public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.armUp = armUp;
}
public WobbleArm(Robot robot, boolean armUp, long waitTime) {
this.robot = robot;
this.armUp = armUp;
this.waitTime = waitTime;
}
@Override
public void init() {
if (waitTime == -1) {
waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
}
}
@Override
public void start() {
robot.wobbleArmMotor.setPower(0.3);
if (armUp) {
robot.wobbleArmMotor.setTargetPosition(0);
} else {
robot.wobbleArmMotor.setTargetPosition(Robot.WOBBLE_ARM_DOWN);
}
}
@Override
public void exec() {
setHasFinished(runTime() > waitTime);
}
}

47
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/WobbleGrab.java Normal file
View File

@@ -0,0 +1,47 @@
package org.timecrafters.UltimateGoal.Competition;
import org.cyberarm.engine.V2.CyberarmState;
public class WobbleGrab extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private boolean open;
private long waitTime = -1;
public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.open = open;
}
public WobbleGrab(Robot robot, boolean armUp, long waitTime) {
this.robot = robot;
this.open = armUp;
this.waitTime = waitTime;
}
@Override
public void init() {
if (waitTime == -1) {
waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
}
}
@Override
public void start() {
if (open) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
} else {
robot.wobbleGrabServo.setPosition(0);
}
}
@Override
public void exec() {
setHasFinished(runTime() > waitTime);
}
}

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

@@ -1,39 +0,0 @@
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() {
}
}

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

@@ -49,7 +49,7 @@ public class MecanumFunctionTest extends CyberarmState {
@Override
public void exec() {
robot.updateLocation();
robot.record();
// robot.record();/////////////
AngularVelocity angularVelocity = robot.imu.getAngularVelocity();
@@ -129,9 +129,9 @@ public class MecanumFunctionTest extends CyberarmState {
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());
engine.telemetry.addData("FL", robot.driveFrontLeft.getPower());
engine.telemetry.addData("FR", robot.driveFrontRight.getPower());
engine.telemetry.addData("BL", robot.driveBackLeft.getPower());
engine.telemetry.addData("BR", robot.driveBackRight.getPower());
}
}

36
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java Normal file
View File

@@ -0,0 +1,36 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.robotcore.hardware.Servo;
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.Competition.Robot;
public class ServoPosTest extends CyberarmState {
private Servo servo;
private double servoPos;
public ServoPosTest() {
}
@Override
public void init() {
servo = engine.hardwareMap.servo.get("look");
servo.setDirection(Servo.Direction.REVERSE );
}
@Override
public void exec() {
servoPos = engine.gamepad1.right_stick_y * 0.2;
servo.setPosition(servoPos);
}
@Override
public void telemetry() {
engine.telemetry.addData("Position", servoPos);
}
}

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

@@ -8,19 +8,19 @@ import org.timecrafters.UltimateGoal.Competition.Robot;
@TeleOp (name = "Hardware test", group = "test")
public class TestingEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
super.init();
}
// private Robot robot;
// @Override
// public void init() {
// robot = new Robot(hardwareMap);
// robot.initHardware();
// super.init();
// }
@Override
public void setup() {
addState(new FullTest(robot));
addState(new ServoPosTest());
}
//
// @Override
// public void stop() {
// robot.saveRecording();

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

@@ -0,0 +1,30 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState;
public class WobbleArmTest extends CyberarmState {
private int currentPos;
private DcMotor motor;
@Override
public void init() {
motor = engine.hardwareMap.dcMotor.get("wobbleArm");
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
@Override
public void exec() {
currentPos = motor.getCurrentPosition();
}
@Override
public void telemetry() {
engine.telemetry.addData("Position", currentPos);
}
}