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

Emergency features & autonomous touches

Browse Source
This commit is contained in:
Nathaniel Palme
2021-02-19 21:03:49 -06:00
parent 55602bb617
commit 902a7cefff
12 changed files with 319 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,16 +26,20 @@ public class AutoEngine extends CyberarmEngine {
private double scoreAPower;
private long scoreABrakeTime;
float scoreBFaceAngle;
double scoreBTolerance;
double scoreBPower;
long scoreBBrakeTime;
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);
// since we've preloaded three rings, the ring belt stage is set to account for this;
robot.ringBeltStage = 3;
launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
@@ -46,10 +50,11 @@ public class AutoEngine extends CyberarmEngine {
// scoreAPower = robot.stateConfiguration.variable("auto","05_0","power").value();
// scoreABrakeTime = robot.stateConfiguration.variable("auto","05_0","brakeMS").value();
//
// scoreBFaceAngle = robot.stateConfiguration.variable("auto","10_0","face").value();
// scoreBTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","10_0","tolPos").value());
// scoreBPower = robot.stateConfiguration.variable("auto","10_0","power").value();
// scoreBBrakeTime = robot.stateConfiguration.variable("auto","10_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();
}
@@ -72,15 +77,15 @@ public class AutoEngine extends CyberarmEngine {
//drive to launch position
addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime));
//
//aligns to goal
addState(new Face(robot, "auto", "04_1"));
//launch rings
addState(new Launch(robot, "auto", "04_2"));
CyberarmState launchState = new Launch(robot, "auto", "04_2");
//drive to scoring area
addState(new DriveToCoordinates(robot, "auto", "05_0", true));
// addState(new DriveToCoordinates(robot, tensorFlowCheck.wobblePosX,tensorFlowCheck.wobblePosY,scoreAFaceAngle,scoreATolerance,scoreAPower,scoreABrakeTime));
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<>();
@@ -95,7 +100,7 @@ public class AutoEngine extends CyberarmEngine {
addState(new DriveToCoordinates(robot, "auto","06_1"));
addState(new DriveToCoordinates(robot, "auto", "07_0"));
addState(new DriveWithColorSensor(robot, "auto", "08_0"));
addState(new FindWobbleGoal(robot, "auto", "08_0"));
//close grabber
addState(new WobbleGrab(robot, "auto", "09_0", false));
@@ -109,10 +114,11 @@ public class AutoEngine extends CyberarmEngine {
addState(new WobbleGrab(robot, "auto", "12_0", true));
//drive to park
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, "auto", "13_0"));
addState(new ThreadStates(threadStatesB));
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));
}
}

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

