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

Some Small preparations for Java Class && the County Fair Demo stuff

Browse Source
This commit is contained in:
Nathaniel Palme
2021-08-03 17:40:35 -05:00
parent 93fcfef1c6
commit 3fdbbb7832
29 changed files with 1346 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.idea/compiler.xml generated
View File

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

2
.idea/misc.xml generated
View File

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

2
TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java
View File

@@ -150,7 +150,7 @@ public abstract class CyberarmEngine extends OpMode {
* Recursively stop states
* @param state State to stop
*/
private void stopState(CyberarmState state) {
public void stopState(CyberarmState state) {
state.setHasFinished(true);
state.stop();

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

@@ -1,10 +1,11 @@
package org.timecrafters.UltimateGoal.Calibration;
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;
@Disabled
@TeleOp (name = "Calibration", group = "test")
public class CalibrationEngine extends CyberarmEngine {
private Robot robot;

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

@@ -35,7 +35,7 @@ public class AutoEngine extends CyberarmEngine {
// since we've preloaded three rings, the ring belt stage is set to account for this;
robot.ringBeltStage = 3;
//Autonomous specific Variables
//Autonomous specific Configuration 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());

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

@@ -1,5 +1,9 @@
package org.timecrafters.UltimateGoal.Competition.Autonomous;
/*
The FindWobbleGoal state is used in teleOp and Autonomous to aid in capturing the wobble goal.
*/
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.UltimateGoal.Competition.Robot;
@@ -47,6 +51,7 @@ public class FindWobbleGoal extends CyberarmState {
robot.updateLocation();
double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
//Stage 1: scan back and forth untile the sensor is in line with the wobble goal.
if (sensorValue > turnCheck) {
float rotation = robot.getRelativeAngle(startRotation,robot.getRotation());
@@ -65,18 +70,23 @@ public class FindWobbleGoal extends CyberarmState {
ccCheckPrev = ccCheck;
} else {
//Stage 2: drive toward wobble goal until it's close enough to grab
if (sensorValue > driveCheck) {
if (!foundGoalRotation) {
foundGoalRotation = true;
wobbleGoalRotation = robot.getRotation();
}
double[] powers = robot.getMecanumPowers(wobbleGoalRotation - 90, power*2 , wobbleGoalRotation);
double[] powers = robot.getMecanumPowers(
wobbleGoalRotation - 90,
power*2 , wobbleGoalRotation);
robot.setDrivePower(powers[0],powers[1],powers[2],powers[3]);
} else {
//stage 3: grab the wobble goal && finish the state
endSearch();
}
}
//if the search takes too long, the robot grabs and finishes the state
if (runTime() > timeLimit) {
endSearch();
}

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

@@ -1,39 +0,0 @@
package org.timecrafters.UltimateGoal.Competition.Demo;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.TeleOp.Player1;
import org.timecrafters.UltimateGoal.Competition.TeleOp.Player2;
public class DemoControl extends CyberarmState {
private Robot robot;
public DemoControl(Robot robot) {
this.robot = robot;
}
@Override
public void start() {
// addParallelState(new Demo1(robot));
// addParallelState(new Demo2(robot));
}
@Override
public void exec() {
}
@Override
public void telemetry() {
engine.telemetry.addLine("Location");
engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
engine.telemetry.addData("Rotation ", robot.getRotation());
}
private float round(double number,double unit) {
return (float) (Math.floor(number/unit) * unit);
}
}

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

@@ -1,14 +1,12 @@
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")
@TeleOp (name = "Demo: Game",group = "demo")
public class DemoEngine extends CyberarmEngine {
private Robot robot;
@@ -17,14 +15,14 @@ public class DemoEngine extends CyberarmEngine {
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
robot.wobbleGrabServo.setPosition(0);
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
robot.webCamServo.setPosition(0);
super.init();
}
@Override
public void setup() {
addState(new DemoControl(robot));
addState(new DemoState(robot));
}
@Override

33
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoEngineTank.java Normal file
View File

@@ -0,0 +1,33 @@
package org.timecrafters.UltimateGoal.Competition.Demo;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
@TeleOp (name = "Demo: Tank",group = "demo")
public class DemoEngineTank extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
robot.webCamServo.setPosition(0);
super.init();
}
@Override
public void setup() {
addState(new DemoStateTank(robot));
}
@Override
public void stop() {
robot.deactivateVuforia();
robot.saveRecording();
super.stop();
}
}

194
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoState.java Normal file
View File

@@ -0,0 +1,194 @@
package org.timecrafters.UltimateGoal.Competition.Demo;
/*
The Player1 state has all the controls for player one. The Player One and Player Two controls are
kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
from running at the same time that shouldn't be.
*/
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl;
public class DemoState 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;
private double refinePower;
public DemoState(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();
refinePower = robot.stateConfiguration.variable(
"tele","control", "refPower").value();
}
@Override
public void start() {
faceDirection = robot.getRotation();
}
@Override
public void exec() {
robot.updateLocation();
Gamepad gamepad = engine.gamepad1;
if (engine.gamepad2.right_trigger != 0) {
gamepad = engine.gamepad2;
}
//toggle for drive speed
boolean lb = engine.gamepad2.left_stick_button;
if (lb && !lbPrev) {
if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
}
lbPrev = lb;
//Calculate Joystick Vector
double leftJoystickX = gamepad.left_stick_x;
double leftJoystickY = gamepad.left_stick_y;
leftJoystickDegrees = robot.getRelativeAngle(0,
(float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
double rightJoystickX = gamepad.right_stick_x;
double rightJoystickY = gamepad.right_stick_y;
rightJoystickDegrees = robot.getRelativeAngle(0,
(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 that the controller and robot can be re-synced in the event of a bad initial
//rotation.
if (gamepad.back) {
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.
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;
//The D-pad is used if the drivers need to get a more precise angle than they can get
//using the face joystick
if (gamepad.dpad_right) {
powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
faceDirection = robot.getRotation();
} else if (gamepad.dpad_left) {
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 {
//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]);
//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 );
} else if (ringBeltPower < 0) {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);
} else {
if (drivePower == 1) {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
} else {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GOLD);
}
}
}
@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());
}
}
}
//This just checks if the wobble arm runmode is already the desired mode before setting it.
//I don't know if this is actually helpful
private void setArmMode(DcMotor.RunMode runMode) {
if (robot.wobbleArmMotor.getMode() != runMode) {
robot.wobbleArmMotor.setMode(runMode);
}
}
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;
}
}

55
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Demo/DemoStateTank.java Normal file
View File

@@ -0,0 +1,55 @@
package org.timecrafters.UltimateGoal.Competition.Demo;
/*
The Player1 state has all the controls for player one. The Player One and Player Two controls are
kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
from running at the same time that shouldn't be.
*/
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.UltimateGoal.Competition.TeleOp.powerShotsControl;
public class DemoStateTank extends CyberarmState {
private Robot robot;
private double drivePower = 1;
private boolean lbPrev;
public DemoStateTank(Robot robot) {
this.robot = robot;
}
@Override
public void exec() {
robot.updateLocation();
Gamepad gamepad = engine.gamepad1;
if (engine.gamepad2.right_bumper) {
gamepad = engine.gamepad2;
}
//toggle for drive speed
boolean lb = engine.gamepad2.left_stick_button;
if (lb && !lbPrev) {
if (drivePower == 1) {
drivePower = 0.5;
} else {
drivePower = 1;
}
}
lbPrev = lb;
double left = -drivePower * gamepad.left_stick_y;
double right = -drivePower * gamepad.right_stick_y;
robot.setDrivePower(left,right,left,right);
}
}

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

@@ -1,6 +1,10 @@
package org.timecrafters.UltimateGoal.Competition;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
/*
The Launch state is used in teleOp and Autonomous. In addition to launching any rings by cycling the
ring belt, this state returns the ring belt to the starting position
*/
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
@@ -45,10 +49,7 @@ public class Launch extends CyberarmState {
@Override
public void exec() {
//detect when limit switch is initially triggered
boolean detectingPass = robot.limitSwitch.isPressed();
int beltPos = robot.ringBeltMotor.getCurrentPosition();
//jam counter measures
if (robot.beltIsStuck() && childrenHaveFinished()) {
long currentTime = System.currentTimeMillis();
if (stuckStartTime == 0) {
@@ -60,6 +61,9 @@ public class Launch extends CyberarmState {
stuckStartTime = 0;
}
//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.

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

@@ -16,7 +16,7 @@ public class Pause extends CyberarmState {
this.actionName = actionName;
}
public Pause(Robot robot, boolean open, long waitTime) {
public Pause(Robot robot, long waitTime) {
this.robot = robot;
this.waitTime = waitTime;
}

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

@@ -1,10 +1,11 @@
package org.timecrafters.UltimateGoal.Competition.PreInit;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
@Disabled
@Autonomous (name = "Load Rings")
public class PreInitEngine extends CyberarmEngine {

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

@@ -1,8 +1,11 @@
package org.timecrafters.UltimateGoal.Competition;
/*
The ProgressRingBelt state is used in teleOp to automatically move the ring belt.
*/
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
public class ProgressRingBelt extends CyberarmState {
@@ -27,13 +30,20 @@ public class ProgressRingBelt extends CyberarmState {
@Override
public void start() {
int currentPos = robot.ringBeltMotor.getCurrentPosition();
//The first two progressions should move to preprare for another ring.
if (robot.ringBeltStage < 2) {
targetPos = currentPos + robot.ringBeltGap;
prep();
//The third progression needs to only move forward enought to block the ring from
//falling out
} else if (robot.ringBeltStage == 2) {
targetPos = currentPos + 150;
prep();
prepLaunch = !robot.initLauncher;
//If the ring belt is already full, It does allow another progression
} else if (robot.ringBeltStage > 2) {
setHasFinished(true);
}
@@ -44,6 +54,7 @@ public class ProgressRingBelt extends CyberarmState {
@Override
public void exec() {
//finished state when the target position is reached
int currentPos = robot.ringBeltMotor.getCurrentPosition();
if (currentPos >= targetPos) {
if(prepLaunch) {
@@ -53,6 +64,7 @@ public class ProgressRingBelt extends CyberarmState {
setHasFinished(true);
}
//belt jam mitigation
if (robot.beltIsStuck() && childrenHaveFinished()) {
long currentTime = System.currentTimeMillis();
if (stuckStartTime == 0) {

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

@@ -1,4 +1,4 @@
package org.timecrafters.UltimateGoal.Competition;
package org.timecrafters.UltimateGoal.Competition;
/*
The robot object contains all the hardware and functions that are used in both teleOp and
@@ -14,6 +14,7 @@ import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.hardware.rev.RevTouchSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@@ -53,7 +54,8 @@ public class Robot {
}
//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.
//variables saved on the phone, without having to re-download the whole program. This is
//especially useful for autonomous route tuning
public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
//We use the IMU to get reliable rotation and angular velocity information. Experimentation has
@@ -74,6 +76,12 @@ public class Robot {
public DcMotor encoderBack;
//Motion Constants
//related graphs
//https://www.desmos.com/calculator/gndnkjndu9
//https://www.desmos.com/calculator/w0rebnftvg
//https://www.desmos.com/calculator/qxa1rq8hrv
static final double CUBIC_CORRECTION = 0.035;
static final double FACE_CUBIC_CORRECTION = 0.025;
static final double LINEAR_CORRECTION = 0.055;
@@ -81,10 +89,12 @@ public class Robot {
static final double FACE_LINEAR_CORRECTION = 0.025;
static final double MOMENTUM_CORRECTION = 1.05;
static final double MOMENTUM_MAX_CORRECTION = 1.4;
static final double MOMENTUM_HORIZONTAL_CORRECTION = -(Math.log10(MOMENTUM_MAX_CORRECTION-1)/Math.log10(MOMENTUM_CORRECTION));
static final double MOMENTUM_HORIZONTAL_CORRECTION =
-(Math.log10(MOMENTUM_MAX_CORRECTION-1) / Math.log10(MOMENTUM_CORRECTION));
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 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
@@ -107,7 +117,7 @@ public class Robot {
private float rotationPrevious = 0;
public float angularVelocity;
//vuforia navigation
//vuforia && tensorFlow Stuff
private WebcamName webcam;
private VuforiaLocalizer vuforia;
@@ -118,8 +128,10 @@ public class Robot {
// 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);
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;
@@ -145,6 +157,7 @@ public class Robot {
public static final double LAUNCH_POWER = 0.715;
private static final long LAUNCH_ACCEL_TIME = 500;
//These variables were originally going to be used in both autonomous and teleOp
public double launchPositionX;
public double launchPositionY;
public float launchRotation;
@@ -182,22 +195,17 @@ public class Robot {
//Debugging
public double totalV;
public double visionX;
public double visionY;
public double visionZ;
public float rawAngle;
private String TestingRecord = "x,y";
private String TestingRecord = "Raw IMU, Delta, Saved";
public double forwardVector;
public double sidewaysVector;
public double traveledForward = 0;
public double traveledRight;
public DcMotorEx motorAmpsTest;
//All our hardware initialization in one place, for everything that is the same in TeleOp and
//Autonomous
public void initHardware() {
limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
//drive motors
driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
@@ -226,9 +234,12 @@ 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();
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");
@@ -236,7 +247,7 @@ public class Robot {
wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
//init ring belt
//init collection motor
collectionMotor = hardwareMap.dcMotor.get("collect");
collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -244,10 +255,17 @@ public class Robot {
ringBeltMotor = hardwareMap.dcMotor.get("belt");
ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
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();
limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
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");
@@ -279,9 +297,12 @@ public class Robot {
webCamServo = hardwareMap.servo.get("look");
webCamServo.setDirection(Servo.Direction.REVERSE );
minCheckVelocity =stateConfiguration.variable("system", "camera", "minCheckV").value();
vuforiaRotationCull = stateConfiguration.variable("system", "camera", "rCull").value();
minCheckDurationMs =stateConfiguration.variable("system", "camera", "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");
@@ -296,8 +317,12 @@ public class Robot {
launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
initLauncher = stateConfiguration.action("system","initLauncher").enabled;
reduceLaunchPos = stateConfiguration.variable("system", "launchPos", "reducePower").value();
//This is a debugging option that automatically turns on the launch wheel during init.
//This can be disabled using a variable in the TimeCraftersConfiguration
initLauncher = stateConfiguration.action(
"system","initLauncher").enabled;
reduceLaunchPos = stateConfiguration.variable(
"system", "launchPos", "reducePower").value();
if (initLauncher) {
double launcherPower = 0;
@@ -307,10 +332,13 @@ public class Robot {
launchMotor.setPower(launcherPower);
}
}
//
launchPositionX = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","x").value());
launchPositionY = inchesToTicks((double) stateConfiguration.variable("system", "launchPos","y").value());
launchRotation = stateConfiguration.variable("system", "launchPos","rot").value();
launchPositionX = inchesToTicks((double) stateConfiguration.variable(
"system", "launchPos","x").value());
launchPositionY = inchesToTicks((double) stateConfiguration.variable(
"system", "launchPos","y").value());
launchRotation = stateConfiguration.variable(
"system", "launchPos","rot").value();
initTensorFlow();
@@ -319,8 +347,10 @@ public class Robot {
private void initVuforia() {
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
"cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
VuforiaLocalizer.Parameters parameters =
new VuforiaLocalizer.Parameters(cameraMonitorViewId);
parameters.vuforiaLicenseKey = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
parameters.cameraName = webcam;
@@ -346,20 +376,27 @@ public class Robot {
redAllianceTarget.setLocation(OpenGLMatrix
.translation(0, -halfField, mmTargetHeight)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, 180)));
.multiplied(Orientation.getRotationMatrix(
EXTRINSIC, XYZ, DEGREES, 90, 0, 180)));
frontWallTarget.setLocation(OpenGLMatrix
.translation(-halfField, 0, mmTargetHeight)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)));
.multiplied(Orientation.getRotationMatrix(
EXTRINSIC, XYZ, DEGREES, 90, 0 , 90)));
redTowerGoalTarget.setLocation(OpenGLMatrix
.translation(halfField, -quadField, mmTargetHeight)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));
.multiplied(Orientation.getRotationMatrix(
EXTRINSIC, XYZ, DEGREES, 90, 0, -90)));
OpenGLMatrix robotFromCamera = OpenGLMatrix
.translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch, CAMERA_LEFT_DISPLACEMENT * mmPerInch, CAMERA_VERTICAL_DISPLACEMENT * mmPerInch)
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, -90, 0, 0));
.translation(CAMERA_FORWARD_DISPLACEMENT * mmPerInch,
CAMERA_LEFT_DISPLACEMENT * mmPerInch,
CAMERA_VERTICAL_DISPLACEMENT * mmPerInch)
.multiplied(Orientation.getRotationMatrix(
EXTRINSIC, YZX, DEGREES, -90, 0, 0));
for (VuforiaTrackable trackable : trackables) {
((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection);
((VuforiaTrackableDefaultListener) trackable.getListener())
.setPhoneInformation(robotFromCamera, parameters.cameraDirection);
}
targetsUltimateGoal.activate();
@@ -373,11 +410,15 @@ 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", "camera", "minConfidence").value();
parameters.minResultConfidence = stateConfiguration.variable(
"system", "camera", "minConfidence").value();
tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
tfObjectDetector.loadModelFromAsset("UltimateGoal.tflite", "Quad", "Single");
tfObjectDetector.loadModelFromAsset(
"UltimateGoal.tflite", "Quad", "Single");
}
//Localization Function! This function is represented in a flow diagram, earlier in the
//software section
//run this in every exec to track the robot's location.
public void updateLocation(){
@@ -417,13 +458,15 @@ public class Robot {
ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD;
}
forwardVector = ((encoderLeftChange+encoderRightChange)/2) - (rotationChange* ticksPerDegreeForward);
forwardVector = ((encoderLeftChange+encoderRightChange)/2) -
(rotationChange* ticksPerDegreeForward);
traveledForward += forwardVector;
sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) + Math.atan2(sidewaysVector,forwardVector);
double direction = Math.toRadians(Robot.rotation + (rotationChange/2)) +
Math.atan2(sidewaysVector,forwardVector);
double xChange = magnitude * (Math.sin(direction));
double yChange = magnitude * (Math.cos(direction));
@@ -434,11 +477,15 @@ public class Robot {
Robot.rotation += rotationChange;
Robot.rotation = scaleAngleRange(Robot.rotation);
totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) + Math.abs(encoderBackChange);
totalV = Math.abs(encoderLeftChange) + Math.abs(encoderRightChange) +
Math.abs(encoderBackChange);
// record(""+System.currentTimeMillis()+", "+imuAngle);
}
//Experimentation has demonstrated that Vuforia dosen't provide good data while the camera
//is moving or rotating. This function detects if conditions are appropriate to run vuforia and
//get more accurate results
public void syncIfStationary() {
if (totalV < minCheckVelocity) {
long timeCurrent = System.currentTimeMillis();
@@ -459,7 +506,9 @@ public class Robot {
trackableVisible = false;
for (VuforiaTrackable trackable : trackables) {
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
OpenGLMatrix robotLocation =
((VuforiaTrackableDefaultListener)trackable.getListener())
.getUpdatedRobotLocation();
//this is used for debugging purposes.
trackableVisible = true;
@@ -470,7 +519,8 @@ 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 orientation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
Orientation orientation =
Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
float vuforiaRotation = 90-orientation.thirdAngle;
if (vuforiaRotation > 180) {
@@ -484,8 +534,10 @@ public class Robot {
double camX = -translation.get(1) / mmPerInch;
double camY = translation.get(0) / mmPerInch;
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);
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);
locationX = inchesToTicks(camX - displaceX);
locationY = inchesToTicks(camY - displaceY);
@@ -508,18 +560,13 @@ public class Robot {
return Robot.locationY;
}
//This is meant to only be used to indicate starting positions and to reorient the robot.
public void setLocalization(float rotation, double x, double y) {
Robot.rotation = rotation;
Robot.locationX = x;
Robot.locationY = y;
}
//Manually set the position of the robot on the field.
public void setCurrentPosition(float rotation, double x, double y) {
Robot.rotation = rotation;
Robot.locationX = x;
Robot.locationY = y;
}
//returns the angle from the robot's current position to the given target position.
public float getAngleToPosition (double x, double y) {
@@ -531,7 +578,7 @@ public class Robot {
}
//Unit conversion
//Unit conversions
public double ticksToInches(double ticks) {
return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
}
@@ -541,26 +588,28 @@ public class Robot {
}
//Returns the angle between two angles, with positive angles indicating that the reference is
//to the right (clockwise) of the current. Negative angles indicate that the reference is to the
//left.
//Returns the shortest angle between two directions, with positive angles indicating that the
// reference is to the right (clockwise) of the current. Negative angles indicate that the
// reference is to the left.
public float getRelativeAngle(float reference, float current) {
return scaleAngleRange(current - reference);
}
//Drive Functions
public void setDrivePower(double powerFrontLeft, double powerFrontRight, double powerBackLeft, double powerBackRight){
public void setDrivePower(double powerFrontLeft, double powerFrontRight,
double powerBackLeft, double powerBackRight){
driveFrontLeft.setPower(powerFrontLeft);
driveFrontRight.setPower(powerFrontRight);
driveBackLeft.setPower(powerBackLeft);
driveBackRight.setPower(powerBackRight);
}
//returns an array of the powers necessary to execute the provided motion. "degreesDirectionMotion"
//is the angle relative to the field that the robot should drive at. "degreesDirectionFace" is
//the angle the robot should face relative to the field. The order of the output powers is
//is ForwardLeft, ForwardRight, BackLeft, BackRight
public double[] getMecanumPowers(float degreesDirectionMotion, double scalar, float degreesDirectionFace) {
//returns an array of the powers necessary to execute the provided motion.
//"degreesDirectionMotion" is the angle relative to the field that the robot should drive at.
//"degreesDirectionFace" is the angle the robot should face relative to the field. The order of
//the output powers is ForwardLeft, ForwardRight, BackLeft, BackRight
public double[] getMecanumPowers(float degreesDirectionMotion, double scalar,
float degreesDirectionFace) {
angularVelocity = imu.getAngularVelocity().xRotationRate;
//calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion
@@ -582,12 +631,14 @@ public class Robot {
LINEAR_CORRECTION * relativeRotation;
if (relativeRotation != 0) {
double momentumRelative = angularVelocity * (relativeRotation / Math.abs(relativeRotation));
double exponential = Math.pow(MOMENTUM_CORRECTION, MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
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;
turnCorrection *= momentumCorrection + ((Math.abs(relativeRotation) *
(1 - momentumCorrection)) / 180 );
}
double powerForwardRight = scalar * (q + turnCorrection);
@@ -660,6 +711,7 @@ public class Robot {
return powers;
}
//Used to for automated jam countermeasures
public boolean beltIsStuck() {
int ringBeltPos = ringBeltMotor.getCurrentPosition();
boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks);
@@ -677,6 +729,7 @@ public class Robot {
return angle;
}
//Debugging tools
public void record(String record) {
TestingRecord+="\n"+record;
}

1
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/LaunchControl.java
View File

@@ -17,7 +17,6 @@ public class LaunchControl extends CyberarmState {
@Override
public void start() {
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
if (robot.ringBeltStage > 2) {
if (robot.ringBeltStage > 4) {

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

@@ -1,5 +1,11 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
/*
The Player1 state has all the controls for player one. The Player One and Player Two controls are
kept in separate states so that the childrenHaveFinished() method can be used to easily stop things
from running at the same time that shouldn't be.
*/
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import org.cyberarm.engine.V2.CyberarmState;
@@ -40,6 +46,7 @@ public class Player1 extends CyberarmState {
private double endGameX;
private double endGameY;
private float endGameRot;
private int loopCount;
private double refinePower;
@@ -49,14 +56,21 @@ public class Player1 extends CyberarmState {
@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();
refinePower = robot.stateConfiguration.variable("tele","control", "refPower").value();
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();
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.stateConfiguration.variable(
"tele","_endGameStart","x").value();
endGameY = robot.stateConfiguration.variable(
"tele","_endGameStart","y").value();
endGameRot = robot.stateConfiguration.variable(
"tele","_endGameStart", "r").value();
}
@Override
@@ -66,8 +80,11 @@ public class Player1 extends CyberarmState {
@Override
public void exec() {
loopCount+=1;
robot.record(""+runTime()+", "+loopCount+", "+robot.getRotation());
robot.updateLocation();
//toggle for drive speed
boolean lb = engine.gamepad1.left_stick_button;
if (lb && !lbPrev) {
if (drivePower == 1) {
@@ -78,7 +95,9 @@ public class Player1 extends CyberarmState {
}
lbPrev = lb;
//This Runs the FindWobbleGoal state as long as the button is pressed. When it's released,
//the state is interrupted. If the state finishes while the button is still pressed,
//it waits until the button is released before running the state again
runNextFindWobble = (findWobbleGoal == null || findWobbleGoal.getHasFinished());
boolean findWobbleInput = engine.gamepad1.x;
@@ -89,7 +108,8 @@ public class Player1 extends CyberarmState {
faceDirection = robot.getRotation();
if (runNextFindWobble && !findWobbleInputPrev) {
findWobbleGoal = new FindWobbleGoal(robot, "auto", "08_0");
findWobbleGoal =
new FindWobbleGoal(robot, "auto", "08_0");
addParallelState(findWobbleGoal);
}
//if the claw is closed, open the claw.
@@ -113,6 +133,8 @@ public class Player1 extends CyberarmState {
}
aPrev = a;
//This logic works the same as the stuff above, accept instead of autonomous wobble grab,
//it
runNextDriveToLaunch = (powerShots == null || powerShots.getHasFinished());
boolean driveToLaunchInput = engine.gamepad1.y && !findWobbleInput;
@@ -127,29 +149,27 @@ public class Player1 extends CyberarmState {
}
driveToLaunchInputPrev = driveToLaunchInput;
// if (engine.gamepad1.y) {
// robot.setLocalization(endGameRot,endGameX,endGameY);
// setHasFinished(true);
// }
//Normal Driver Controls
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)));
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)));
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 that the controller and robot can be re-synced in the event of a bad initial
//position.
//rotation.
if (engine.gamepad1.back) {
float newRotation = 0;
@@ -161,14 +181,19 @@ public class Player1 extends CyberarmState {
faceDirection = newRotation;
}
//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 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
//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;
//The D-pad is used if the drivers need to get a more precise angle than they can get
//using the face joystick
if (engine.gamepad1.dpad_right) {
powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
faceDirection = robot.getRotation();
@@ -182,7 +207,9 @@ public class Player1 extends CyberarmState {
//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);
powers = robot.getMecanumPowers(
snapToCardinal(leftJoystickDegrees, pairSnapping, faceDirection),
drivePower, faceDirection);
}
robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
@@ -190,7 +217,8 @@ public class Player1 extends CyberarmState {
//LED feedback control
double ringBeltPower = robot.ringBeltMotor.getPower();
if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() - robot.ringBeltMotor.getCurrentPosition()) > 10) {
if (ringBeltPower > 0 && Math.abs(robot.ringBeltMotor.getTargetPosition() -
robot.ringBeltMotor.getCurrentPosition()) > 10) {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_BLUE );
} else if (ringBeltPower < 0) {
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);

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

@@ -1,9 +1,15 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
/*
The Player2 state has all the controls for player two. This includes a lot of automation and
emergency features, for if the driver wants to take control unexpectedly
*/
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import org.timecrafters.UltimateGoal.Competition.Launch;
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
import org.timecrafters.UltimateGoal.Competition.Robot;
@@ -22,6 +28,7 @@ public class Player2 extends CyberarmState {
private DcMotor.RunMode runModePrev;
private boolean lbPrev;
private boolean manualArmHold;
private int loops = 0;
@@ -45,9 +52,10 @@ public class Player2 extends CyberarmState {
@Override
public void exec() {
// loops+=1;
// robot.record(""+runTime()+", "+loops+", "+robot.getRotation());
//Collector control
double rt = engine.gamepad2.right_trigger;
double lt = engine.gamepad2.left_trigger;
if (rt < lt) {
@@ -58,6 +66,8 @@ public class Player2 extends CyberarmState {
robot.collectionMotor.setPower(0);
}
//The childrenHaveFinished() method tracks if there are parallel states that are still
//running.
if (childrenHaveFinished()) {
//belt progression control
boolean rb = engine.gamepad2.right_bumper;
@@ -140,33 +150,40 @@ public class Player2 extends CyberarmState {
}
lbPrev = lb;
// if (engine.gamepad1.y) {
// setHasFinished(true);
// }
if (engine.gamepad2.back) {
for (CyberarmState state : children) {
engine.stopState(state);
}
}
}
//This just checks if the wobble arm runmode is already the desired mode before setting it.
//I don't know if this is actually helpful
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.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());
// }
// }
engine.telemetry.addData("Player 2 children", childrenHaveFinished());
for (CyberarmState state : children) {
if (!state.getHasFinished()) {
engine.telemetry.addLine("" + state.getClass());
}
}
}
}

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

@@ -1,12 +1,13 @@
package org.timecrafters.UltimateGoal.Competition.TeleOp;
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.Autonomous.DriveToCoordinates;
import org.timecrafters.UltimateGoal.Competition.Autonomous.Face;
import org.timecrafters.UltimateGoal.Competition.Robot;
@Disabled
@TeleOp (name = "TeleOp",group = "comp")
public class TeleOpEngine extends CyberarmEngine {

7
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/powerShotsControl.java
View File

@@ -3,6 +3,7 @@ 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.Pause;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.util.ArrayList;
@@ -15,6 +16,8 @@ public class powerShotsControl extends CyberarmState {
private double endGameY;
private float endGameRot;
private double endGamePower;
private int nextState = 0;
private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
@@ -28,11 +31,13 @@ public class powerShotsControl extends CyberarmState {
endGameX = robot.stateConfiguration.variable("tele","_endGameStart","x").value();
endGameY = robot.stateConfiguration.variable("tele","_endGameStart","y").value();
endGameRot = robot.stateConfiguration.variable("tele","_endGameStart", "r").value();
endGamePower = robot.stateConfiguration.variable("tele","_endGameStart", "power").value();
endGameX = robot.inchesToTicks(endGameX);
endGameY = robot.inchesToTicks(endGameY);
endGameRot = (float) robot.inchesToTicks(endGameRot);
states.add(new Pause(robot, "tele","_endGameStart"));
states.add(new DriveToCoordinates(robot, "tele", "_pow1"));
states.add(new Face(robot,"tele","_faceZero"));
states.add(new LaunchControl(robot));
@@ -47,7 +52,7 @@ public class powerShotsControl extends CyberarmState {
@Override
public void start() {
robot.setLocalization(endGameRot,endGameX,endGameY);
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
robot.launchMotor.setPower(endGamePower);
}
@Override

22
TeamCode/src/main/java/org/timecrafters/javaClass/aubrey/AubreyFirstState.java Normal file
View File

@@ -0,0 +1,22 @@
package org.timecrafters.javaClass.aubrey;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.javaClass.samples.SampleRobot;
public class AubreyFirstState extends CyberarmState {
//here, you'll find some of your variables. you can add more as you need them.
private SampleRobot robot;
//This is the constructor. It lets other code bits run use the code you put here
public AubreyFirstState(SampleRobot robot) {
this.robot = robot;
}
//This is a method. methods are bits of code that can be run elsewhere.
//This one is set up to repeat every few milliseconds
@Override
public void exec() {
}
}

26
TeamCode/src/main/java/org/timecrafters/javaClass/samples/AubreyFirstEngine.java Normal file
View File

@@ -0,0 +1,26 @@
package org.timecrafters.javaClass.samples;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.javaClass.aubrey.AubreyFirstState;
@Autonomous(name = "Aubrey: First Program", group = "aubrey")
public class AubreyFirstEngine extends CyberarmEngine {
SampleRobot robot;
@Override
public void init() {
robot = new SampleRobot(hardwareMap);
robot.initHardware();
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
super.init();
}
@Override
public void setup() {
addState(new AubreyFirstState(robot));
}
}

191
TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobot.java Normal file
View File

@@ -0,0 +1,191 @@
package org.timecrafters.javaClass.samples;
/*
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;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.hardware.rev.RevTouchSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
import java.io.File;
import java.io.FileWriter;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX;
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
public class SampleRobot {
private HardwareMap hardwareMap;
public SampleRobot(HardwareMap hardwareMap) {
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. This is
//especially useful for autonomous route tuning
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 and dead-wheal hardware
public DcMotor driveFrontLeft;
public DcMotor driveBackLeft;
public DcMotor driveFrontRight;
public DcMotor driveBackRight;
public DcMotor encoderLeft;
public DcMotor encoderRight;
public DcMotor encoderBack;
//Unit Conversion Constants
static final double ENCODER_CIRCUMFERENCE = Math.PI * 2.3622;
static final int COUNTS_PER_REVOLUTION = 8192;
//Launcher
public DcMotor launchMotor;
public static final double LAUNCH_POWER = 0.715;
//Ring Intake
public DcMotor collectionMotor;
//Ring Belt
public DcMotor ringBeltMotor;
public RevTouchSensor limitSwitch;
//Wobble Goal Arm & Grabber
public DcMotor wobbleArmMotor;
public Servo wobbleGrabServo;
public RevColorSensorV3 wobbleColorSensor;
public RevTouchSensor wobbleTouchSensor;
//All our hardware initialization in one place, for everything that is the same in TeleOp and
//Autonomous
public void initHardware() {
//drive motors
driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
//Localization encoders
encoderRight = hardwareMap.dcMotor.get("driveFrontRight");
encoderBack = hardwareMap.dcMotor.get("driveFrontLeft");
encoderLeft = hardwareMap.dcMotor.get("collect");
encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
//init wobble arm
wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm");
wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
wobbleArmMotor.setTargetPosition(0);
wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
//init collection motor
collectionMotor = hardwareMap.dcMotor.get("collect");
collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
//init ring belt
ringBeltMotor = hardwareMap.dcMotor.get("belt");
ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
//init IMU
imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
imu.initialize(parameters);
//Init Launch Motor
DcMotor launcher = hardwareMap.dcMotor.get("launcher");
MotorConfigurationType launchMotorType = launcher.getMotorType();
launchMotorType.setGearing(3);
launchMotorType.setTicksPerRev(84);
launchMotorType.setMaxRPM(2400);
launchMotor = launcher;
launchMotor.setMotorType(launchMotorType);
launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds");
}
public float getIMURotation() {
return imu.getAngularOrientation().firstAngle;
}
//Unit conversions
public double ticksToInches(double ticks) {
return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
}
public double inchesToTicks(double inches) {
return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE);
}
}

515
TeamCode/src/main/java/org/timecrafters/javaClass/samples/SampleRobotEx.java Normal file
View File

@@ -0,0 +1,515 @@
package org.timecrafters.javaClass.samples;
/*
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 com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.hardware.rev.RevTouchSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.timecrafters.TimeCraftersConfigurationTool.TimeCraftersConfiguration;
import org.timecrafters.UltimateGoal.Competition.Robot;
import java.util.ArrayList;
import java.util.List;
public class SampleRobotEx {
private HardwareMap hardwareMap;
public SampleRobotEx(HardwareMap hardwareMap) {
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. This is
//especially useful for autonomous route tuning
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 and dead-wheal hardware
public DcMotor driveFrontLeft;
public DcMotor driveBackLeft;
public DcMotor driveFrontRight;
public DcMotor driveBackRight;
public DcMotor encoderLeft;
public DcMotor encoderRight;
public DcMotor encoderBack;
//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;
static final double MOMENTUM_CORRECTION = 1.05;
static final double MOMENTUM_MAX_CORRECTION = 1.4;
static final double MOMENTUM_HORIZONTAL_CORRECTION =
-(Math.log10(MOMENTUM_MAX_CORRECTION-1) / Math.log10(MOMENTUM_CORRECTION));
static final double 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;
static final int COUNTS_PER_REVOLUTION = 8192;
static final float mmPerInch = 25.4f;
static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD = 12.3;
static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD = 18.8;
static final double TICKS_PER_ROBOT_DEGREE_CLOCKWISE = 8.4;
static final double TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE = 8.6;
//Robot Localization
private static double locationX;
private static double locationY;
private static float rotation;
private int encoderLeftPrevious = 0;
private int encoderBackPrevious = 0;
private int encoderRightPrevious = 0;
private float rotationPrevious = 0;
public float angularVelocity;
public double forwardVector;
public double sidewaysVector;
//vuforia && tensorFlow Stuff
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;
public static final double LAUNCH_POWER = 0.715;
private static final long LAUNCH_ACCEL_TIME = 500;
//These variables were originally going to be used in both autonomous and teleOp
public double launchPositionX;
public double launchPositionY;
public float launchRotation;
public int reduceLaunchPos;
public boolean initLauncher;
//Ring Intake
public DcMotor collectionMotor;
//Ring Belt
public DcMotor ringBeltMotor;
public RevTouchSensor limitSwitch;
public int ringBeltStage;
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 & Grabber
public DcMotor wobbleArmMotor;
public Servo wobbleGrabServo;
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;
public RevTouchSensor wobbleTouchSensor;
//Debugging
public double totalV;
private String TestingRecord = "Raw IMU, Delta, Saved";
public double traveledForward = 0;
public DcMotorEx motorAmpsTest;
//All our hardware initialization in one place, for everything that is the same in TeleOp and
//Autonomous
public void initHardware() {
//drive motors
driveFrontLeft = hardwareMap.dcMotor.get("driveFrontLeft");
driveFrontRight = hardwareMap.dcMotor.get("driveFrontRight");
driveBackLeft = hardwareMap.dcMotor.get("driveBackLeft");
driveBackRight = hardwareMap.dcMotor.get("driveBackRight");
driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
//Localization encoders
encoderRight = hardwareMap.dcMotor.get("driveFrontRight");
encoderBack = hardwareMap.dcMotor.get("driveFrontLeft");
encoderLeft = hardwareMap.dcMotor.get("collect");
encoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderBack.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
encoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
//init wobble arm
wobbleArmMotor = hardwareMap.dcMotor.get("wobbleArm");
wobbleArmMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
wobbleArmMotor.setTargetPosition(0);
wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
//init collection motor
collectionMotor = hardwareMap.dcMotor.get("collect");
collectionMotor.setDirection(DcMotorSimple.Direction.REVERSE);
//init ring belt
ringBeltMotor = hardwareMap.dcMotor.get("belt");
ringBeltMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
ringBeltMotor .setMode(DcMotor.RunMode.RUN_USING_ENCODER);
limitSwitch = hardwareMap.get(RevTouchSensor.class, "magLim");
//init IMU
imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.mode = BNO055IMU.SensorMode.IMU;
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
parameters.loggingEnabled = false;
imu.initialize(parameters);
Velocity startVelocity = new Velocity();
startVelocity.xVeloc = 0;
startVelocity.yVeloc = 0;
startVelocity.zVeloc = 0;
Position startPosition = new Position();
startPosition.x = 0;
startPosition.y = 0;
startPosition.z = 0;
imu.startAccelerationIntegration(startPosition,startVelocity, 10);
//Init Launch Motor
DcMotor launcher = hardwareMap.dcMotor.get("launcher");
MotorConfigurationType launchMotorType = launcher.getMotorType();
launchMotorType.setGearing(3);
launchMotorType.setTicksPerRev(84);
launchMotorType.setMaxRPM(2400);
launchMotor = launcher;
launchMotor.setMotorType(launchMotorType);
launchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
launchMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds");
}
//Localization Function! This function is represented in a flow diagram, earlier in the
//software section
//run this in every exec to track the robot's location.
public void updateLocation(){
// orientation is inverted to have clockwise be positive.
float imuAngle = -imu.getAngularOrientation().firstAngle;
double rotationChange = getRelativeAngle(rotationPrevious, imuAngle);
int encoderLeftCurrent = -encoderLeft.getCurrentPosition();
int encoderRightCurrent = encoderRight.getCurrentPosition();
int encoderBackCurrent = encoderBack.getCurrentPosition();
double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
double encoderRightChange = encoderRightCurrent - encoderRightPrevious;
double encoderBackChange = encoderBackCurrent - encoderBackPrevious;
encoderLeftPrevious = encoderLeftCurrent;
encoderRightPrevious = encoderRightCurrent;
encoderBackPrevious = encoderBackCurrent;
rotationPrevious = imuAngle;
//The forward Vector has the luxury of having an odometer on both sides of the robot.
//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
//based on a separate calibration program.
double ticksPerDegreeForward;
double ticksPerDegreeSideways;
if (rotationChange < 0) {
ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE;
ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_COUNTERCLOCKWISE_FORWARD;
} else {
ticksPerDegreeSideways = TICKS_PER_ROBOT_DEGREE_CLOCKWISE;
ticksPerDegreeForward = TICKS_PER_ROBOT_DEGREE_CLOCKWISE_FORWARD;
}
forwardVector = ((encoderLeftChange+encoderRightChange)/2) -
(rotationChange* ticksPerDegreeForward);
sidewaysVector = encoderBackChange + (rotationChange * ticksPerDegreeSideways);
double magnitude = Math.sqrt((forwardVector*forwardVector) + (sidewaysVector*sidewaysVector));
double direction = Math.toRadians(SampleRobotEx.rotation + (rotationChange/2)) +
Math.atan2(sidewaysVector,forwardVector);
double xChange = magnitude * (Math.sin(direction));
double yChange = magnitude * (Math.cos(direction));
locationX += xChange;
locationY += yChange;
SampleRobotEx.rotation += rotationChange;
SampleRobotEx.rotation = scaleAngleRange(SampleRobotEx.rotation);
}
public float getRotation() {
return SampleRobotEx.rotation;
}
public double getLocationX() {
return SampleRobotEx.locationX;
}
public double getLocationY() {
return SampleRobotEx.locationY;
}
//This is meant to only be used to indicate starting positions and to reorient the robot.
public void setLocalization(float rotation, double x, double y) {
SampleRobotEx.rotation = rotation;
SampleRobotEx.locationX = x;
SampleRobotEx.locationY = y;
}
//returns the angle from the robot's current position to the given target position.
public float getAngleToPosition (double x, double y) {
double differenceX = x- getLocationX();
double differenceY = y- getLocationY();
double angle = Math.toDegrees(Math.atan2(differenceX, differenceY ));
return (float) angle;
}
//Unit conversions
public double ticksToInches(double ticks) {
return ticks * (ENCODER_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
}
public double inchesToTicks(double inches) {
return inches * (COUNTS_PER_REVOLUTION / ENCODER_CIRCUMFERENCE);
}
//Returns the shortest angle between two directions, with positive angles indicating that the
// reference is to the right (clockwise) of the current. Negative angles indicate that the
// reference is to the left.
public float getRelativeAngle(float reference, float current) {
return scaleAngleRange(current - reference);
}
//Drive Functions
public void setDrivePower(double powerFrontLeft, double powerFrontRight,
double powerBackLeft, double powerBackRight){
driveFrontLeft.setPower(powerFrontLeft);
driveFrontRight.setPower(powerFrontRight);
driveBackLeft.setPower(powerBackLeft);
driveBackRight.setPower(powerBackRight);
}
public void setDrivePower(double[] powers){
driveFrontLeft.setPower(powers[0]);
driveFrontRight.setPower(powers[1]);
driveBackLeft.setPower(powers[2]);
driveBackRight.setPower(powers[3]);
}
//returns an array of the powers necessary to execute the provided motion.
//"degreesDirectionMotion" is the angle relative to the field that the robot should drive at.
//"degreesDirectionFace" is the angle the robot should face relative to the field. The order of
//the output powers is ForwardLeft, ForwardRight, BackLeft, BackRight
public double[] getMecanumPowers(float degreesDirectionMotion, double scalar,
float degreesDirectionFace) {
angularVelocity = imu.getAngularVelocity().xRotationRate;
//calculating the base mecanum powers so that the robot drives along the degreesDirectionMotion
//once it is pointed towards the degreesDirectionFace
double rad = Math.toRadians(getRelativeAngle(degreesDirectionFace,degreesDirectionMotion));
double y = Math.cos(rad);
double x = Math.sin(rad);
double p = y + x;
double q = y - x;
//calculating correction needed to steer the robot towards the degreesDirectionFace
float relativeRotation =
getRelativeAngle(degreesDirectionFace, SampleRobotEx.rotation);
double turnCorrection =
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 );
}
double powerForwardRight = scalar * (q + turnCorrection);
double powerForwardLeft = scalar * (p - turnCorrection);
double powerBackRight = scalar * (p + turnCorrection);
double powerBackLeft = scalar * (q - turnCorrection);
// 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.
double extreme = Math.max(
Math.max(Math.abs(powerForwardRight),Math.abs(powerForwardLeft)),
Math.max(Math.abs(powerBackRight),Math.abs(powerBackLeft)));
if (extreme > 1) {
powerForwardRight = powerForwardRight/extreme;
powerForwardLeft = powerForwardLeft/extreme;
powerBackRight = powerBackRight/extreme;
powerBackLeft = powerBackLeft/extreme;
}
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;
}
//Outputs the power necessary to turn and face a provided direction
public double[] getFacePowers(float direction, double power) {
angularVelocity = imu.getAngularVelocity().xRotationRate;
double relativeAngle = getRelativeAngle(direction, SampleRobotEx.rotation);
double scaler = Math.pow(FACE_CUBIC_CORRECTION * relativeAngle, 3) +
FACE_LINEAR_CORRECTION * relativeAngle;
if (relativeAngle > 0.5) {
scaler += FACE_MIN_CORRECTION;
} else if (relativeAngle < -0.5) {
scaler -= FACE_MIN_CORRECTION;
}
if (relativeAngle != 0) {
double momentumRelative = angularVelocity * (relativeAngle / Math.abs(relativeAngle));
double exponential = Math.pow(FACE_MOMENTUM_CORRECTION,
FACE_MOMENTUM_HORIZONTAL_CORRECTION-momentumRelative);
double momentumCorrection = (MOMENTUM_MAX_CORRECTION*exponential)/(1+exponential);
scaler *= momentumCorrection;
}
double left = -power * scaler;
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;
}
public float scaleAngleRange(float angle) {
if (angle < -180) {
angle += 360;
}
if (angle > 180) {
angle -= 360;
}
return angle;
}
}

27
TeamCode/src/main/java/org/timecrafters/javaClass/samples/SpencerFirstEngine.java Normal file
View File

@@ -0,0 +1,27 @@
package org.timecrafters.javaClass.samples;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Competition.Robot;
import org.timecrafters.javaClass.aubrey.AubreyFirstState;
import org.timecrafters.javaClass.spencer.SpencerFirstState;
@Autonomous (name = "Spencer: First Program", group = "spencer")
public class SpencerFirstEngine extends CyberarmEngine {
SampleRobot robot;
@Override
public void init() {
robot = new SampleRobot(hardwareMap);
robot.initHardware();
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
super.init();
}
@Override
public void setup() {
addState(new SpencerFirstState(robot));
}
}

22
TeamCode/src/main/java/org/timecrafters/javaClass/spencer/SpencerFirstState.java Normal file
View File

@@ -0,0 +1,22 @@
package org.timecrafters.javaClass.spencer;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.javaClass.samples.SampleRobot;
public class SpencerFirstState extends CyberarmState {
//here, you'll find some of your variables. you can add more as you need them.
private SampleRobot robot;
//This is the constructor. It lets other code bits run use the code you put here
public SpencerFirstState(SampleRobot robot) {
this.robot = robot;
}
//This is a method. methods are bits of code that can be run elsewhere.
//This one is set up to repeat every few milliseconds
@Override
public void exec() {
}
}

2
build.gradle
View File

@@ -15,7 +15,7 @@ buildscript {
jcenter()
}
dependencies {
classpath 'com.android.tools.build:gradle:4.0.1'
classpath 'com.android.tools.build:gradle:4.1.2'
}
}

4
gradle/wrapper/gradle-wrapper.properties vendored
View File

@@ -1,6 +1,6 @@
#Fri Jul 24 14:30:03 PDT 2020
#Sat Jul 24 09:32:10 CDT 2021
distributionBase=GRADLE_USER_HOME
distributionPath=wrapper/dists
zipStoreBase=GRADLE_USER_HOME
zipStorePath=wrapper/dists
distributionUrl=https\://services.gradle.org/distributions/gradle-6.1.1-all.zip
distributionUrl=https\://services.gradle.org/distributions/gradle-6.5-bin.zip