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

End game Autononmous

Browse Source
This commit is contained in:
Nathaniel Palme
2021-04-02 19:30:29 -05:00
parent afcd9f0e62
commit 3b15aa0ee8
20 changed files with 342 additions and 530 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,10 +1,12 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.Pause;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.WobbleArm;
import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
@@ -21,26 +23,19 @@ public class AutoEngine extends CyberarmEngine {
private double launchPower;
private long launchBrakeTime;
private float scoreAFaceAngle;
private double scoreATolerance;
private double scoreAPower;
private long scoreABrakeTime;
double parkY;
float parkFaceAngle;
double parkTolerance;
double parkPower;
long parkBrakeTime;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN);
robot.wobbleGrabServo.setPosition(0.1 * Robot.WOBBLE_SERVO_MAX);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
robot.wobbleArmMotor.setTargetPosition(0);
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// since we've preloaded three rings, the ring belt stage is set to account for this;
robot.ringBeltStage = 3;
//Autonomous specific Variables
float rotation = robot.stateConfiguration.variable("system", "startPos", "direction").value();
double locationX = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "x").value());
double locationY = robot.inchesToTicks((double) robot.stateConfiguration.variable("system", "startPos", "y").value());
@@ -50,17 +45,6 @@ public class AutoEngine extends CyberarmEngine {
launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();
// scoreAFaceAngle = robot.stateConfiguration.variable("auto","05_0","face").value();
// scoreATolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","05_0","tolPos").value());
// scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value();
// scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","brakeMS").value();
//
parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","yPos").value());
parkFaceAngle = robot.stateConfiguration.variable("auto","13_0","face").value();
parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","13_0","tolPos").value());
parkPower = robot.stateConfiguration.variable("auto","13_0","power").value();
parkBrakeTime = robot.stateConfiguration.variable("auto","13_0","brakeMS").value();
super.init();
}
@@ -85,18 +69,18 @@ public class AutoEngine extends CyberarmEngine {
//aligns to goal
addState(new Face(robot, "auto", "04_1"));
//launch rings
//launch rings and drive to scoreing area. LaunchDriveControl allows makes the robot begin
//driving while the belt is resetting
CyberarmState launchState = new Launch(robot, "auto", "04_2");
//drive to scoring area
CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true);
addState(new LaunchDriveControl(robot,launchState,driveState));
//turn arm towards scoreing area.
ArrayList<CyberarmState> threadStates = new ArrayList<>();
threadStates.add(new Face(robot, "auto", "05_1"));
threadStates.add(new WobbleArm(robot, "auto", "05_2",false));
addState(new ThreadStates(threadStates));
ArrayList<CyberarmState> threadStates0 = new ArrayList<>();
threadStates0.add(new Face(robot, "auto", "05_1"));
threadStates0.add(new WobbleArm(robot, "auto", "05_2",robot.wobbleDownPos));
addState(new ThreadStates(threadStates0));
//open the wobble grab arm
addState(new WobbleGrab(robot, "auto", "06_0", true));
@@ -105,25 +89,32 @@ public class AutoEngine extends CyberarmEngine {
addState(new DriveToCoordinates(robot, "auto","06_1"));
addState(new DriveToCoordinates(robot, "auto", "07_0"));
addState(new FindWobbleGoal(robot, "auto", "08_0"));
addState(new Face(robot,"auto","07_1"));
//close grabber
addState(new WobbleGrab(robot, "auto", "09_0", false));
addState(new FindWobbleGoal(robot, "auto", "08_0"));
addState(new Pause(robot,"auto","09_0"));
//drive to scoring area
// addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreBFaceAngle,scoreBTolerance,scoreBPower,scoreBBrakeTime));
addState(new DriveToCoordinates(robot, "auto", "10_0", true));
ArrayList<CyberarmState> threadStates1 = new ArrayList<>();
// threadStates1.add(new Face(robot, "auto", "09_0"));
threadStates1.add(new WobbleArm(robot, "auto", "09_1",robot.wobbleUpPos));
threadStates1.add(new DriveToCoordinates(robot, "auto", "10_0", true));
addState(new ThreadStates(threadStates1));
ArrayList<CyberarmState> threadStates2 = new ArrayList<>();
threadStates2.add(new Face(robot, "auto", "11_0"));
threadStates2.add(new WobbleArm(robot, "auto", "11_1",robot.wobbleDownPos));
addState(new ThreadStates(threadStates2));
//release wobble goal 2
addState(new Face(robot, "auto", "11_0"));
addState(new WobbleGrab(robot, "auto", "12_0", true));
//drive to park
addState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
// ArrayList<CyberarmState> threadStatesB = new ArrayList<>();
// threadStatesB.add(new WobbleGrab(robot, "auto", "12_1", false));
// threadStatesB.add(new WobbleArm(robot, "auto", "12_2",true));
// threadStatesB.add(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
// addState(new ThreadStates(threadStatesB));
ArrayList<CyberarmState> threadStates3 = new ArrayList<>();
threadStates3.add(new WobbleArm(robot, "auto", "12_2", 0));
threadStates3.add(new Park(robot,"auto","13_0"));
addState(new ThreadStates(threadStates3));
}
}

