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

Upload Day

Browse Source
This commit is contained in:
Nathaniel Palme
2021-04-06 17:43:12 -05:00
parent 395f679e87
commit 59d79b117e
13 changed files with 407 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -12,11 +12,13 @@ public class DriveToCoordinates extends CyberarmState {
private double yTarget;
private float faceAngle;
private double tolerancePos;
private double power;
private double basePower;
private boolean braking;
private long breakStartTime;
private long brakeTime;
private boolean autoFace;
private double decelRange;
private double decelMin;
private boolean scoringArea;
public DriveToCoordinates(Robot robot, String groupName, String actionName) {
@@ -38,25 +40,49 @@ public class DriveToCoordinates extends CyberarmState {
this.yTarget = yTarget;
this.faceAngle = faceAngle;
this.tolerancePos = tolerance;
this.power = power;
this.basePower = power;
this.brakeTime = brakeTime;
}
public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime, double decelRange, double decelMin) {
this.robot = robot;
this.xTarget = xTarget;
this.yTarget = yTarget;
this.faceAngle = faceAngle;
this.tolerancePos = tolerance;
this.basePower = power;
this.brakeTime = brakeTime;
this.decelRange = decelRange;
this.decelMin = decelMin;
}
@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();
basePower = 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();
try {
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
} catch (NullPointerException e) {
} catch (RuntimeException e) {
autoFace = true;
}
try {
decelRange = robot.stateConfiguration.variable(groupName, actionName, "decelR").value();
decelMin = robot.stateConfiguration.variable(groupName, actionName, "decelM").value();
} catch (RuntimeException e) {
decelRange = 0;
}
}
if (decelRange > 0) {
decelRange = robot.inchesToTicks(decelRange);
}
}
@Override
@@ -87,9 +113,16 @@ public class DriveToCoordinates extends CyberarmState {
double distanceToTarget = Math.hypot(xTarget - robot.getLocationX(), yTarget - robot.getLocationY());
// deceleration
double power = basePower;
if (distanceToTarget < decelRange) {
power = ((distanceToTarget / decelRange) * (basePower - decelMin)) + decelMin;
}
double[] powers = robot.getMecanumPowers(robot.getAngleToPosition(xTarget,yTarget), power, faceAngle);
robot.setDrivePower(powers[0], powers[1],powers[2],powers[3]);
//Once the robot is within the acceptable range,
if (distanceToTarget > tolerancePos) {
braking = false;
} else {

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

@@ -18,6 +18,7 @@ public class FindWobbleGoal extends CyberarmState {
private float startRotation;
private boolean foundGoalRotation;
private float wobbleGoalRotation;
private long timeLimit;
public FindWobbleGoal(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -31,6 +32,7 @@ public class FindWobbleGoal extends CyberarmState {
turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value();
driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value();
range = robot.stateConfiguration.variable(groupName,actionName,"r").value();
timeLimit = robot.stateConfiguration.variable(groupName,actionName,"limit").value();
}
@Override
@@ -71,11 +73,19 @@ public class FindWobbleGoal extends CyberarmState {
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);
endSearch();
}
}
if (runTime() > timeLimit) {
endSearch();
}
}
private void endSearch() {
robot.setDrivePower(0,0,0,0);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
setHasFinished(true);
}
@Override

9
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Autonomous/Park.java
View File

@@ -15,6 +15,8 @@ public class Park extends CyberarmState {
double parkTolerance;
double parkPower;
long parkBrakeTime;
double parkDecelRange;
double parkDecelMin;
public Park(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -29,12 +31,15 @@ public class Park extends CyberarmState {
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();
parkDecelMin = robot.stateConfiguration.variable(groupName,actionName,"decelM").value();
parkDecelRange = robot.stateConfiguration.variable(groupName,actionName,"decelR").value();
}
@Override
public void start() {
if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8))
addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower,parkBrakeTime));
if (Math.abs(robot.getLocationY()) > robot.inchesToTicks(8)) {
addParallelState(new DriveToCoordinates(robot, robot.getLocationX(), parkY, parkFaceAngle, parkTolerance, parkPower, parkBrakeTime,parkDecelRange,parkDecelMin));
}
}
@Override

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