@@ -45,6 +45,8 @@ public class DriveToCoordinates extends CyberarmState {
@Override
public void init() {
if (!groupName.equals("manual")) {
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();
tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
@@ -62,12 +64,15 @@ public class DriveToCoordinates extends CyberarmState {
if (!groupName.equals("manual")) {
setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
if (!scoringArea) {
xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
} else {
xTarget = robot.wobbleScoreX;
yTarget = robot.wobbleScoreY;
//used to navigate towards the randomly generated scoreing area. the original target
//becomes an offset of the scoring area position.
if (scoringArea) {
xTarget += robot.wobbleScoreX;
yTarget += robot.wobbleScoreY;
}
if (faceAngle == 360) {
faceAngle = robot.getRelativeAngle(180, robot.getAngleToPosition(xTarget,yTarget));
}
}

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

@@ -58,6 +58,12 @@ public class Face extends CyberarmState {
double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
faceAngle = robot.getAngleToPosition(x, y);
}
if (faceAngle == 360) {
double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value());
double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
faceAngle = robot.getRelativeAngle(180,robot.getAngleToPosition(x, y));
}
}
}

76
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/FindWobbleGoal.java Normal file
View File

@@ -0,0 +1,76 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class FindWobbleGoal extends CyberarmState {
private Robot robot;
private String groupName;
private String actionName;
private double power;
private double turnCheck;
private double driveCheck;
private double range;
private boolean cCheckPrev;
private boolean ccCheckPrev;
public FindWobbleGoal(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
}
@Override
public void init() {
power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value();
driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value();
range = robot.stateConfiguration.variable(groupName,actionName,"r").value();
}
@Override
public void start() {
setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
robot.setDrivePower(power,-power,power,-power);
}
@Override
public void exec() {
robot.updateLocation();
double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
if (sensorValue > turnCheck) {
boolean cCheck = robot.getRotation() > range;
boolean ccCheck = robot.getRotation() < -range;
if (cCheck && !cCheckPrev) {
robot.setDrivePower(-power, power, -power, power);
}
if (ccCheck && !ccCheckPrev) {
robot.setDrivePower(power, -power, power, -power);
}
cCheckPrev = cCheck;
ccCheckPrev = ccCheck;
} else {
if (sensorValue > driveCheck) {
robot.setDrivePower(-power,-power,-power,-power);
} else {
robot.setDrivePower(0,0,0,0);
setHasFinished(true);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("sensor", robot.wobbleColorSensor.getDistance(DistanceUnit.MM));
engine.telemetry.addData("red", robot.wobbleColorSensor.red());
engine.telemetry.addData("green", robot.wobbleColorSensor.green());
engine.telemetry.addData("blue", robot.wobbleColorSensor.blue());
}
}

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

@@ -0,0 +1,41 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class LaunchDriveControl extends CyberarmState {
private Robot robot;
private CyberarmState launchState;
private CyberarmState driveState;
private boolean checkConditionPrev;
public LaunchDriveControl(Robot robot, CyberarmState launchState, CyberarmState driveState) {
this.robot = robot;
this.launchState = launchState;
this.driveState = driveState;
}
@Override
public void start() {
addParallelState(launchState);
}
@Override
public void exec() {
boolean checkCondition = (robot.ringBeltStage > 3);
if (checkCondition && !checkConditionPrev) {
addParallelState(driveState);
}
checkConditionPrev = checkCondition;
setHasFinished(childrenHaveFinished());
}
@Override
public void telemetry() {
engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
engine.telemetry.addData("check prev", checkConditionPrev);
engine.telemetry.addData("drive prev", checkConditionPrev);
}
}

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

@@ -9,6 +9,8 @@ public class Launch extends CyberarmState {
private String actionName;
boolean hasCycled;
boolean detectedPass;
private boolean reduceConditionPrev;
private double reducePos;
public Launch(Robot robot) {
this.robot = robot;
@@ -24,12 +26,12 @@ public class Launch extends CyberarmState {
public void start() {
try {
if (robot.stateConfiguration.action(groupName, actionName).enabled) {
robot.ringBeltOn();
robot.ringBeltMotor.setPower(0.5);
} else {
setHasFinished(true);
}
} catch (NullPointerException e){
robot.ringBeltOn();
robot.ringBeltMotor.setPower(0.5);
}
}
@@ -37,9 +39,12 @@ public class Launch extends CyberarmState {
public void exec() {
//detect when limit switch is initially triggered
boolean detectingPass = robot.limitSwitch.isPressed();
int beltPos = robot.getBeltPos();
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;
@@ -51,9 +56,24 @@ public class Launch extends CyberarmState {
setHasFinished(true);
} else {
hasCycled = true;
reducePos = robot.loopPos((int) (beltPos + (1.5 * Robot.RING_BELT_GAP)));
}
}
detectedPass = detectingPass;
boolean reduceCondition = (hasCycled && beltPos > reducePos && beltPos < reducePos + Robot.RING_BELT_GAP);
if (reduceCondition && !reduceConditionPrev){
robot.ringBeltOn();
//the ring belt stage lets other states know that the robot has finished launching all three rings
//and is now returning to loading position.
robot.ringBeltStage += 1;
}
reduceConditionPrev = reduceCondition;
}
@Override
public void telemetry() {
engine.telemetry.addData("hasCycled", hasCycled);
}
}

5
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/PreInit/LoadRings.java
View File

@@ -26,5 +26,10 @@ public class LoadRings extends CyberarmState {
addParallelState(ringBeltState);
}
if (robot.ringBeltStage > 2) {
robot.launchMotor.setPower(0);
setHasFinished(true);
}
}
}

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

@@ -98,7 +98,7 @@ public class Robot {
//Launcher
public DcMotor launchMotor;
public static final double LAUNCH_POWER = 0.7;
public static final double LAUNCH_POWER = 0.715;
private static final long LAUNCH_ACCEL_TIME = 500;
public double launchPositionX;
@@ -212,7 +212,7 @@ public class Robot {
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");
@@ -364,8 +364,8 @@ public class Robot {
rotationPrevious = imuAngle;
//The forward Vector has the luxury of having an odometer on both sides of the robot.
//This allows us to eliminate the unwanted influence of turning the robot by averaging
//the two.
//This allows us to reduce the unwanted influence of turning the robot by averaging
//the two. unfortunatly we the current positioning of the odometers
//Since there isn't a second wheel to remove the influence of robot rotation, we have to
//instead do this by approximating the number of ticks that were removed due to rotation
@@ -474,6 +474,10 @@ public class Robot {
return locationY;
}
public void resetRotation(float rotation) {
this.rotation = rotation;
}
//Manually set the position of the robot on the field.
public void setCurrentPosition(float rotation, double x, double y) {
this.rotation = rotation;
@@ -541,27 +545,20 @@ public class Robot {
double p = y + x;
double q = y - x;
//calculating correction needed to steer the robot towards the degreesDirectionFace
float relativeRotation = getRelativeAngle(degreesDirectionFace, rotation);
double turnCorrection = Math.pow(LARGE_CORRECTION * relativeRotation, 3) + FINE_CORRECTION * relativeRotation;
float relativeRotation =
getRelativeAngle(degreesDirectionFace, rotation);
double turnCorrection =
Math.pow(LARGE_CORRECTION * relativeRotation, 3) +
FINE_CORRECTION * relativeRotation;
double powerForwardRight = scalar * (q + turnCorrection);
double powerForwardLeft = scalar * (p - turnCorrection);
double powerBackRight = scalar * (p + turnCorrection);
double powerBackLeft = scalar * (q - turnCorrection);
// //the turnCorrection often results in powers with magnitudes significantly larger than the
// //scalar. The scaleRatio mitigates this without altering the quality of the motion by making
// //it so that the average of the four magnitudes is equal to the scalar magnitude.
// double powerSum = Math.abs(powerForwardRight) + Math.abs(powerForwardLeft) +
// Math.abs(powerBackRight) + Math.abs(powerBackLeft);
// double scaleRatio = (4 * Math.abs(scalar))/powerSum;
//
// powerForwardRight *= scaleRatio;
// powerForwardLeft *= scaleRatio;
// powerBackRight *= scaleRatio;
// powerBackLeft *= scaleRatio;
if (relativeRotation != 0) {
double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation));
@@ -588,15 +585,6 @@ public class Robot {
powerBackLeft = powerBackLeft/extreme;
}
// double powerControlThreshold = 0.6 * ;
//
// if (Math.min(extreme, 1) > powerControlThreshold) {
// powerForwardLeft *= powerControlThreshold;
// powerForwardRight *= powerControlThreshold;
// powerBackLeft *= powerControlThreshold;
// powerBackRight *= powerControlThreshold;
// }
double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
return powers;

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

@@ -1,5 +1,7 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
@@ -27,18 +29,23 @@ public class TeleOpState extends CyberarmState {
private double[] powers = {0,0,0,0};
private double drivePower = 1;
private static final double TURN_POWER = 1;
private static final double LAUNCH_TOLERANCE_FACE = 1;
private Launch launchState;
private boolean launching;
private ProgressRingBelt ringBeltState;
private boolean CollectorOn;
private boolean xPrev;
private boolean rbPrev;
private boolean yPrev;
private boolean aPrev;
private boolean xPrev;
private boolean bPrev;
private boolean lbPrev;
private boolean wobbleArmUp = true;
private boolean wobbleArmUp = false;
private boolean wobbleGrabOpen = false;
private boolean emergencyLaunchPrev;
private double beltPowerPrev;
private boolean reverseBeltPrev;
private int manualArmHoldPos;
private boolean manualArmHold;
private boolean launchInput = false;
@@ -47,11 +54,17 @@ public class TeleOpState extends CyberarmState {
this.robot = robot;
}
private double LAUNCH_TOLERANCE_POS;
@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();
robot.wobbleArmMotor.setTargetPosition(0);
robot.wobbleArmMotor.setPower(0.5);
LAUNCH_TOLERANCE_POS = robot.inchesToTicks(3);
robot.resetRotation(180);
}
@Override
@@ -59,25 +72,6 @@ public class TeleOpState extends CyberarmState {
robot.updateLocation();
robot.record(""+robot.ticksToInches(robot.getLocationX())+", "+robot.ticksToInches(robot.getLocationY()));
double leftJoystickX = engine.gamepad1.left_stick_x;
double leftJoystickY = engine.gamepad1.left_stick_y;
leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
double rightJoystickX = engine.gamepad1.right_stick_x;
double rightJoystickY = engine.gamepad1.right_stick_y;
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
//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;
boolean lb = engine.gamepad1.left_stick_button;
if (lb && !lbPrev) {
if (drivePower == 1) {
@@ -91,37 +85,58 @@ public class TeleOpState extends CyberarmState {
double[] powers = {0,0,0,0};
boolean y = engine.gamepad1.y;
if (y) {
//Launch Sequence
//Drive to Launch Pos
double distanceToTarget = Math.hypot(robot.launchPositionX - robot.getLocationX(), robot.launchPositionY - robot.getLocationY());
if (distanceToTarget > Robot.LAUNCH_TOLERANCE_POS) {
if (distanceToTarget > LAUNCH_TOLERANCE_POS) {
powers = robot.getMecanumPowers(robot.getAngleToPosition(robot.launchPositionX, robot.launchPositionY), drivePower, robot.launchRotation);
} else if (robot.getRelativeAngle(robot.launchRotation, robot.getRotation()) > Robot.LAUNCH_TOLERANCE_FACE) {
launchState = new Launch(robot);
addParallelState(launchState);
} else {
} else if (Math.abs(robot.getRelativeAngle(robot.launchRotation,robot.getRotation())) > LAUNCH_TOLERANCE_FACE) {
double[] facePowers = robot.getFacePowers(robot.launchRotation, TURN_POWER);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {
}
}
if (!y || ( launchState != null && launchState.getHasFinished())) {
} else {
//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);
//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;
//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.resetRotation(faceDirection);
}
if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(faceDirection, TURN_POWER);
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);
}
@@ -137,11 +152,11 @@ public class TeleOpState extends CyberarmState {
robot.collectionMotor.setPower(0);
}
boolean x = engine.gamepad2.x;
if (x && !xPrev && childrenHaveFinished()) {
boolean rb = engine.gamepad2.right_bumper;
if (rb && !rbPrev && childrenHaveFinished()) {
addParallelState(new ProgressRingBelt(robot));
}
xPrev = x;
rbPrev = rb;
boolean y2 = engine.gamepad2.y;
if (y2 && !yPrev && childrenHaveFinished()) {
@@ -150,24 +165,64 @@ public class TeleOpState extends CyberarmState {
}
yPrev = y2;
boolean a = engine.gamepad2.a;
if (a && !aPrev && childrenHaveFinished()) {
boolean x = engine.gamepad2.x;
if (x && !xPrev) {
wobbleGrabOpen = !wobbleGrabOpen;
addParallelState(new WobbleGrab( robot, wobbleGrabOpen, 100));
if (wobbleGrabOpen) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
} else {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX * 0.1 );
}
}
aPrev = a;
xPrev = x;
boolean b = engine.gamepad2.b;
if (b && !bPrev && childrenHaveFinished()) {
if (b && !bPrev) {
wobbleArmUp = !wobbleArmUp;
addParallelState(new WobbleArm(robot, wobbleArmUp, 100));
if (wobbleArmUp) {
robot.wobbleArmMotor.setTargetPosition(550);
} else {
robot.wobbleArmMotor.setTargetPosition(0);
}
}
bPrev = b;
if (engine.gamepad2.right_bumper && engine.gamepad2.left_bumper) {
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
boolean emergencyLaunch = engine.gamepad2.a;
if (emergencyLaunch && !emergencyLaunchPrev) {
if (robot.launchMotor.getPower() == 0) {
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
} else {
robot.launchMotor.setPower(0);
}
}
emergencyLaunchPrev = emergencyLaunch;
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.5);
manualArmHold = true;
} else if (manualArmHold) {
manualArmHold = false;
robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
boolean reverseBelt = engine.gamepad2.left_bumper;
if (reverseBelt && !reverseBeltPrev) {
beltPowerPrev = robot.ringBeltMotor.getPower();
robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
}
if (!reverseBelt && reverseBeltPrev) {
robot.ringBeltMotor.setPower(beltPowerPrev);
}
reverseBeltPrev = reverseBelt;
}
@@ -181,9 +236,7 @@ public class TeleOpState extends CyberarmState {
}
}
engine.telemetry.addData("Vision Z", robot.visionZ);
engine.telemetry.addData("belt stage", robot.ringBeltStage);
engine.telemetry.addData("wobble Arm Up", wobbleArmUp);
engine.telemetry.addLine("Location");
engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");

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

@@ -12,7 +12,7 @@ public class WobbleArm extends CyberarmState {
public WobbleArm(Robot robot, String groupName, String actionName, boolean armUp) {
this.robot = robot;
this.groupName = groupName;
this.groupName = groupName;
this.actionName = actionName;
this.armUp = armUp;
}

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

@@ -9,6 +9,7 @@ public class WobbleGrab extends CyberarmState {
private String actionName;
private boolean open;
private long waitTime = -1;
private boolean enabled;
public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) {
this.robot = robot;
@@ -26,13 +27,14 @@ public class WobbleGrab extends CyberarmState {
@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 (robot.stateConfiguration.action(groupName, actionName).enabled) {
if (enabled) {
if (open) {
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_MAX);
} else {

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

@@ -3,22 +3,23 @@ package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
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 ServoPosTest());
addState(new FindWobbleGoal(robot, "auto", "08_0"));
}
// @Override