11
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java
View File

@@ -16,6 +16,8 @@ public class FindWobbleGoal extends CyberarmState {
private boolean cCheckPrev;
private boolean ccCheckPrev;
private float startRotation;
private boolean foundGoalRotation;
private float wobbleGoalRotation;
public FindWobbleGoal(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -44,6 +46,7 @@ public class FindWobbleGoal extends CyberarmState {
double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
if (sensorValue > turnCheck) {
float rotation = robot.getRelativeAngle(startRotation,robot.getRotation());
boolean cCheck = rotation > range;
@@ -61,9 +64,15 @@ public class FindWobbleGoal extends CyberarmState {
} else {
if (sensorValue > driveCheck) {
robot.setDrivePower(-power,-power,-power,-power);
if (!foundGoalRotation) {
foundGoalRotation = true;
wobbleGoalRotation = robot.getRotation();
}
double[] powers = robot.getMecanumPowers(wobbleGoalRotation - 90, power*2 , wobbleGoalRotation);
robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
} else {
robot.setDrivePower(0,0,0,0);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
setHasFinished(true);
}
}

45
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java Normal file
View File

@@ -0,0 +1,45 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.util.ArrayList;
public class Park extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
double parkY;
float parkFaceAngle;
double parkTolerance;
double parkPower;
long parkBrakeTime;
public Park(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
}
@Override
public void init() {
parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"yPos").value());
parkFaceAngle = robot.stateConfiguration.variable(groupName,actionName,"face").value();
parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto",actionName,"tolPos").value());
parkPower = robot.stateConfiguration.variable(groupName,actionName,"power").value();
parkBrakeTime = robot.stateConfiguration.variable(groupName,actionName,"brakeMS").value();
}
@Override
public void start() {
if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8))
addState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
}
@Override
public void exec() {
setHasFinished(childrenHaveFinished());
}
}

164
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo1.java
View File