@@ -31,7 +31,7 @@ public class ProgressRingBelt extends CyberarmState {
targetPos = currentPos + robot.ringBeltGap;
prep();
} else if (robot.ringBeltStage == 2) {
targetPos = currentPos + 240;
targetPos = currentPos + 150;
prep();
prepLaunch = !robot.initLauncher;
} else if (robot.ringBeltStage > 2) {

57
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ResetRingBelt.java Normal file
View File

@@ -0,0 +1,57 @@
package org.timecrafters.UltimateGoal.Competition;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
public class ResetRingBelt extends CyberarmState {
private Robot robot;
boolean detectedPass;
int reducePos;
boolean reduceConditionPrev;
public ResetRingBelt(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
reducePos = robot.ringBeltMotor.getCurrentPosition() + robot.reduceLaunchPos;
}
@Override
public void exec() {
//detect when limit switch is initially triggered
boolean detectingPass = robot.limitSwitch.isPressed();
int beltPos = robot.ringBeltMotor.getCurrentPosition();
if (detectingPass && !detectedPass) {
//finish once the ring belt has cycled all the way through and then returned to
//the first receiving position.
robot.ringBeltStage = 0;
robot.ringBeltMotor.setTargetPosition(beltPos);
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.launchMotor.setPower(0);
setHasFinished(true);
}
detectedPass = detectingPass;
boolean reduceCondition = (beltPos > reducePos);
if (reduceCondition && !reduceConditionPrev){
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.
}
reduceConditionPrev = reduceCondition;
}
}

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

@@ -10,7 +10,6 @@ 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.RevBlinkinLedDriver;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.hardware.rev.RevTouchSensor;
@@ -86,6 +85,7 @@ public class Robot {
static final double FACE_MOMENTUM_MAX_CORRECTION = 1.1;
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));
static final double ZERO_POWER_THRESHOLD = 0.25;
//Unit Conversion Constants
static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
@@ -432,19 +432,13 @@ public class Robot {
locationY += yChange;
Robot.rotation += rotationChange;
Robot.rotation = scaleAngleRange(Robot.rotation);
totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
if (Robot.rotation > 180) {
Robot.rotation -= 360;
}
if (Robot.rotation < -180) {
Robot.rotation += 360;
}
}
public void syncIfStationary() {
if (totalV < minCheckVelocity) {
long timeCurrent = System.currentTimeMillis();
@@ -551,16 +545,7 @@ public class Robot {
//to the right (clockwise) of the current. Negative angles indicate that the reference is to the
//left.
public float getRelativeAngle(float reference, float current) {
float relative = current - reference;
if (relative < -180) {
relative += 360;
}
if (relative > 180) {
relative -= 360;
}
return relative;
return scaleAngleRange(current - reference);
}
//Drive Functions
@@ -628,6 +613,15 @@ public class Robot {
double[] powers = {powerForwardLeft, powerForwardRight, powerBackLeft, powerBackRight};
double totalPower = Math.abs(powerForwardLeft) +
Math.abs(powerForwardRight) +
Math.abs(powerBackLeft) +
Math.abs(powerBackRight);
if (totalPower < ZERO_POWER_THRESHOLD) {
powers = new double[] {0,0,0,0};
}
return powers;
}
@@ -635,7 +629,8 @@ public class Robot {
public double[] getFacePowers(float direction, double power) {
angularVelocity = imu.getAngularVelocity().xRotationRate;
double relativeAngle = getRelativeAngle(direction, Robot.rotation);
double scaler = Math.pow(FACE_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;
@@ -645,7 +640,8 @@ public class Robot {
if (relativeAngle != 0) {
double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle));
double exponential = Math.pow(FACE_MOMENTUM_CORRECTION, FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
double exponential = Math.pow(FACE_MOMENTUM_CORRECTION,
FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
scaler *= momentumCorrection;
@@ -655,6 +651,12 @@ public class Robot {
double right = power *scaler;
double[] powers = {left,right};
double totalPower = 2 * (Math.abs(left) + Math.abs(right));
if (totalPower < ZERO_POWER_THRESHOLD) {
powers = new double[] {0,0};
}
return powers;
}
@@ -665,6 +667,16 @@ public class Robot {
return notMoved;
}
public float scaleAngleRange(float angle) {
if (angle < -180) {
angle += 360;
}
if (angle > 180) {
angle -= 360;
}
return angle;
}
public void record(String record) {
TestingRecord+="\n"+record;
}

58
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java Normal file
View File

@@ -0,0 +1,58 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
import org.timecrafters.UltimateGoal.Competition.ResetRingBelt;
import org.timecrafters.UltimateGoal.Competition.Robot;
public class LaunchControl extends CyberarmState {
private Robot robot;
private boolean ranLaunch;
public LaunchControl(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
if (robot.ringBeltStage > 2) {
if (robot.ringBeltStage > 4) {
addParallelState(new ResetRingBelt(robot));
} else {
addParallelState(new LaunchSingle(robot));
}
ranLaunch = true;
}
}
@Override
public void exec() {
if (childrenHaveFinished()) {
if (!ranLaunch) {
if (robot.ringBeltStage <= 2) {
addParallelState(new ProgressRingBelt(robot));
} else {
addParallelState(new LaunchSingle(robot));
ranLaunch = true;
}
} else {
setHasFinished(true);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("Launch Control children", childrenHaveFinished());
for (CyberarmState state : children) {
if (!state.getHasFinished()) {
engine.telemetry.addLine("" + state.getClass());
}
}
}
}

52
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchSingle.java Normal file
View File

@@ -0,0 +1,52 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.UnstickRingBelt;
public class LaunchSingle extends CyberarmState {
private Robot robot;
boolean hasCycled;
boolean detectedPass;
private int targetPos;
private boolean reduceConditionPrev;
private double reducePos;
private long stuckStartTime;
public LaunchSingle(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
robot.ringBeltStage +=1;
robot.ringBeltMotor.setTargetPosition(robot.ringBeltMotor.getCurrentPosition() + robot.ringBeltGap);
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
}
@Override
public void exec() {
int currentPos = robot.ringBeltMotor.getCurrentPosition();
if (currentPos >= targetPos) {
setHasFinished(true);
}
if (robot.beltIsStuck() && childrenHaveFinished()) {
long currentTime = System.currentTimeMillis();
if (stuckStartTime == 0) {
stuckStartTime = currentTime;
} else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
addParallelState(new UnstickRingBelt(robot));
}
} else {
stuckStartTime = 0;
}
}
}

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

@@ -33,18 +33,15 @@ public class Player1 extends CyberarmState {
private boolean aPrev;
//Drive to launch control
private DriveToCoordinates driveToLaunch;
private powerShotsControl powerShots;
private boolean runNextDriveToLaunch;
private boolean driveToLaunchInputPrev;
private double launchTolerance;
private double launchPower;
private long launchBrakeTime;
private double endGameX;
private double endGameY;
private float endGameRot;
private float launchAngleGoal;
private float launchAnglePower1;
private float launchAnglePower2;
private float launchAnglePower3;
private double refinePower;
public Player1(Robot robot) {
this.robot = robot;
@@ -55,15 +52,11 @@ public class Player1 extends CyberarmState {
cardinalSnapping = robot.stateConfiguration.variable("tele","control", "cardinalSnapping").value();
pairSnapping = robot.stateConfiguration.variable("tele","control", "pairSnapping").value();
faceControlThreshold = robot.stateConfiguration.variable("tele","control", "faceControlT").value();
refinePower = robot.stateConfiguration.variable("tele","control", "refPower").value();
launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("tele","launchPosG","tolPos").value());
launchPower = robot.stateConfiguration.variable("tele","launchPosG","power").value();
launchBrakeTime = robot.stateConfiguration.variable("tele","launchPosG","brakeMS").value();
launchAngleGoal = robot.stateConfiguration.variable("tele","launchAngles","goal").value();
launchAnglePower1 = robot.stateConfiguration.variable("tele","launchAngles","p1").value();
launchAnglePower2 = robot.stateConfiguration.variable("tele","launchAngles","p2").value();
launchAnglePower3 = robot.stateConfiguration.variable("tele","launchAngles","p3").value();
endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
}
@Override
@@ -85,9 +78,6 @@ public class Player1 extends CyberarmState {
}
lbPrev = lb;
if (engine.gamepad1.guide) {
robot.syncIfStationary();
}
runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
@@ -123,20 +113,25 @@ public class Player1 extends CyberarmState {
}
aPrev = a;
runNextDriveToLaunch = (driveToLaunch == null || driveToLaunch.getHasFinished());
runNextDriveToLaunch = (powerShots == null || powerShots.getHasFinished());
boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput;
if (driveToLaunchInput) {
if (runNextDriveToLaunch && !driveToLaunchInputPrev) {
driveToLaunch = new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime);
addParallelState(driveToLaunch);
powerShots = new powerShotsControl(robot);
addParallelState(powerShots);
}
faceDirection = robot.getRotation();
} else if (!runNextDriveToLaunch) {
driveToLaunch.setHasFinished(true);
powerShots.setHasFinished(true);
}
driveToLaunchInputPrev = driveToLaunchInput;
// if (engine.gamepad1.y) {
// robot.setLocalization(endGameRot,endGameX,endGameY);
// setHasFinished(true);
// }
if (childrenHaveFinished()) {
//Normal Driver Controls
@@ -156,8 +151,14 @@ public class Player1 extends CyberarmState {
//so that the controller and robot can be re-synced in the event of a bad initial
//position.
if (engine.gamepad1.back) {
robot.setLocalization(rightJoystickDegrees, robot.getLocationX(), robot.getLocationY());
faceDirection = rightJoystickDegrees;
float newRotation = 0;
if (rightJoystickMagnitude != 0) {
newRotation = rightJoystickDegrees;
}
robot.setLocalization(newRotation, robot.getLocationX(), robot.getLocationY());
faceDirection = newRotation;
}
//if the driver is letting go of the face joystick, the robot should maintain it's current face direction.
@@ -168,18 +169,13 @@ public class Player1 extends CyberarmState {
}
rightJoystickMagnitudePrevious = rightJoystickMagnitude;
//sets the launch positions to
if (engine.gamepad1.dpad_up) {
faceDirection = launchAngleGoal;
} else if (engine.gamepad1.dpad_right) {
faceDirection = launchAnglePower1;
} else if (engine.gamepad1.dpad_down) {
faceDirection = launchAnglePower2;
if (engine.gamepad1.dpad_right) {
powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
faceDirection = robot.getRotation();
} else if (engine.gamepad1.dpad_left) {
faceDirection = launchAnglePower3;
}
if (leftJoystickMagnitude == 0) {
powers = new double[]{-refinePower, refinePower, -refinePower, refinePower};
faceDirection = robot.getRotation();
} else if (leftJoystickMagnitude == 0) {
double[] facePowers = robot.getFacePowers(faceDirection, 0.4);
powers = new double[]{facePowers[0], facePowers[1], facePowers[0], facePowers[1]};
} else {

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

@@ -58,33 +58,29 @@ public class Player2 extends CyberarmState {
robot.collectionMotor.setPower(0);
}
//belt progression control
boolean rb = engine.gamepad2.right_bumper;
if (rb && !rbPrev && childrenHaveFinished()) {
addParallelState(new ProgressRingBelt(robot));
if (childrenHaveFinished()) {
//belt progression control
boolean rb = engine.gamepad2.right_bumper;
if (rb && !rbPrev) {
addParallelState(new ProgressRingBelt(robot));
}
rbPrev = rb;
//main launch sequence control
boolean y2 = engine.gamepad2.y;
if (y2 && !yPrev) {
addParallelState(new Launch(robot));
}
yPrev = y2;
//special launch sequence for single shots
boolean x = engine.gamepad2.x;
if (x && !bPrev) {
addParallelState(new LaunchControl(robot));
}
bPrev = x;
}
rbPrev = rb;
//launch sequence control
boolean y2 = engine.gamepad2.y;
if (y2 && !yPrev && childrenHaveFinished()) {
addParallelState(new Launch(robot));
}
yPrev = y2;
//toggles the wobble arm up and down.
// 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;
@@ -142,8 +138,11 @@ public class Player2 extends CyberarmState {
robot.ringBeltMotor.setPower(beltPowerPrev);
robot.ringBeltMotor.setMode(runModePrev);
}
lbPrev = lb;
// if (engine.gamepad1.y) {
// setHasFinished(true);
// }
}
private void setArmMode(DcMotor.RunMode runMode) {
@@ -159,14 +158,15 @@ public class Player2 extends CyberarmState {
// 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()) {
engine.telemetry.addLine("" + state.getClass());
}
}
// engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
// 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()) {
// engine.telemetry.addLine("" + state.getClass());
// }
// }
}
}

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

@@ -3,6 +3,8 @@ package org.timecrafters.UltimateGoal.Competition.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
import org.timecrafters.UltimateGoal.Competition.Robot;
@TeleOp (name = "TeleOp",group = "comp")
@@ -14,7 +16,7 @@ public class TeleOpEngine extends CyberarmEngine {
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
robot.webCamServo.setPosition(0);
super.init();
}
@@ -22,6 +24,8 @@ public class TeleOpEngine extends CyberarmEngine {
@Override
public void setup() {
addState(new TeleOpState(robot));
}
@Override

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

@@ -25,7 +25,7 @@ public class TeleOpState extends CyberarmState {
@Override
public void exec() {
// setHasFinished(childrenHaveFinished());
}
@Override

75
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java Normal file
View File

@@ -0,0 +1,75 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.util.ArrayList;
public class powerShotsControl extends CyberarmState {
private Robot robot;
private double endGameX;
private double endGameY;
private float endGameRot;
private int nextState = 0;
private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
public powerShotsControl(Robot robot) {
this.robot = robot;
}
@Override
public void init() {
endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
endGameX = robot.inchesToTicks(endGameX);
endGameY = robot.inchesToTicks(endGameY);
endGameRot = (float) robot.inchesToTicks(endGameRot);
states.add(new DriveToCoordinates(robot, "tele", "_pow1"));
states.add(new Face(robot,"tele","_faceZero"));
states.add(new LaunchControl(robot));
states.add(new DriveToCoordinates(robot, "tele", "_pow2"));
states.add(new Face(robot,"tele","_faceZero"));
states.add(new LaunchControl(robot));
states.add(new DriveToCoordinates(robot, "tele", "_pow3"));
states.add(new Face(robot,"tele","_faceZero"));
states.add(new LaunchControl(robot));
}
@Override
public void start() {
robot.setLocalization(endGameRot,endGameX,endGameY);
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
}
@Override
public void exec() {
if (childrenHaveFinished()) {
if (nextState < states.size()) {
addParallelState(states.get(nextState));
nextState += 1;
} else {
robot.launchMotor.setPower(0);
setHasFinished(true);
}
}
}
@Override
public void telemetry() {
engine.telemetry.addData("Power shots children", childrenHaveFinished());
for (CyberarmState state : children) {
if (!state.getHasFinished()) {
engine.telemetry.addLine("" + state.getClass());
}
}
}
}