@@ -1,164 +0,0 @@
package org.timecrafters.UltimateGoal.Competition.Demo;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class Demo1 extends CyberarmState {
private Robot robot;
//normal drive control
private float leftJoystickDegrees;
private double leftJoystickMagnitude;
private float rightJoystickDegrees;
private double rightJoystickMagnitude;
private double rightJoystickMagnitudePrevious;
private double faceControlThreshold;
private float cardinalSnapping;
private float pairSnapping;
private float faceDirection = 0;
private double[] powers = {0,0,0,0};
private double drivePower = 1;
private boolean lbPrev;
//find wobble goal control
private FindWobbleGoal findWobbleGoal;
private boolean runNextFindWobble;
private boolean findWobbleInputPrev;
//Drive to launch control
private DriveToCoordinates driveToLaunch;
private boolean runNextDriveToLaunch;
private boolean driveToLaunchInputPrev;
private double launchTolerance;
private double launchPower;
private long launchBrakeTime;
private float launchAngleGoal;
private float launchAnglePower1;
private float launchAnglePower2;
private float launchAnglePower3;
public Demo1(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
}
@Override
public void start() {
faceDirection = robot.getRotation();
}
@Override
public void exec() {
robot.updateLocation();
boolean lb = engine.gamepad1.left_stick_button;
if (lb && !lbPrev) {
if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
}
lbPrev = lb;
runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
boolean findWobbleInput = engine.gamepad1.dpad_up;
if (findWobbleInput) {
if (runNextFindWobble && !findWobbleInputPrev) {
findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
addParallelState(findWobbleGoal);
}
faceDirection = robot.getRotation();
} else if (!runNextFindWobble) {
findWobbleGoal.setHasFinished(true);
}
findWobbleInputPrev = findWobbleInput;
if (childrenHaveFinished()) {
//Normal Driver Controls
double leftJoystickX = engine.gamepad1.left_stick_x;
double leftJoystickY = engine.gamepad1.left_stick_y;
leftJoystickDegrees = robot.getRelativeAngle(90, (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 = robot.getRelativeAngle(90, (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
//allows the the driver to indicate which direction the robot is currently looking so
//so that the controller and robot can be re-synced in the event of a bad initial
//position.
if (engine.gamepad1.right_stick_button) {
robot.setLocalization(rightJoystickDegrees, robot.getLocationX(), robot.getLocationY());
faceDirection = rightJoystickDegrees;
}
//if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
if (rightJoystickMagnitude > faceControlThreshold || rightJoystickMagnitude - rightJoystickMagnitudePrevious > 0) {
//if the joystick is close to one of the cardinal directions, it is set to exactly the cardinal direction
faceDirection = snapToCardinal(rightJoystickDegrees, cardinalSnapping, 0);
}
rightJoystickMagnitudePrevious = rightJoystickMagnitude;
if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {
//drives the robot in the direction of the move joystick while facing the direction
//of the look joystick. if the move direction is almost aligned/perpendicular to the
//look joystick,
powers = robot.getMecanumPowers(snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection), drivePower, faceDirection);
}
robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
}
}
@Override
public void telemetry() {
engine.telemetry.addLine("Controler Directions");
engine.telemetry.addData("Right", rightJoystickDegrees);
engine.telemetry.addData("Left", leftJoystickDegrees);
engine.telemetry.addData("face", faceDirection);
engine.telemetry.addData("Player 1 children", childrenHaveFinished());
for (CyberarmState state : children) {
if (!state.getHasFinished()) {
engine.telemetry.addLine("" + state.getClass());
}
}
}
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;
}
}

142
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/Demo2.java
View File

@@ -1,142 +0,0 @@
package org.timecrafters.UltimateGoal.Competition.Demo;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class Demo2 extends CyberarmState {
private Robot robot;
private boolean rbPrev;
private boolean yPrev;
private boolean xPrev;
private boolean bPrev;
private boolean wobbleArmUp = false;
private boolean wobbleGrabOpen = false;
private boolean aPrev;
private double beltPowerPrev;
private boolean lbPrev;
private boolean manualArmHold;
private boolean launchInput = false;
public Demo2(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
robot.wobbleArmMotor.setTargetPosition(0);
robot.wobbleArmMotor.setPower(0.5);
}
@Override
public void start() {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
}
@Override
public void exec() {
//Collector control
if (childrenHaveFinished()) {
robot.collectionMotor.setPower(engine.gamepad1.right_trigger);
} else {
robot.collectionMotor.setPower(0);
}
//belt progression control
boolean rb = engine.gamepad1.right_bumper;
if (rb && !rbPrev && childrenHaveFinished()) {
addParallelState(new ProgressRingBelt(robot));
}
rbPrev = rb;
//launch sequence control
boolean y2 = engine.gamepad1.y;
if (y2 && !yPrev && childrenHaveFinished()) {
addParallelState(new Launch(robot));
}
yPrev = y2;
//toggles wobble grabber open and closed
boolean x = engine.gamepad1.x;
if (x && !xPrev) {
wobbleGrabOpen = !wobbleGrabOpen;
if (wobbleGrabOpen) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
} else {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.05 );
}
}
xPrev = x;
//toggles the wobble arm up and down.
boolean b = engine.gamepad1.b;
if (b && !bPrev) {
wobbleArmUp = !wobbleArmUp;
if (wobbleArmUp) {
robot.wobbleArmMotor.setTargetPosition(550);
} else {
robot.wobbleArmMotor.setTargetPosition(0);
}
}
bPrev = b;
//manually toggle the launch wheel for emergencies
boolean a = engine.gamepad1.a;
if (a && !aPrev) {
if (robot.launchMotor.getPower() == 0) {
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
} else {
robot.launchMotor.setPower(0);
}
}
aPrev = a;
//manually control the wobble arm for when it's initialized in an unexpected position.
if (engine.gamepad1.dpad_up) {
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.wobbleArmMotor.setPower(0.5);
manualArmHold = true;
} else if (engine.gamepad1.dpad_down) {
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.wobbleArmMotor.setPower(-0.1);
manualArmHold = true;
} else if (manualArmHold) {
manualArmHold = false;
robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
//allows the driver to revers the belt in the event of a jam
boolean lb = engine.gamepad1.left_bumper;
if (lb && !lbPrev) {
beltPowerPrev = robot.ringBeltMotor.getPower();
robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
}
if (!lb && lbPrev) {
robot.ringBeltMotor.setPower(beltPowerPrev);
}
lbPrev = lb;
}
@Override
public void telemetry() {
engine.telemetry.addData("Player 2 children", childrenHaveFinished());
for (CyberarmState state : children) {
if (!state.getHasFinished()) {
engine.telemetry.addLine("" + state.getClass());
}
}
}
}

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

@@ -15,8 +15,8 @@ public class DemoControl extends CyberarmState {
@Override
public void start() {
addParallelState(new Demo1(robot));
addParallelState(new Demo2(robot));
// addParallelState(new Demo1(robot));
// addParallelState(new Demo2(robot));
}
@Override

2
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngine.java
View File

@@ -1,11 +1,13 @@
package org.timecrafters.UltimateGoal.Competition.Demo;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.TeleOp.TeleOpState;
@Disabled
@TeleOp (name = "Demo")
public class DemoEngine extends CyberarmEngine {

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

@@ -31,14 +31,14 @@ public class Launch extends CyberarmState {
try {
if (robot.stateConfiguration.action(groupName, actionName).enabled) {
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.ringBeltMotor.setPower(0.7);
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
} else {
setHasFinished(true);
}
} catch (NullPointerException e){
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.ringBeltMotor.setPower(0.7);
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
}
}
@@ -65,25 +65,25 @@ public class Launch extends CyberarmState {
//the first receiving position.
if (hasCycled) {
robot.ringBeltMotor.setPower(0);
robot.ringBeltStage = 0;
robot.ringBeltMotor.setTargetPosition(beltPos);
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
if (!robot.initLauncher) {
robot.launchMotor.setPower(0);
}
setHasFinished(true);
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
} else {
hasCycled = true;
reducePos = (int) (beltPos + (robot.reduceLaunchPos));
reducePos = beltPos + (robot.reduceLaunchPos);
}
}
detectedPass = detectingPass;
boolean reduceCondition = (hasCycled && beltPos > reducePos);
if (reduceCondition && !reduceConditionPrev){
robot.ringBeltOn();
robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
//the ring belt stage lets other states know that the robot has finished launching all three rings
//and is now returning to loading position.

23
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/CamServo.java → TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Pause.java
View File

@@ -2,40 +2,37 @@ package org.timecrafters.UltimateGoal.Competition;
import org.cyberarm.engine.V2.CyberarmState;
public class CamServo extends CyberarmState {
public class Pause extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private boolean open;
private long waitTime = -1;
private boolean enabled;
public CamServo(Robot robot, String groupName, String actionName, boolean open) {
public Pause(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.open = open;
}
public CamServo(Robot robot, boolean armUp, long waitTime) {
public Pause(Robot robot, boolean open, long waitTime) {
this.robot = robot;
this.open = armUp;
this.waitTime = waitTime;
}
@Override
public void init() {
if (waitTime == -1) {
enabled = robot.stateConfiguration.action(groupName, actionName).enabled;
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);
if (!enabled) {
setHasFinished(true);
}
}
@@ -44,4 +41,10 @@ public class CamServo extends CyberarmState {
setHasFinished(runTime() > waitTime);
}
@Override
public void telemetry() {
engine.telemetry.addData("runTime", runTime());
engine.telemetry.addData("wait", waitTime);
}
}

2
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/FindLimitSwitch.java
View File

@@ -13,7 +13,7 @@ public class FindLimitSwitch extends CyberarmState {
@Override
public void start() {
robot.ringBeltOn();
robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
}
@Override

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

@@ -19,7 +19,7 @@ public class ProgressRingBelt extends CyberarmState {
private void prep(){
robot.ringBeltMotor.setTargetPosition(targetPos);
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.ringBeltMotor.setPower(0.7);
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
robot.ringBeltStage += 1;
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
}
@@ -28,7 +28,7 @@ public class ProgressRingBelt extends CyberarmState {
public void start() {
int currentPos = robot.ringBeltMotor.getCurrentPosition();
if (robot.ringBeltStage < 2) {
targetPos = currentPos + Robot.RING_BELT_GAP;
targetPos = currentPos + robot.ringBeltGap;
prep();
} else if (robot.ringBeltStage == 2) {
targetPos = currentPos + 240;

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

@@ -1,5 +1,11 @@
package org.timecrafters.UltimateGoal.Competition;
/*
The robot object contains all the hardware and functions that are used in both teleOp and
Autonomous. This includes drive functions, localization functions, shared constants, and a few
general calculations and debugging tools.
*/
import android.os.Environment;
import android.util.Log;
@@ -47,12 +53,18 @@ public class Robot {
this.hardwareMap = hardwareMap;
}
//The TimeCraftersConfiguration is part of a debugging and tuning tool that allows us to edit
//variables saved on the phone, without having to re-download the whole program.
public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
//We use the IMU to get reliable rotation and angular velocity information. Experimentation has
//demonstrated that the accelerometer and related integrations aren't as accurate.
public BNO055IMU imu;
//The LEDs are used to provide driver feedback and for looking beautiful
public RevBlinkinLedDriver ledDriver;
//drive system
//drive and dead-wheal hardware
public DcMotor driveFrontLeft;
public DcMotor driveBackLeft;
public DcMotor driveFrontRight;
@@ -62,8 +74,9 @@ public class Robot {
public DcMotor encoderRight;
public DcMotor encoderBack;
//Steering Constants
static final double CUBIC_CORRECTION = 0.025;
//Motion Constants
static final double CUBIC_CORRECTION = 0.035;
static final double FACE_CUBIC_CORRECTION = 0.025;
static final double LINEAR_CORRECTION = 0.055;
static final double FACE_MIN_CORRECTION = 0.2;
static final double FACE_LINEAR_CORRECTION = 0.025;
@@ -74,7 +87,7 @@ public class Robot {
static final double FACE_MOMENTUM_CORRECTION = 1.06;
static final double FACE_MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(FACE_MOMENTUM_MAX_CORRECTION-1)/Math.log10(FACE_MOMENTUM_CORRECTION));
//Conversion Constants
//Unit Conversion Constants
static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
static final int COUNTS_PER_REVOLUTION = 8192;
static final float mmPerInch = 25.4f;
@@ -83,16 +96,6 @@ public class Robot {
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 = 8f;
// Inches above Ground
static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
// Inches Left of axis of rotation
static final float CAMERA_LEFT_DISPLACEMENT = 4f;
static final double CAMERA_DISPLACEMENT_MAG = Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
//Robot Localization
private static double locationX;
private static double locationY;
@@ -104,6 +107,38 @@ public class Robot {
private float rotationPrevious = 0;
public float angularVelocity;
//vuforia navigation
private WebcamName webcam;
private VuforiaLocalizer vuforia;
// Inches Forward of axis of rotation
static final float CAMERA_FORWARD_DISPLACEMENT = 8f;
// Inches above Ground
static final float CAMERA_VERTICAL_DISPLACEMENT = 9.5f;
// Inches Left of axis of rotation
static final float CAMERA_LEFT_DISPLACEMENT = 4f;
static final double CAMERA_DISPLACEMENT_MAG = Math.hypot(CAMERA_FORWARD_DISPLACEMENT,CAMERA_LEFT_DISPLACEMENT);
static final float CAMERA_DISPLACEMENT_DIRECTION = (float) -Math.atan(CAMERA_LEFT_DISPLACEMENT/CAMERA_FORWARD_DISPLACEMENT);
public boolean trackableVisible;
private VuforiaTrackables targetsUltimateGoal;
private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
private OpenGLMatrix lastConfirmendLocation;
private long timeStartZeroVelocity = 0;
private long minCheckDurationMs = 500;
private int minCheckVelocity = 1;
private float vuforiaRotationCull;
//The servo mount for our camera allows us to look down for ideal TensorFlow and look up for
//ideal Vuforia Navigation
public Servo webCamServo;
public static final double CAM_SERVO_DOWN = 0.15;
//TensorFlow Object Detection
public TFObjectDetector tfObjectDetector;
private static final float MINIMUM_CONFIDENCE = 0.8f;
//Launcher
public DcMotor launchMotor;
@@ -114,57 +149,36 @@ public class Robot {
public double launchPositionY;
public float launchRotation;
public int reduceLaunchPos;
public static final double LAUNCH_TOLERANCE_POS = 0.5 * (COUNTS_PER_REVOLUTION/ENCODER_CIRCUMFERENCE);
public static final double LAUNCH_TOLERANCE_FACE = 0.5;
public boolean initLauncher;
//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 = 700;
public static final double RING_BELT_POWER = 0.2;
public int ringBeltGap = 700;
public static final double RING_BELT_SLOW_POWER = 0.2;
public static final double RING_BELT_NORMAL_POWER = 0.6;
private int ringBeltPrev;
public long beltMaxStopTime;
public int beltReverseTicks;
public int beltMaxStopTicks;
//Wobble Goal Arm
public DcMotor wobbleArmMotor;
//Wobble Goal Arm & Grabber
public DcMotor wobbleArmMotor;
public Servo wobbleGrabServo;
public static final int WOBBLE_ARM_DOWN = -710;
public static final double WOBBLE_SERVO_MAX = 0.3;
public int wobbleDownPos;
public int wobbleUpPos;
public int wobbleDropPos;
public static final double WOBBLE_SERVO_OPEN = 0;
public static final double WOBBLE_SERVO_CLOSED = 1;
public RevColorSensorV3 wobbleColorSensor;
public double wobbleScoreX;
public double wobbleScoreY;
//vuforia navigation
private WebcamName webcam;
private VuforiaLocalizer vuforia;
public Servo webCamServo;
public static final double CAM_SERVO_DOWN = 0.15;
public boolean trackableVisible;
private VuforiaTrackables targetsUltimateGoal;
private List<VuforiaTrackable> trackables = new ArrayList<VuforiaTrackable>();
private OpenGLMatrix lastConfirmendLocation;
private long timeStartZeroVelocity = 0;
private long minCheckDurationMs = 500;
private int minCheckVelocity = 1;
//TensorFlow Object Detection
public TFObjectDetector tfObjectDetector;
private static final float MINIMUM_CONFIDENCE = 0.8f;
public RevTouchSensor wobbleTouchSensor;
//Debugging
public double totalV;
@@ -172,7 +186,6 @@ public class Robot {
public double visionY;
public double visionZ;
public float rawAngle;
// private String TestingRecord = "FrontLeft,FrontRight,BackLeft,BackRight";
private String TestingRecord = "x,y";
public double forwardVector;
@@ -213,9 +226,15 @@ public class Robot {
wobbleArmMotor.setTargetPosition(0);
wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
wobbleUpPos = stateConfiguration.variable("system","arm", "up").value();
wobbleDownPos = stateConfiguration.variable("system","arm", "down").value();
wobbleDropPos = stateConfiguration.variable("system","arm", "drop").value();
wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
//init ring belt
collectionMotor = hardwareMap.dcMotor.get("collect");
@@ -228,6 +247,7 @@ public class Robot {
beltMaxStopTime = stateConfiguration.variable("system","belt", "maxStopTime").value();
beltMaxStopTicks = stateConfiguration.variable("system","belt", "maxStopTicks").value();
beltReverseTicks = stateConfiguration.variable("system","belt", "reverseTicks").value();
ringBeltGap = stateConfiguration.variable("system","belt","gap").value();
//init IMU
imu = hardwareMap.get(BNO055IMU.class, "imu");
@@ -259,8 +279,9 @@ public class Robot {
webCamServo = hardwareMap.servo.get("look");
webCamServo.setDirection(Servo.Direction.REVERSE );
minCheckVelocity =stateConfiguration.variable("system", "tensorFlow", "minCheckV").value();
minCheckDurationMs =stateConfiguration.variable("system", "tensorFlow", "minCheckMS").value();
minCheckVelocity =stateConfiguration.variable("system", "camera", "minCheckV").value();
vuforiaRotationCull = stateConfiguration.variable("system", "camera", "rCull").value();
minCheckDurationMs =stateConfiguration.variable("system", "camera", "minCheckMS").value();
//Init Launch Motor
DcMotor launcher = hardwareMap.dcMotor.get("launcher");
@@ -352,7 +373,7 @@ public class Robot {
int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
"tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
parameters.minResultConfidence = stateConfiguration.variable("system", "tensorFlow", "minConfidence").value();
parameters.minResultConfidence = stateConfiguration.variable("system", "camera", "minConfidence").value();
tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
}
@@ -413,22 +434,7 @@ public class Robot {
Robot.rotation += rotationChange;
// totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
//
// if (totalV < minCheckVelocity) {
// long timeCurrent = System.currentTimeMillis();
//
// if (timeStartZeroVelocity == 0) {
// timeStartZeroVelocity = timeCurrent;
// } else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs) {
// syncWithVuforia();
// }
//
// } else {
// timeStartZeroVelocity = 0;
// }
totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
if (Robot.rotation > 180) {
Robot.rotation -= 360;
@@ -439,6 +445,22 @@ public class Robot {
}
public void syncIfStationary() {
if (totalV < minCheckVelocity) {
long timeCurrent = System.currentTimeMillis();
if (timeStartZeroVelocity == 0) {
timeStartZeroVelocity = timeCurrent;
} else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs ) {
syncWithVuforia();
}
} else {
timeStartZeroVelocity = 0;
}
}
public void syncWithVuforia() {
trackableVisible = false;
for (VuforiaTrackable trackable : trackables) {
@@ -454,24 +476,28 @@ public class Robot {
//For our tournament, it makes sense to make zero degrees towards the goal.
//Orientation is inverted to have clockwise be positive.
Orientation rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
Robot.rotation = 90-rotation.thirdAngle;
Orientation orientation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
float vuforiaRotation = 90-orientation.thirdAngle;
if (Robot.rotation > 180) {
Robot.rotation -= -180;
if (vuforiaRotation > 180) {
vuforiaRotation -= -180;
}
VectorF translation = lastConfirmendLocation.getTranslation();
double camX = -translation.get(1) / mmPerInch;
double camY = translation.get(0) / mmPerInch;
if (Math.abs(rotation - vuforiaRotation) < vuforiaRotationCull) {
rotation = vuforiaRotation;
double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
VectorF translation = lastConfirmendLocation.getTranslation();
double camX = -translation.get(1) / mmPerInch;
double camY = translation.get(0) / mmPerInch;
locationX = inchesToTicks(camX - displaceX);
locationY = inchesToTicks(camY - displaceY);
double displaceX = CAMERA_DISPLACEMENT_MAG * Math.sin(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
double displaceY = CAMERA_DISPLACEMENT_MAG * Math.cos(Robot.rotation + 180 - CAMERA_DISPLACEMENT_DIRECTION);
break;
locationX = inchesToTicks(camX - displaceX);
locationY = inchesToTicks(camY - displaceY);
break;
}
}
}
}
@@ -570,22 +596,21 @@ public class Robot {
Math.pow(CUBIC_CORRECTION * relativeRotation, 3) +
LINEAR_CORRECTION * relativeRotation;
if (relativeRotation != 0) {
double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation));
double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
//reduces concern for momentum when the angle is far away from target
turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) * (1 - momentumCorrection)) / 180 );
// turnCorrection *= momentumCorrection;
}
double powerForwardRight = scalar * (q + turnCorrection);
double powerForwardLeft = scalar * (p - turnCorrection);
double powerBackRight = scalar * (p + turnCorrection);
double powerBackLeft = scalar * (q - turnCorrection);
if (relativeRotation != 0) {
double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation));
double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
double momentumCorrection = (MOMENTUM_MAX_CORRECTION*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.
@@ -610,7 +635,7 @@ public class Robot {
public double[] getFacePowers(float direction, double power) {
angularVelocity = imu.getAngularVelocity().xRotationRate;
double relativeAngle = getRelativeAngle(direction, Robot.rotation);
double scaler = Math.pow(CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle;
double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) + FACE_LINEAR_CORRECTION * relativeAngle;
if (relativeAngle > 0.5) {
scaler += FACE_MIN_CORRECTION;
@@ -633,10 +658,6 @@ public class Robot {
return powers;
}
public void ringBeltOn() {
ringBeltMotor.setPower(RING_BELT_POWER);
}
public boolean beltIsStuck() {
int ringBeltPos = ringBeltMotor.getCurrentPosition();
boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks);

34
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player1.java
View File

@@ -30,6 +30,7 @@ public class Player1 extends CyberarmState {
private FindWobbleGoal findWobbleGoal;
private boolean runNextFindWobble;
private boolean findWobbleInputPrev;
private boolean aPrev;
//Drive to launch control
private DriveToCoordinates driveToLaunch;
@@ -84,20 +85,43 @@ public class Player1 extends CyberarmState {
}
lbPrev = lb;
if (engine.gamepad1.guide) {
robot.syncIfStationary();
}
runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
boolean findWobbleInput = engine.gamepad1.x;
if (findWobbleInput) {
if (runNextFindWobble && !findWobbleInputPrev) {
findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
addParallelState(findWobbleGoal);
//if the claw is open, run FindWobbleGoal
if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
faceDirection = robot.getRotation();
if (runNextFindWobble && !findWobbleInputPrev) {
findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
addParallelState(findWobbleGoal);
}
//if the claw is closed, open the claw.
} else if (!findWobbleInputPrev) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
}
faceDirection = robot.getRotation();
//if the button is released cancel the search
} else if (!runNextFindWobble) {
findWobbleGoal.setHasFinished(true);
}
findWobbleInputPrev = findWobbleInput;
//toggles wobble grabber open and closed
boolean a = engine.gamepad1.a;
if (a && !aPrev) {
if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
} else {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
}
}
aPrev = a;
runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished());
@@ -168,7 +192,7 @@ public class Player1 extends CyberarmState {
robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
}
//LED feedback control
double ringBeltPower = robot.ringBeltMotor.getPower();
if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - robot.ringBeltMotor.getCurrentPosition()) > 10) {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );

104
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java
View File

@@ -23,6 +23,8 @@ public class Player2 extends CyberarmState {
private boolean lbPrev;
private boolean manualArmHold;
private boolean launchInput = false;
public Player2(Robot robot) {
@@ -45,14 +47,13 @@ public class Player2 extends CyberarmState {
public void exec() {
//Collector control
if (childrenHaveFinished()) {
double rt = engine.gamepad2.right_trigger;
double lt = engine.gamepad2.left_trigger;
if (rt >= lt) {
robot.collectionMotor.setPower(rt);
} else {
robot.collectionMotor.setPower(-lt);
}
double rt = engine.gamepad2.right_trigger;
double lt = engine.gamepad2.left_trigger;
if (rt < lt) {
robot.collectionMotor.setPower(-lt);
} else if (childrenHaveFinished()) {
robot.collectionMotor.setPower(rt);
} else {
robot.collectionMotor.setPower(0);
}
@@ -71,29 +72,48 @@ public class Player2 extends CyberarmState {
}
yPrev = y2;
//toggles wobble grabber open and closed
boolean x = engine.gamepad2.x;
if (x && !xPrev) {
wobbleGrabOpen = !wobbleGrabOpen;
if (wobbleGrabOpen) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
} else {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.05 );
}
}
xPrev = x;
//toggles the wobble arm up and down.
boolean b = engine.gamepad2.b;
if (b && !bPrev) {
wobbleArmUp = !wobbleArmUp;
if (wobbleArmUp) {
robot.wobbleArmMotor.setTargetPosition(550);
// boolean b = engine.gamepad2.b;
// if (b && !bPrev) {
// wobbleArmUp = !wobbleArmUp;
// if (wobbleArmUp) {
// robot.wobbleArmMotor.setTargetPosition(550);
// } else {
// robot.wobbleArmMotor.setTargetPosition(0);
// }
// }
// bPrev = b;
//manually control the wobble arm for when it's initialized in an unexpected position.
double leftStickY = engine.gamepad2.left_stick_y;
if (engine.gamepad2.dpad_right) {
if (!robot.wobbleTouchSensor.isPressed()) {
setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.wobbleArmMotor.setPower(-0.2);
robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
} else {
setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.wobbleArmMotor.setTargetPosition(0);
setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
}
} else if (leftStickY != 0 ) {
setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.wobbleArmMotor.setPower(0.15 * leftStickY);
robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
} else {
setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.wobbleArmMotor.setPower(0.5);
if (engine.gamepad2.dpad_up) {
robot.wobbleArmMotor.setTargetPosition(robot.wobbleUpPos);
} else if (engine.gamepad2.dpad_down) {
robot.wobbleArmMotor.setTargetPosition(robot.wobbleDownPos);
} else if (engine.gamepad2.dpad_left) {
robot.wobbleArmMotor.setTargetPosition(robot.wobbleDropPos );
}
}
bPrev = b;
//manually toggle the launch wheel for emergencies
boolean a = engine.gamepad2.a;
@@ -106,20 +126,8 @@ public class Player2 extends CyberarmState {
}
aPrev = a;
//manually control the wobble arm for when it's initialized in an unexpected position.
if (engine.gamepad2.dpad_up) {
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.wobbleArmMotor.setPower(0.5);
manualArmHold = true;
} else if (engine.gamepad2.dpad_down) {
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.wobbleArmMotor.setPower(-0.1);
manualArmHold = true;
} else if (manualArmHold) {
manualArmHold = false;
robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
//allows the driver to revers the belt in the event of a jam
boolean lb = engine.gamepad2.left_bumper;
@@ -127,7 +135,7 @@ public class Player2 extends CyberarmState {
runModePrev = robot.ringBeltMotor.getMode();
beltPowerPrev = robot.ringBeltMotor.getPower();
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
}
if (!lb && lbPrev) {
@@ -138,13 +146,21 @@ public class Player2 extends CyberarmState {
lbPrev = lb;
}
private void setArmMode(DcMotor.RunMode runMode) {
if (robot.wobbleArmMotor.getMode() != runMode) {
robot.wobbleArmMotor.setMode(runMode);
}
}
@Override
public void telemetry() {
engine.telemetry.addLine("belt");
engine.telemetry.addData("power", robot.ringBeltMotor.getPower());
engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
// engine.telemetry.addLine("belt");
// engine.telemetry.addData("power", robot.ringBeltMotor.getPower());
// engine.telemetry.addData("pos", robot.ringBeltMotor.getCurrentPosition());
// engine.telemetry.addData("target", robot.ringBeltMotor.getTargetPosition());
engine.telemetry.addData("Touch Sensor Pressed", robot.wobbleTouchSensor.isPressed());
engine.telemetry.addData(" Sensor value", robot.wobbleTouchSensor.getValue());
engine.telemetry.addData("Player 2 children", childrenHaveFinished());
for (CyberarmState state : children) {
if (!state.getHasFinished()) {

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

@@ -14,7 +14,7 @@ public class TeleOpEngine extends CyberarmEngine {
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
robot.wobbleGrabServo.setPosition(0);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
robot.webCamServo.setPosition(0);
super.init();
}

3
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java
View File

@@ -1,6 +1,5 @@
package org.timecrafters.UltimateGoal.Competition;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
@@ -22,7 +21,7 @@ public class UnstickRingBelt extends CyberarmState {
lastRunMode = robot.ringBeltMotor.getMode();
int currentPos = robot.ringBeltMotor.getCurrentPosition();
targetPos = currentPos - robot.beltReverseTicks;
robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
}
@Override

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

@@ -7,19 +7,20 @@ public class WobbleArm extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private boolean armUp;
private int suggestedPos;
private int targetPos = -1;
private long waitTime = -1;
public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) {
public WobbleArm(Robot robot, String groupName, String actionName, int targetPos) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
this.armUp = armUp;
this.suggestedPos = targetPos;
}
public WobbleArm(Robot robot, boolean armUp, long waitTime) {
public WobbleArm(Robot robot, int targetPos, long waitTime) {
this.robot = robot;
this.armUp = armUp;
this.targetPos = targetPos;
this.waitTime = waitTime;
}
@@ -27,17 +28,17 @@ public class WobbleArm extends CyberarmState {
public void init() {
if (waitTime == -1) {
waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
targetPos = robot.stateConfiguration.variable(groupName,actionName,"armPos").value();
if (targetPos == -1) {
targetPos = suggestedPos;
}
}
}
@Override
public void start() {
robot.wobbleArmMotor.setPower(0.3);
if (armUp) {
robot.wobbleArmMotor.setTargetPosition(0);
} else {
robot.wobbleArmMotor.setTargetPosition(Robot.WOBBLE_ARM_DOWN);
}
robot.wobbleArmMotor.setPower(0.5);
robot.wobbleArmMotor.setTargetPosition(targetPos);
}
@Override
@@ -48,5 +49,7 @@ public class WobbleArm extends CyberarmState {
@Override
public void telemetry() {
engine.telemetry.addData("runTime", runTime());
engine.telemetry.addData("target", targetPos);
engine.telemetry.addData("wait", waitTime);
}
}

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

@@ -36,9 +36,9 @@ public class WobbleGrab extends CyberarmState {
public void start() {
if (enabled) {
if (open) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
} else {
robot.wobbleGrabServo.setPosition(0);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
}
} else {
setHasFinished(true);
@@ -50,4 +50,10 @@ public class WobbleGrab extends CyberarmState {
setHasFinished(runTime() > waitTime);
}
@Override
public void telemetry() {
engine.telemetry.addData("runTime", runTime());
engine.telemetry.addData("wait", waitTime);
}
}

3
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/ServoPosTest.java
View File

@@ -20,12 +20,11 @@ public class ServoPosTest extends CyberarmState {
@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;
servoPos = engine.gamepad1.right_stick_y;
servo.setPosition(servoPos);
}

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

@@ -13,7 +13,7 @@ public class TestingEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new LEDTest());
addState(new ServoPosTest());
}
}