mirror of
https://github.com/TimeCrafters/FreightFrenzy.git
synced 2025-12-13 05:02:34 +00:00
Initial Commit; Stubbed directories for FreightFrenzy, included source of UltimateGoal for quick reference, including TimeCraftersConfigurationTool library until I get it published to maven, disabled UltimateGoal engines.
This commit is contained in:
@@ -0,0 +1,58 @@
|
||||
package org.cyberarm.container;
|
||||
|
||||
import android.util.Log;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import java.lang.reflect.Field;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
|
||||
public class InputChecker {
|
||||
private Gamepad gamepad;
|
||||
private HashMap<String, Byte> buttons;
|
||||
private ArrayList<String> buttonList;
|
||||
private byte NULL = 0,
|
||||
PRESSED = 1,
|
||||
RELEASED= 2;
|
||||
public InputChecker(Gamepad gamepad) {
|
||||
this.gamepad = gamepad;
|
||||
buttons = new HashMap<>();
|
||||
buttonList = new ArrayList<>();
|
||||
|
||||
buttonList.add("a"); buttonList.add("b"); buttonList.add("x"); buttonList.add("y");
|
||||
buttonList.add("start"); buttonList.add("guide"); buttonList.add("back");
|
||||
buttonList.add("left_bumper"); buttonList.add("right_bumper");
|
||||
buttonList.add("left_stick_button"); buttonList.add("right_stick_button");
|
||||
buttonList.add("dpad_left"); buttonList.add("dpad_right");
|
||||
buttonList.add("dpad_up"); buttonList.add("dpad_down");
|
||||
}
|
||||
|
||||
public void update() {
|
||||
for (int i = 0; i < buttonList.size(); i++) {
|
||||
try {
|
||||
Field field = gamepad.getClass().getDeclaredField(buttonList.get(i));
|
||||
|
||||
if (field.getBoolean(gamepad)) {
|
||||
buttons.put(buttonList.get(i), PRESSED);
|
||||
} else {
|
||||
if (buttons.get(buttonList.get(i)) != null && buttons.get(buttonList.get(i)) == PRESSED) {
|
||||
buttons.put(buttonList.get(i), RELEASED);
|
||||
}
|
||||
}
|
||||
} catch (NoSuchFieldException|IllegalAccessException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public boolean check(String button) {
|
||||
boolean state = false;
|
||||
if (buttons.containsKey(button) && buttons.get(button) == RELEASED) {
|
||||
Log.d("InputChecker","button \""+button+"\" has been released on \"gamepad"+gamepad.getGamepadId()+"\"");
|
||||
state = true;
|
||||
buttons.put(button, NULL);
|
||||
}
|
||||
return state;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,231 @@
|
||||
package org.cyberarm.engine.V2;
|
||||
|
||||
import android.util.Log;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
|
||||
import java.util.concurrent.CopyOnWriteArrayList;
|
||||
|
||||
/**
|
||||
* CyberarmEngine Version 2.0 | October 26th 2018
|
||||
* AN Experimental reimplementation of GoldfishPi's original Engine system.
|
||||
* Designed to be easily maintainable, extensible, and easy to understand.
|
||||
*/
|
||||
public abstract class CyberarmEngine extends OpMode {
|
||||
|
||||
public static CyberarmEngine instance;
|
||||
//Array To Hold States
|
||||
private CopyOnWriteArrayList<CyberarmState> cyberarmStates = new CopyOnWriteArrayList<>();
|
||||
private int activeStateIndex = 0;
|
||||
private boolean isRunning;
|
||||
|
||||
private static String TAG = "PROGRAM.ENGINE";
|
||||
public boolean showStateChildrenListInTelemetry = false;
|
||||
|
||||
/**
|
||||
* Called when INIT button on Driver Station is pushed
|
||||
* ENSURE to call super.init() if you override this method
|
||||
*/
|
||||
public void init() {
|
||||
CyberarmEngine.instance = this;
|
||||
isRunning = false;
|
||||
|
||||
setup();
|
||||
|
||||
isRunning = true;
|
||||
|
||||
for (CyberarmState state: cyberarmStates) {
|
||||
initState(state);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Setup states for engine to use
|
||||
* For example:
|
||||
* <pre>
|
||||
* @<code>
|
||||
* public void setup() {
|
||||
* addState(new TestState());
|
||||
* addState(new AnotherState(100, 500));
|
||||
* }
|
||||
* </code>
|
||||
* </pre>
|
||||
*/
|
||||
public abstract void setup();
|
||||
|
||||
/**
|
||||
* Called when START button on Driver Station is pushed
|
||||
* ENSURE to call super.start() if you override this method
|
||||
*/
|
||||
public void start() {
|
||||
if (cyberarmStates.size() > 0) {
|
||||
runState(cyberarmStates.get(0));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Engine main loop
|
||||
* ENSURE to call super.loop() if you override this method
|
||||
*/
|
||||
public void loop() {
|
||||
CyberarmState state;
|
||||
|
||||
// Try to set state to the current state, if it fails assume that there are no states to run
|
||||
try {
|
||||
state = cyberarmStates.get(activeStateIndex);
|
||||
} catch(IndexOutOfBoundsException e) {
|
||||
// The engine is now out of states.
|
||||
stop();
|
||||
|
||||
telemetry.addLine("" + this.getClass().getSimpleName() + " is out of states to run!");
|
||||
telemetry.addLine();
|
||||
return;
|
||||
}
|
||||
|
||||
// Add telemetry to show currently running state
|
||||
telemetry.addLine("Running state: " +state.getClass().getSimpleName() + ". State: " + activeStateIndex + " of " + (cyberarmStates.size()-1));
|
||||
if (showStateChildrenListInTelemetry && state.hasChildren()) {
|
||||
for(CyberarmState child: state.children) {
|
||||
telemetry.addLine(" Child: " + child.getClass().getSimpleName() + " [" + child.children.size() + "] grandchildren");
|
||||
}
|
||||
}
|
||||
telemetry.addLine();
|
||||
|
||||
if (state.getHasFinished() && state.childrenHaveFinished()) {
|
||||
activeStateIndex++;
|
||||
|
||||
try {
|
||||
state = cyberarmStates.get(activeStateIndex);
|
||||
runState(state);
|
||||
} catch(IndexOutOfBoundsException e) { /* loop will handle this in a few milliseconds */ }
|
||||
|
||||
} else {
|
||||
stateTelemetry(state);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Stops every known state
|
||||
*/
|
||||
@Override
|
||||
public void stop() {
|
||||
for (CyberarmState state: cyberarmStates) {
|
||||
stopState(state);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Recursively calls telemetry() on states
|
||||
* @param state State to get telemetry
|
||||
*/
|
||||
private void stateTelemetry(CyberarmState state) {
|
||||
if (!state.getHasFinished()) {
|
||||
state.telemetry();
|
||||
}
|
||||
|
||||
for(CyberarmState childState : state.children) {
|
||||
if (!childState.getHasFinished()) {
|
||||
stateTelemetry(childState);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Called when INIT button on Driver Station is pressed
|
||||
* Recursively initiates states
|
||||
* @param state State to initiate
|
||||
*/
|
||||
private void initState(CyberarmState state) {
|
||||
state.init();
|
||||
|
||||
for(CyberarmState childState : state.children) {
|
||||
initState(childState);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Called when programs ends or STOP button on Driver Station is pressed
|
||||
* Recursively stop states
|
||||
* @param state State to stop
|
||||
*/
|
||||
public void stopState(CyberarmState state) {
|
||||
state.setHasFinished(true);
|
||||
state.stop();
|
||||
|
||||
for(CyberarmState childState : state.children) {
|
||||
stopState(childState);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Recursively start up states
|
||||
* @param state State to run
|
||||
*/
|
||||
protected void runState(CyberarmState state) {
|
||||
final CyberarmState finalState = state;
|
||||
// if (state.isRunning()) { return; } // Assume that we have already started running this state
|
||||
|
||||
new Thread(new Runnable() {
|
||||
@Override
|
||||
public void run() {
|
||||
finalState.prestart();
|
||||
finalState.start();
|
||||
finalState.startTime = System.currentTimeMillis();
|
||||
finalState.run();
|
||||
}
|
||||
}).start();
|
||||
|
||||
for (CyberarmState kid : state.children) {
|
||||
runState(kid);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Add state to queue, will call init() on state if engine is running
|
||||
* @param state State to add to queue
|
||||
*/
|
||||
public CyberarmState addState(CyberarmState state) {
|
||||
Log.i(TAG, "Adding cyberarmState "+ state.getClass());
|
||||
cyberarmStates.add(state);
|
||||
|
||||
if (isRunning()) { initState(state); }
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Inserts state after the query state plus an offset to ensure logical insertion
|
||||
* @param query State to add state after
|
||||
* @param state State to be inserted
|
||||
* @return
|
||||
*/
|
||||
public CyberarmState insertState(CyberarmState query, CyberarmState state) {
|
||||
int index = cyberarmStates.indexOf(query) + query.insertOffset;
|
||||
Log.i(TAG, "Adding cyberarmState "+ state.getClass());
|
||||
|
||||
cyberarmStates.add(index, state);
|
||||
query.insertOffset++;
|
||||
|
||||
if (isRunning()) { initState(state); }
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
/**
|
||||
* This will return false while Engine.setup() is executing, and be true after.
|
||||
* @return Whether the engine main loop is running
|
||||
*/
|
||||
public boolean isRunning() {
|
||||
return isRunning;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @return The index used to lookup the current state from cyberarmStates
|
||||
*/
|
||||
public int getActiveStateIndex() {
|
||||
return activeStateIndex;
|
||||
}
|
||||
}
|
||||
216
TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java
Normal file
216
TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmState.java
Normal file
@@ -0,0 +1,216 @@
|
||||
package org.cyberarm.engine.V2;
|
||||
|
||||
import android.util.Log;
|
||||
|
||||
import java.util.concurrent.CopyOnWriteArrayList;
|
||||
|
||||
/**
|
||||
* A State for use with CyberarmEngineV2
|
||||
*/
|
||||
public abstract class CyberarmState implements Runnable {
|
||||
|
||||
private volatile boolean isRunning, hasFinished;
|
||||
public static String TAG = "PROGRAM.STATE";
|
||||
public CyberarmEngine engine = CyberarmEngine.instance;
|
||||
public CopyOnWriteArrayList<CyberarmState> children = new CopyOnWriteArrayList<>();
|
||||
public long startTime = 0;
|
||||
public int insertOffset = 1;
|
||||
|
||||
/**
|
||||
* Called when INIT button on Driver Station is pushed
|
||||
*/
|
||||
public void init() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Called just before start to ensure state is in correct state
|
||||
*/
|
||||
protected void prestart() {
|
||||
isRunning = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Called when state has begin to run
|
||||
*/
|
||||
public void start() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Called while State is running
|
||||
*/
|
||||
public abstract void exec();
|
||||
|
||||
/**
|
||||
* State's main loop, calls exec() until hasFinished is true
|
||||
* DO NO OVERRIDE
|
||||
*/
|
||||
@Override
|
||||
public void run() {
|
||||
while (!hasFinished) {
|
||||
exec();
|
||||
}
|
||||
isRunning = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Place telemetry calls in here instead of inside exec() to have them displayed correctly on the Driver Station
|
||||
* (States update thousands of times per second, resulting in missing or weirdly formatted telemetry if telemetry is added in exec())
|
||||
*/
|
||||
public void telemetry() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Called when Engine is finished
|
||||
*/
|
||||
public void stop() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a state which runs in parallel with this one
|
||||
*/
|
||||
public CyberarmState addParallelState(CyberarmState state) {
|
||||
Log.i(TAG, "Adding " + state.getClass() + " to " + this.getClass());
|
||||
children.add(state);
|
||||
|
||||
if (isRunning()) {
|
||||
state.init();
|
||||
engine.runState(state);
|
||||
Log.i(TAG, "Started " + state.getClass() + " in " + this.getClass());
|
||||
}
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a state to engine which will run after this one finishes
|
||||
*/
|
||||
public CyberarmState addState(CyberarmState state) {
|
||||
engine.insertState(this, state);
|
||||
|
||||
return state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether or not state has children
|
||||
* @return True if state has children, false otherwise
|
||||
*/
|
||||
public boolean hasChildren() {
|
||||
return (children.size() > 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Have all of the states children finished running themselves?
|
||||
* @return Whether or not all children have finished running
|
||||
*/
|
||||
public boolean childrenHaveFinished() {
|
||||
return childrenHaveFinished(children);
|
||||
}
|
||||
|
||||
/**
|
||||
* Have all of the states children finished running themselves?
|
||||
* @param kids CopyOnWriteArrayList of children to check for hasFinished()
|
||||
* @return Whether or not all children have finished running
|
||||
*/
|
||||
public boolean childrenHaveFinished(CopyOnWriteArrayList<CyberarmState> kids) {
|
||||
boolean allDone = true;
|
||||
|
||||
for (CyberarmState state : kids) {
|
||||
if (!state.hasFinished) {
|
||||
allDone = false;
|
||||
break;
|
||||
} else {
|
||||
if (!state.childrenHaveFinished()) {
|
||||
allDone = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return allDone;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @return The number of milliseconds this state has been running for
|
||||
*/
|
||||
public double runTime() {
|
||||
return (System.currentTimeMillis() - startTime);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set whether state has finished or not
|
||||
* @param value
|
||||
*/
|
||||
public void setHasFinished(boolean value) {
|
||||
hasFinished = value;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @return Get value of hasFinished
|
||||
*/
|
||||
public boolean getHasFinished() {
|
||||
return hasFinished;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @return Get value of isRunning
|
||||
*/
|
||||
public boolean isRunning() {
|
||||
return isRunning;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @param timems How long to sleep in milliseconds
|
||||
*/
|
||||
public void sleep(long timems) {
|
||||
try {
|
||||
Thread.sleep(timems);
|
||||
} catch (InterruptedException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @param width How many characters wide to be
|
||||
* @param percentCompleted Number between 0.0 and 100.0
|
||||
* @param bar What character to draw the completion bar with
|
||||
* @param padding What character to draw non-completed bar with
|
||||
* @return A string
|
||||
*/
|
||||
public String progressBar(int width, double percentCompleted, String bar, String padding) {
|
||||
String percentCompletedString = "" + Math.round(percentCompleted) + "%";
|
||||
double activeWidth = (width - 2) - percentCompletedString.length();
|
||||
|
||||
String string = "[";
|
||||
double completed = (percentCompleted / 100.0) * activeWidth;
|
||||
|
||||
for (int i = 0; i <= ((int) activeWidth); i++) {
|
||||
if (i == ((int) activeWidth) / 2) {
|
||||
string += percentCompletedString;
|
||||
} else {
|
||||
if (i <= (int) completed && (int) completed > 0) {
|
||||
string += bar;
|
||||
} else {
|
||||
string += padding;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
string += "]";
|
||||
return string;
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @param width How many characters wide to be
|
||||
* @param percentCompleted Number between 0.0 and 100.0
|
||||
* @return A string
|
||||
*/
|
||||
public String progressBar(int width, double percentCompleted) {
|
||||
return progressBar(width, percentCompleted, "=", " ");
|
||||
}
|
||||
}
|
||||
28
TeamCode/src/main/java/org/cyberarm/engine/V2/README.txt
Normal file
28
TeamCode/src/main/java/org/cyberarm/engine/V2/README.txt
Normal file
@@ -0,0 +1,28 @@
|
||||
CyberarmEngine V2 Architecture
|
||||
|
||||
Engine
|
||||
-> [States]
|
||||
-> [ParallelStates]
|
||||
|
||||
Start with an Engine and override setup():
|
||||
|
||||
public class Engine extends CyberarmEngineV2 {
|
||||
public void setup() {
|
||||
addState(new State(arguments));
|
||||
}
|
||||
}
|
||||
|
||||
NOTE: states do not need to be passed the instance of Engine as they have a field 'cyberarmEngine'
|
||||
which is set to CyberarmEngineV2.instance when they are created.
|
||||
|
||||
States can have 'children' which are also States, which run in parallel with their parent. There is
|
||||
no fixed limit to how many grandchildren can exist (Children themselves can have children.):
|
||||
|
||||
public class State extends CyberarmEngineStateV2 {
|
||||
public init() {
|
||||
addParallelState(new ParallelState(arguments));
|
||||
}
|
||||
|
||||
public exec() {
|
||||
}
|
||||
}
|
||||
@@ -1,121 +0,0 @@
|
||||
## TeamCode Module
|
||||
|
||||
Welcome!
|
||||
|
||||
This module, TeamCode, is the place where you will write/paste the code for your team's
|
||||
robot controller App. This module is currently empty (a clean slate) but the
|
||||
process for adding OpModes is straightforward.
|
||||
|
||||
## Creating your own OpModes
|
||||
|
||||
The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
|
||||
|
||||
Sample opmodes exist in the FtcRobotController module.
|
||||
To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
|
||||
|
||||
Expand the following tree elements:
|
||||
FtcRobotController / java / org.firstinspires.ftc.robotcontroller / external / samples
|
||||
|
||||
A range of different samples classes can be seen in this folder.
|
||||
The class names follow a naming convention which indicates the purpose of each class.
|
||||
The full description of this convention is found in the samples/sample_convention.md file.
|
||||
|
||||
A brief synopsis of the naming convention is given here:
|
||||
The prefix of the name will be one of the following:
|
||||
|
||||
* Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
|
||||
of a particular style of OpMode. These are bare bones examples.
|
||||
* Sensor: This is a Sample OpMode that shows how to use a specific sensor.
|
||||
It is not intended as a functioning robot, it is simply showing the minimal code
|
||||
required to read and display the sensor values.
|
||||
* Hardware: This is not an actual OpMode, but a helper class that is used to describe
|
||||
one particular robot's hardware devices: eg: for a Pushbot. Look at any
|
||||
Pushbot sample to see how this can be used in an OpMode.
|
||||
Teams can copy one of these to create their own robot definition.
|
||||
* Pushbot: This is a Sample OpMode that uses the Pushbot robot structure as a base.
|
||||
* Concept: This is a sample OpMode that illustrates performing a specific function or concept.
|
||||
These may be complex, but their operation should be explained clearly in the comments,
|
||||
or the header should reference an external doc, guide or tutorial.
|
||||
* Library: This is a class, or set of classes used to implement some strategy.
|
||||
These will typically NOT implement a full OpMode. Instead they will be included
|
||||
by an OpMode to provide some stand-alone capability.
|
||||
|
||||
Once you are familiar with the range of samples available, you can choose one to be the
|
||||
basis for your own robot. In all cases, the desired sample(s) needs to be copied into
|
||||
your TeamCode module to be used.
|
||||
|
||||
This is done inside Android Studio directly, using the following steps:
|
||||
|
||||
1) Locate the desired sample class in the Project/Android tree.
|
||||
|
||||
2) Right click on the sample class and select "Copy"
|
||||
|
||||
3) Expand the TeamCode / java folder
|
||||
|
||||
4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
|
||||
|
||||
5) You will be prompted for a class name for the copy.
|
||||
Choose something meaningful based on the purpose of this class.
|
||||
Start with a capital letter, and remember that there may be more similar classes later.
|
||||
|
||||
Once your copy has been created, you should prepare it for use on your robot.
|
||||
This is done by adjusting the OpMode's name, and enabling it to be displayed on the
|
||||
Driver Station's OpMode list.
|
||||
|
||||
Each OpMode sample class begins with several lines of code like the ones shown below:
|
||||
|
||||
```
|
||||
@TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
|
||||
@Disabled
|
||||
```
|
||||
|
||||
The name that will appear on the driver station's "opmode list" is defined by the code:
|
||||
``name="Template: Linear OpMode"``
|
||||
You can change what appears between the quotes to better describe your opmode.
|
||||
The "group=" portion of the code can be used to help organize your list of OpModes.
|
||||
|
||||
As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
|
||||
``@Disabled`` annotation which has been included.
|
||||
This line can simply be deleted , or commented out, to make the OpMode visible.
|
||||
|
||||
|
||||
|
||||
## ADVANCED Multi-Team App management: Cloning the TeamCode Module
|
||||
|
||||
In some situations, you have multiple teams in your club and you want them to all share
|
||||
a common code organization, with each being able to *see* the others code but each having
|
||||
their own team module with their own code that they maintain themselves.
|
||||
|
||||
In this situation, you might wish to clone the TeamCode module, once for each of these teams.
|
||||
Each of the clones would then appear along side each other in the Android Studio module list,
|
||||
together with the FtcRobotController module (and the original TeamCode module).
|
||||
|
||||
Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
|
||||
prior to clicking to the green Run arrow.
|
||||
|
||||
Warning: This is not for the inexperienced Software developer.
|
||||
You will need to be comfortable with File manipulations and managing Android Studio Modules.
|
||||
These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
|
||||
|
||||
Also.. Make a full project backup before you start this :)
|
||||
|
||||
To clone TeamCode, do the following:
|
||||
|
||||
Note: Some names start with "Team" and others start with "team". This is intentional.
|
||||
|
||||
1) Using your operating system file management tools, copy the whole "TeamCode"
|
||||
folder to a sibling folder with a corresponding new name, eg: "Team0417".
|
||||
|
||||
2) In the new Team0417 folder, delete the TeamCode.iml file.
|
||||
|
||||
3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
|
||||
to a matching name with a lowercase 'team' eg: "team0417".
|
||||
|
||||
4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
|
||||
package="org.firstinspires.ftc.teamcode"
|
||||
to be
|
||||
package="org.firstinspires.ftc.team0417"
|
||||
|
||||
5) Add: include ':Team0417' to the "/settings.gradle" file.
|
||||
|
||||
6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""
|
||||
@@ -0,0 +1,11 @@
|
||||
package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.StubState;
|
||||
|
||||
public class AutonomousEngine extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
addState(new StubState());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
package org.timecrafters.FreightFrenzy.Competition.Autonomous.States;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class StubState extends CyberarmState {
|
||||
@Override
|
||||
public void exec() {
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,4 @@
|
||||
package org.timecrafters.FreightFrenzy.Competition.Common;
|
||||
|
||||
public class Robot {
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
package org.timecrafters.FreightFrenzy.HardwareTesting.Engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.FreightFrenzy.HardwareTesting.States.ISO8061State;
|
||||
|
||||
@TeleOp(name = "ISO8061-Is-Best", group = "testing")
|
||||
public class ISO8061Engine extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() {
|
||||
addState(new ISO8061State());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
package org.timecrafters.FreightFrenzy.HardwareTesting.States;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class ISO8061State extends CyberarmState {
|
||||
@Override
|
||||
public void exec() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addLine("The ISO8061 Date Format is Best");
|
||||
engine.telemetry.addLine("YYYY-MM-DD");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,150 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library;
|
||||
|
||||
import com.google.gson.Gson;
|
||||
import com.google.gson.GsonBuilder;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.TAC;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ActionDeserializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ActionSerializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigDeserializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigSerializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigurationDeserializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.ConfigurationSerializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.GroupDeserializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.GroupSerializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.PresetsDeserializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.PresetsSerializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.SettingsDeserializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.SettingsSerializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.VariableDeserializer;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.serializers.VariableSerializer;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.FileNotFoundException;
|
||||
import java.io.FileReader;
|
||||
|
||||
public class TimeCraftersConfiguration {
|
||||
private static final String TAG = "TCT|TCConfig";
|
||||
private Config config;
|
||||
|
||||
public TimeCraftersConfiguration() {
|
||||
Settings settings = loadSettings();
|
||||
this.config = loadConfig(settings.config);
|
||||
}
|
||||
|
||||
public TimeCraftersConfiguration(String configName) {
|
||||
this.config = loadConfig(configName);
|
||||
}
|
||||
|
||||
public Config getConfig() {
|
||||
return config;
|
||||
}
|
||||
|
||||
public Group group(String groupName) {
|
||||
for (final Group group : config.getGroups()) {
|
||||
if (group.name.trim().equals(groupName.trim())) {
|
||||
return group;
|
||||
}
|
||||
}
|
||||
|
||||
throw(new RuntimeException("Failed to find a group named:\"" + groupName.trim() + "\" in config \"" + config.getName() + "\""));
|
||||
}
|
||||
|
||||
public Action action(String groupName, String actionName) {
|
||||
final Group group = group(groupName);
|
||||
|
||||
for (Action action : group.getActions()) {
|
||||
if (action.name.trim().equals(actionName.trim())) {
|
||||
return action;
|
||||
}
|
||||
}
|
||||
|
||||
throw(new RuntimeException("Failed to find an action named:\"" + actionName.trim() + "\" in group \"" + groupName.trim() + "\" in config \"" + config.getName() + "\""));
|
||||
}
|
||||
|
||||
public Variable variable(String groupName, String actionName, String variableName) {
|
||||
final Action action = action(groupName, actionName);
|
||||
|
||||
for (Variable variable : action.getVariables()) {
|
||||
if (variable.name.trim().equals(variableName.trim())) {
|
||||
return variable;
|
||||
}
|
||||
}
|
||||
|
||||
throw(new RuntimeException("Failed to find a variable named \"" + variableName.trim() + "\" in action:\"" + actionName.trim() +
|
||||
"\" in group \"" + groupName.trim() + "\" in config \"" + config.getName() + "\""));
|
||||
}
|
||||
|
||||
private Settings loadSettings() {
|
||||
File settingsFile = new File(TAC.SETTINGS_PATH);
|
||||
|
||||
if (!settingsFile.exists()) {
|
||||
throw( new RuntimeException("Unable to load settings.json, file does not exist!") );
|
||||
}
|
||||
|
||||
try {
|
||||
return gsonForSettings().fromJson(new FileReader(settingsFile), Settings.class);
|
||||
} catch (FileNotFoundException e) {
|
||||
throw( new RuntimeException("Unable to load settings.json") );
|
||||
}
|
||||
}
|
||||
|
||||
private Config loadConfig(String name) {
|
||||
if (name.equals("")) {
|
||||
throw(new RuntimeException("Cannot load a config with an empty name!"));
|
||||
}
|
||||
|
||||
String path = TAC.CONFIGS_PATH + File.separator + name + ".json";
|
||||
File configFile = new File(path);
|
||||
|
||||
if (configFile.exists() && configFile.isFile()) {
|
||||
try {
|
||||
Config config = gsonForConfig().fromJson(new FileReader(configFile), Config.class);
|
||||
config.setName(name);
|
||||
|
||||
return config;
|
||||
} catch (FileNotFoundException e) {
|
||||
e.printStackTrace();
|
||||
throw(new RuntimeException("Unable to find a config file named \"" + name + "\""));
|
||||
}
|
||||
} else {
|
||||
throw(new RuntimeException("Unable to find a config file named \"" + name + "\""));
|
||||
}
|
||||
}
|
||||
|
||||
private Gson gsonForSettings() {
|
||||
return new GsonBuilder()
|
||||
.registerTypeAdapter(Settings.class, new SettingsSerializer())
|
||||
.registerTypeAdapter(Settings.class, new SettingsDeserializer())
|
||||
.create();
|
||||
}
|
||||
|
||||
public Gson gsonForConfig() {
|
||||
return new GsonBuilder()
|
||||
.registerTypeAdapter(Config.class, new ConfigSerializer())
|
||||
.registerTypeAdapter(Config.class, new ConfigDeserializer())
|
||||
|
||||
.registerTypeAdapter(Configuration.class, new ConfigurationSerializer())
|
||||
.registerTypeAdapter(Configuration.class, new ConfigurationDeserializer())
|
||||
|
||||
.registerTypeAdapter(Group.class, new GroupSerializer())
|
||||
.registerTypeAdapter(Group.class, new GroupDeserializer())
|
||||
|
||||
.registerTypeAdapter(Action.class, new ActionSerializer())
|
||||
.registerTypeAdapter(Action.class, new ActionDeserializer())
|
||||
|
||||
.registerTypeAdapter(Variable.class, new VariableSerializer())
|
||||
.registerTypeAdapter(Variable.class, new VariableDeserializer())
|
||||
|
||||
.registerTypeAdapter(Presets.class, new PresetsSerializer())
|
||||
.registerTypeAdapter(Presets.class, new PresetsDeserializer())
|
||||
.create();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Date;
|
||||
|
||||
public class Config {
|
||||
private String name;
|
||||
private Configuration configuration;
|
||||
private ArrayList<Group> groups;
|
||||
private Presets presets;
|
||||
|
||||
public Config(String name) {
|
||||
this.name = name;
|
||||
this.configuration = new Configuration(new Date(), new Date(), TAC.CONFIG_SPEC_VERSION, 0);
|
||||
groups = new ArrayList<>();
|
||||
presets = new Presets(new ArrayList<Group>(), new ArrayList<Action>());
|
||||
}
|
||||
|
||||
public Config(Configuration configuration, ArrayList<Group> groups, Presets presets) {
|
||||
this.configuration = configuration;
|
||||
this.groups = groups;
|
||||
this.presets = presets;
|
||||
}
|
||||
|
||||
public String getName() { return name; }
|
||||
public void setName(String name) { this.name = name; }
|
||||
|
||||
public Configuration getConfiguration() {
|
||||
return configuration;
|
||||
}
|
||||
|
||||
public Presets getPresets() {
|
||||
return presets;
|
||||
}
|
||||
|
||||
public ArrayList<Group> getGroups() {
|
||||
return groups;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend;
|
||||
|
||||
public class Settings {
|
||||
public String hostname, config;
|
||||
public int port;
|
||||
|
||||
public Settings(String hostname, int port, String config) {
|
||||
this.hostname = hostname;
|
||||
this.port = port;
|
||||
this.config = config;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend;
|
||||
|
||||
import android.os.Environment;
|
||||
|
||||
import java.io.File;
|
||||
|
||||
public class TAC {
|
||||
// TODO: Update filesystem handling
|
||||
public static final String ROOT_PATH = Environment.getExternalStorageDirectory().getAbsolutePath() + File.separator + "TimeCrafters_Configuration_Tool",
|
||||
CONFIGS_PATH = ROOT_PATH + File.separator + "/configs",
|
||||
SETTINGS_PATH = ROOT_PATH + File.separator + "settings.json";
|
||||
|
||||
public static final int CONFIG_SPEC_VERSION = 2;
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class Action {
|
||||
public String name, comment;
|
||||
public boolean enabled;
|
||||
private ArrayList<Variable> variables;
|
||||
|
||||
public Action(String name, String comment, boolean enabled, ArrayList<Variable> variables) {
|
||||
this.name = name;
|
||||
this.comment = comment;
|
||||
this.enabled = enabled;
|
||||
this.variables = variables;
|
||||
}
|
||||
|
||||
public ArrayList<Variable> getVariables() { return variables; }
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
|
||||
|
||||
import java.util.Date;
|
||||
|
||||
public class Configuration {
|
||||
public static final String DATE_FORMAT = "yyyy-MM-dd HH:mm:ss Z";
|
||||
|
||||
public Date createdAt, updatedAt;
|
||||
private int specVersion;
|
||||
public int revision;
|
||||
|
||||
public Configuration(Date createdAt, Date updatedAt, int specVersion, int revision) {
|
||||
this.createdAt = createdAt;
|
||||
this.updatedAt = updatedAt;
|
||||
this.specVersion = specVersion;
|
||||
this.revision = revision;
|
||||
}
|
||||
|
||||
public int getSpecVersion() { return specVersion; }
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class Group {
|
||||
public String name;
|
||||
private ArrayList<Action> actions;
|
||||
|
||||
public Group(String name, ArrayList<Action> actions) {
|
||||
this.name = name;
|
||||
this.actions = actions;
|
||||
}
|
||||
|
||||
public static boolean nameIsUnique(ArrayList<Group> groups, String name) {
|
||||
for (Group group: groups) {
|
||||
if (group.name.equals(name)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public ArrayList<Action> getActions() {
|
||||
return actions;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
|
||||
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class Presets {
|
||||
private ArrayList<Group> groups;
|
||||
private ArrayList<Action> actions;
|
||||
|
||||
public Presets(ArrayList<Group> groups, ArrayList<Action> actions) {
|
||||
this.groups = groups;
|
||||
this.actions = actions;
|
||||
}
|
||||
|
||||
public ArrayList<Group> getGroups() {
|
||||
return groups;
|
||||
}
|
||||
|
||||
public ArrayList<Action> getActions() {
|
||||
return actions;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.backend.config;
|
||||
|
||||
import android.util.Log;
|
||||
|
||||
import java.util.Arrays;
|
||||
|
||||
public class Variable {
|
||||
public String name;
|
||||
private String value;
|
||||
|
||||
public Variable(String name, String value) {
|
||||
this.name = name;
|
||||
this.value = value;
|
||||
}
|
||||
|
||||
public String rawValue() {
|
||||
return value;
|
||||
}
|
||||
|
||||
public <T> T value() {
|
||||
return valueOf(value);
|
||||
}
|
||||
|
||||
public void setValue(String value) {
|
||||
this.value = value;
|
||||
}
|
||||
|
||||
@SuppressWarnings("unchecked")
|
||||
static public <T> T valueOf(String value) {
|
||||
String[] split = value.split("x", 2);
|
||||
// Log.d("Variable", "valueOf split: " + Arrays.toString(split));
|
||||
|
||||
switch (split[0]) {
|
||||
case "B": {
|
||||
return (T) Boolean.valueOf(split[(split.length-1)]);
|
||||
}
|
||||
case "D": {
|
||||
return (T) Double.valueOf(split[(split.length-1)]);
|
||||
}
|
||||
case "F": {
|
||||
return (T) Float.valueOf(split[(split.length-1)]);
|
||||
}
|
||||
case "I": {
|
||||
return (T) Integer.valueOf(split[(split.length-1)]);
|
||||
}
|
||||
case "L": {
|
||||
return (T) Long.valueOf(split[(split.length-1)]);
|
||||
}
|
||||
case "S": {
|
||||
String string = "";
|
||||
int i = 0;
|
||||
for(String str : split) {
|
||||
if (i == 0) { i++; continue; }
|
||||
|
||||
string += str;
|
||||
}
|
||||
return (T) string;
|
||||
}
|
||||
default: {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static public String typeOf(String value) {
|
||||
String[] split = value.split("x");
|
||||
|
||||
switch (split[0]) {
|
||||
case "B": {
|
||||
return "Boolean";
|
||||
}
|
||||
case "D": {
|
||||
return "Double";
|
||||
}
|
||||
case "F": {
|
||||
return "Float";
|
||||
}
|
||||
case "I": {
|
||||
return "Integer";
|
||||
}
|
||||
case "L": {
|
||||
return "Long";
|
||||
}
|
||||
case "S": {
|
||||
return "String";
|
||||
}
|
||||
default: {
|
||||
return "=!UNKNOWN!=";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class ActionDeserializer implements JsonDeserializer<Action> {
|
||||
@Override
|
||||
public Action deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
|
||||
JsonObject jsonObject = json.getAsJsonObject();
|
||||
|
||||
final String name = jsonObject.get("name").getAsString();
|
||||
final String comment = jsonObject.get("comment").getAsString();
|
||||
final boolean enabled = jsonObject.get("enabled").getAsBoolean();
|
||||
Variable[] variablesArray = context.deserialize(jsonObject.get("variables"), Variable[].class);
|
||||
|
||||
List<Variable> variablesList = Arrays.asList(variablesArray);
|
||||
ArrayList<Variable> variables = new ArrayList<>(variablesList);
|
||||
|
||||
return new Action(name, comment, enabled, variables);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonPrimitive;
|
||||
import com.google.gson.JsonSerializationContext;
|
||||
import com.google.gson.JsonSerializer;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class ActionSerializer implements JsonSerializer<Action> {
|
||||
@Override
|
||||
public JsonElement serialize(Action action, Type type, JsonSerializationContext context) {
|
||||
JsonObject container = new JsonObject();
|
||||
|
||||
container.add("name", new JsonPrimitive(action.name));
|
||||
container.add("comment", new JsonPrimitive(action.comment));
|
||||
container.add("enabled", new JsonPrimitive(action.enabled));
|
||||
container.add("variables", context.serialize(action.getVariables().toArray(), Variable[].class));
|
||||
|
||||
return container;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class ConfigDeserializer implements JsonDeserializer<Config> {
|
||||
@Override
|
||||
public Config deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
|
||||
JsonObject jsonObject = json.getAsJsonObject();
|
||||
JsonObject data = jsonObject.get("data").getAsJsonObject();
|
||||
|
||||
Configuration configuration = context.deserialize(jsonObject.get("config"), Configuration.class);
|
||||
Group[] groupsArray = context.deserialize(data.get("groups"), Group[].class);
|
||||
List<Group> groupsList = Arrays.asList(groupsArray);
|
||||
ArrayList<Group> groups = new ArrayList<>(groupsList);
|
||||
|
||||
Presets presets = context.deserialize(data.get("presets"), Presets.class);
|
||||
|
||||
return new Config(configuration, groups, presets);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonSerializationContext;
|
||||
import com.google.gson.JsonSerializer;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class ConfigSerializer implements JsonSerializer<Config> {
|
||||
@Override
|
||||
public JsonElement serialize(Config config, Type type, JsonSerializationContext context) {
|
||||
JsonObject container = new JsonObject();
|
||||
JsonObject result = new JsonObject();
|
||||
JsonObject presets = new JsonObject();
|
||||
container.add("config", context.serialize(config.getConfiguration(), Configuration.class));
|
||||
result.add("groups", context.serialize(config.getGroups().toArray(), Group[].class));
|
||||
|
||||
presets.add("groups", context.serialize(config.getPresets().getGroups().toArray(), Group[].class));
|
||||
presets.add("actions", context.serialize(config.getPresets().getActions().toArray(), Action[].class));
|
||||
|
||||
result.add("presets", presets);
|
||||
container.add("data", result);
|
||||
|
||||
return container;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
import java.text.ParseException;
|
||||
import java.text.SimpleDateFormat;
|
||||
import java.util.Date;
|
||||
|
||||
public class ConfigurationDeserializer implements JsonDeserializer<Configuration> {
|
||||
@Override
|
||||
public Configuration deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
|
||||
JsonObject config = json.getAsJsonObject();
|
||||
|
||||
SimpleDateFormat dateFormat = new SimpleDateFormat(Configuration.DATE_FORMAT);
|
||||
Date createdAt = new Date();
|
||||
Date updatedAt = new Date();
|
||||
try {
|
||||
createdAt = dateFormat.parse(config.get("created_at").getAsString());
|
||||
updatedAt = dateFormat.parse(config.get("updated_at").getAsString());
|
||||
} catch (ParseException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
final int spec_version = config.get("spec_version").getAsInt();
|
||||
final int revision = config.get("revision").getAsInt();
|
||||
|
||||
return new Configuration(createdAt, updatedAt, spec_version, revision);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonPrimitive;
|
||||
import com.google.gson.JsonSerializationContext;
|
||||
import com.google.gson.JsonSerializer;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
import java.text.SimpleDateFormat;
|
||||
|
||||
public class ConfigurationSerializer implements JsonSerializer<Configuration> {
|
||||
@Override
|
||||
public JsonElement serialize(Configuration configuration, Type type, JsonSerializationContext context) {
|
||||
JsonObject container = new JsonObject();
|
||||
|
||||
SimpleDateFormat dateFormat = new SimpleDateFormat(Configuration.DATE_FORMAT);
|
||||
|
||||
container.add("created_at", new JsonPrimitive(dateFormat.format(configuration.createdAt)));
|
||||
container.add("updated_at", new JsonPrimitive(dateFormat.format(configuration.updatedAt)));
|
||||
container.add("spec_version", new JsonPrimitive(configuration.getSpecVersion()));
|
||||
container.add("revision", new JsonPrimitive(configuration.revision));
|
||||
|
||||
return container;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class GroupDeserializer implements JsonDeserializer<Group> {
|
||||
@Override
|
||||
public Group deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
|
||||
JsonObject jsonObject = json.getAsJsonObject();
|
||||
|
||||
final String name = jsonObject.get("name").getAsString();
|
||||
Action[] actionsArray = context.deserialize(jsonObject.get("actions"), Action[].class);
|
||||
|
||||
List<Action> actionsList = Arrays.asList(actionsArray);
|
||||
ArrayList<Action> actions = new ArrayList<>(actionsList);
|
||||
|
||||
return new Group(name, actions);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonPrimitive;
|
||||
import com.google.gson.JsonSerializationContext;
|
||||
import com.google.gson.JsonSerializer;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Config;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class GroupSerializer implements JsonSerializer<Group> {
|
||||
@Override
|
||||
public JsonElement serialize(Group group, Type type, JsonSerializationContext context) {
|
||||
JsonObject container = new JsonObject();
|
||||
|
||||
container.add("name", new JsonPrimitive(group.name));
|
||||
container.add("actions", context.serialize(group.getActions().toArray(), Action[].class));
|
||||
|
||||
return container;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class PresetsDeserializer implements JsonDeserializer<Presets> {
|
||||
@Override
|
||||
public Presets deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
|
||||
JsonObject jsonObject = json.getAsJsonObject();
|
||||
|
||||
Group[] GroupsArray = context.deserialize(jsonObject.get("groups"), Group[].class);
|
||||
Action[] actionsArray = context.deserialize(jsonObject.get("actions"), Action[].class);
|
||||
|
||||
List<Group> groupsList = Arrays.asList(GroupsArray);
|
||||
ArrayList<Group> groups = new ArrayList<>(groupsList);
|
||||
List<Action> actionsList = Arrays.asList(actionsArray);
|
||||
ArrayList<Action> actions = new ArrayList<>(actionsList);
|
||||
|
||||
return new Presets(groups, actions);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonPrimitive;
|
||||
import com.google.gson.JsonSerializationContext;
|
||||
import com.google.gson.JsonSerializer;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class PresetsSerializer implements JsonSerializer<Presets> {
|
||||
@Override
|
||||
public JsonElement serialize(Presets presets, Type type, JsonSerializationContext context) {
|
||||
JsonObject container = new JsonObject();
|
||||
|
||||
container.add("groups", context.serialize(presets.getGroups().toArray(), Group[].class));
|
||||
container.add("actions", context.serialize(presets.getActions().toArray(), Action[].class));
|
||||
|
||||
return container;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class SettingsDeserializer implements JsonDeserializer<Settings> {
|
||||
@Override
|
||||
public Settings deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
|
||||
JsonObject jsonObject = json.getAsJsonObject();
|
||||
JsonObject data = jsonObject.get("data").getAsJsonObject();
|
||||
|
||||
return new Settings(
|
||||
data.get("hostname").getAsString(),
|
||||
data.get("port").getAsInt(),
|
||||
data.get("config").getAsString()
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
import com.google.gson.JsonPrimitive;
|
||||
import com.google.gson.JsonSerializationContext;
|
||||
import com.google.gson.JsonSerializer;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.Settings;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class SettingsSerializer implements JsonSerializer<Settings> {
|
||||
@Override
|
||||
public JsonElement serialize(Settings settings, Type type, JsonSerializationContext context) {
|
||||
JsonObject container = new JsonObject();
|
||||
JsonObject result = new JsonObject();
|
||||
result.add("hostname", new JsonPrimitive(settings.hostname));
|
||||
result.add("port", new JsonPrimitive(settings.port));
|
||||
result.add("config", new JsonPrimitive(settings.config));
|
||||
|
||||
container.add("data", result);
|
||||
|
||||
return container;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,28 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonDeserializationContext;
|
||||
import com.google.gson.JsonDeserializer;
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonParseException;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class VariableDeserializer implements JsonDeserializer<Variable> {
|
||||
@Override
|
||||
public Variable deserialize(JsonElement json, Type type, JsonDeserializationContext context) throws JsonParseException {
|
||||
JsonObject jsonObject = json.getAsJsonObject();
|
||||
|
||||
final String name = jsonObject.get("name").getAsString();
|
||||
final String value = jsonObject.get("value").getAsString();
|
||||
|
||||
return new Variable(name, value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
package org.timecrafters.TimeCraftersConfigurationTool.library.serializers;
|
||||
|
||||
import com.google.gson.JsonElement;
|
||||
import com.google.gson.JsonObject;
|
||||
import com.google.gson.JsonPrimitive;
|
||||
import com.google.gson.JsonSerializationContext;
|
||||
import com.google.gson.JsonSerializer;
|
||||
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable;
|
||||
|
||||
import java.lang.reflect.Type;
|
||||
|
||||
public class VariableSerializer implements JsonSerializer<Variable> {
|
||||
@Override
|
||||
public JsonElement serialize(Variable variable, Type type, JsonSerializationContext context) {
|
||||
JsonObject container = new JsonObject();
|
||||
|
||||
container.add("name", new JsonPrimitive(variable.name));
|
||||
container.add("value", new JsonPrimitive(variable.rawValue()));
|
||||
|
||||
return container;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
package org.timecrafters.UltimateGoal.Calibration;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class CalibrateRingBeltLoop extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private float startingTick;
|
||||
private int currentTick;
|
||||
private double ticksPerLoop;
|
||||
private boolean limit;
|
||||
private boolean hasSeenLimit;
|
||||
private boolean limitPrevious;
|
||||
|
||||
public CalibrateRingBeltLoop(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
currentTick = robot.ringBeltMotor.getCurrentPosition();
|
||||
limit = robot.magnetSensor.isPressed();
|
||||
|
||||
if (engine.gamepad1.x || (engine.gamepad1.a && !limit)) {
|
||||
robot.ringBeltMotor.setPower(0.5);
|
||||
} else {
|
||||
robot.ringBeltMotor.setPower(0);
|
||||
}
|
||||
|
||||
if (engine.gamepad1.y) {
|
||||
robot.ringBeltMotor.setPower(-0.1);
|
||||
} else {
|
||||
robot.ringBeltMotor.setPower(0);
|
||||
}
|
||||
|
||||
if (engine.gamepad1.b) {
|
||||
startingTick = currentTick;
|
||||
}
|
||||
|
||||
if (limit && !limitPrevious){
|
||||
if (hasSeenLimit) {
|
||||
ticksPerLoop = currentTick - startingTick;
|
||||
}
|
||||
startingTick = currentTick;
|
||||
hasSeenLimit = true;
|
||||
}
|
||||
limitPrevious = limit;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("ticks", currentTick - startingTick);
|
||||
engine.telemetry.addData("limit swich", limit );
|
||||
engine.telemetry.addData("Clockwise", ticksPerLoop);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
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;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
robot = new Robot(hardwareMap);
|
||||
robot.initHardware();
|
||||
super.init();
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
addState(new OdometryCalibration(robot));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
package org.timecrafters.UltimateGoal.Calibration;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class MechanumBiasCalibrator extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private ArrayList<double[]> Powers = new ArrayList<>();
|
||||
private double BiasFR;
|
||||
private double BiasFL;
|
||||
private double BiasBR;
|
||||
private double BiasBL;
|
||||
private boolean hasCalculated;
|
||||
|
||||
|
||||
public MechanumBiasCalibrator(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.updateLocation();
|
||||
|
||||
if (engine.gamepad1.x) {
|
||||
double[] mecanumPowers = robot.getMecanumPowers(-45, 0.5, 0);
|
||||
|
||||
|
||||
robot.driveFrontLeft.setPower(mecanumPowers[0]);
|
||||
robot.driveFrontRight.setPower(mecanumPowers[1]);
|
||||
robot.driveBackLeft.setPower(mecanumPowers[2]);
|
||||
robot.driveBackRight.setPower(mecanumPowers[3]);
|
||||
|
||||
Powers.add(mecanumPowers);
|
||||
} else {
|
||||
robot.setDrivePower(0,0,0,0 );
|
||||
}
|
||||
|
||||
if (engine.gamepad1.y && !hasCalculated) {
|
||||
hasCalculated = true;
|
||||
|
||||
double sumFR = 0;
|
||||
double sumFL = 0;
|
||||
double sumBR = 0;
|
||||
double sumBL = 0;
|
||||
|
||||
for (double[] powers : Powers) {
|
||||
sumFL+= Math.abs(powers[0]);
|
||||
sumFR+= Math.abs(powers[1]);
|
||||
sumBL+= Math.abs(powers[2]);
|
||||
sumBR+= Math.abs(powers[3]);
|
||||
}
|
||||
|
||||
int length = Powers.size();
|
||||
BiasFR = sumFR / length;
|
||||
BiasFL = sumFL / length;
|
||||
BiasBR = sumBR / length;
|
||||
BiasBL = sumBL / length;
|
||||
|
||||
double max = Math.max(Math.max(BiasFL,BiasFR),Math.max(BiasBL,BiasBR));
|
||||
|
||||
BiasFL = BiasFL /max;
|
||||
BiasFR = BiasFR /max;
|
||||
BiasBL = BiasBL /max;
|
||||
BiasBR = BiasBR /max;
|
||||
|
||||
} else if (!engine.gamepad1.y) {
|
||||
hasCalculated = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("FrontLeft", BiasFL);
|
||||
engine.telemetry.addData("FrontRight", BiasFR);
|
||||
engine.telemetry.addData("BackLeft", BiasBL);
|
||||
engine.telemetry.addData("BackRight", BiasBR);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
package org.timecrafters.UltimateGoal.Calibration;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.Engagable;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.DriveMotor;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class MotorCheck extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private double[] Powers;
|
||||
private DcMotor[] Motors;
|
||||
private int currentMotor = 0;
|
||||
private double currentPower = 0.001;
|
||||
|
||||
private boolean yPrevious;
|
||||
|
||||
|
||||
public MotorCheck(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
Motors = new DcMotor[]{robot.driveFrontLeft, robot.driveFrontRight, robot.driveBackLeft, robot.driveBackRight};
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
Motors[currentMotor].setPower(0.2);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
if (currentMotor <= 3) {
|
||||
|
||||
boolean y = engine.gamepad1.y;
|
||||
if (y && !yPrevious) {
|
||||
|
||||
Motors[currentMotor].setPower(0);
|
||||
currentMotor += 1;
|
||||
Motors[currentMotor].setPower(0.2);
|
||||
sleep(20);
|
||||
}
|
||||
yPrevious = y;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
|
||||
engine.telemetry.addData("current Motor", Motors[currentMotor].toString());
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
package org.timecrafters.UltimateGoal.Calibration;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class OdometryCalibration extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private float rotation = 0;
|
||||
private float rotationPrev = 0;
|
||||
private int currentTick;
|
||||
private double ticksPerDegreeClockwise;
|
||||
private double ticksPerDegreeCounterClockwise;
|
||||
private long timePrevious;
|
||||
private long timeChange;
|
||||
|
||||
public OdometryCalibration(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.updateLocation();
|
||||
|
||||
long timeCurrent = System.currentTimeMillis();
|
||||
timeChange = timeCurrent - timePrevious;
|
||||
|
||||
if (timeChange >= 1200) {
|
||||
timePrevious = timeCurrent;
|
||||
|
||||
|
||||
double power = 0.25;
|
||||
float imu = -robot.imu.getAngularOrientation().firstAngle;
|
||||
rotation += robot.getRelativeAngle(imu, rotationPrev);
|
||||
rotationPrev = imu;
|
||||
|
||||
currentTick = (robot.encoderRight.getCurrentPosition() - robot.encoderLeft.getCurrentPosition()) / 2;
|
||||
|
||||
if (engine.gamepad1.x) {
|
||||
robot.setDrivePower(power, -power, power, -power);
|
||||
ticksPerDegreeClockwise = currentTick / rotation;
|
||||
} else if (engine.gamepad1.y) {
|
||||
robot.setDrivePower(-power, power, -power, power);
|
||||
ticksPerDegreeCounterClockwise = currentTick / rotation;
|
||||
} else {
|
||||
robot.setDrivePower(0, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
if (engine.gamepad1.b) {
|
||||
robot.record("Clockwise : "+ticksPerDegreeClockwise);
|
||||
robot.record("Counter Clockwise : "+ticksPerDegreeCounterClockwise);
|
||||
robot.saveRecording();
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Position ","("+round(robot.ticksToInches(robot.getLocationX()),0.1)+","+round(robot.ticksToInches(robot.getLocationY()),0.1)+")");
|
||||
engine.telemetry.addData("Rotation ", robot.getRotation());
|
||||
//
|
||||
// engine.telemetry.addData("degrees", rotation);
|
||||
engine.telemetry.addData("ticks", robot.encoderRight.getCurrentPosition() );
|
||||
engine.telemetry.addData("Clockwise", ticksPerDegreeClockwise);
|
||||
engine.telemetry.addData("CounterClockwise", ticksPerDegreeCounterClockwise);
|
||||
// engine.telemetry.addData("forward", robot.forwardVector);
|
||||
// engine.telemetry.addData("sideways", robot.sidewaysVector);
|
||||
}
|
||||
|
||||
private float round(double number,double unit) {
|
||||
return (float) (Math.floor(number/unit) * unit);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
package org.timecrafters.UltimateGoal.Calibration;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class PerformanceTest extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private ArrayList<Float> angles = new ArrayList<Float>();
|
||||
|
||||
|
||||
public PerformanceTest(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.updateLocation();
|
||||
// robot.record();
|
||||
|
||||
if (engine.gamepad1.x) {
|
||||
double[] powers = robot.getMecanumPowers(0, 1, 0);
|
||||
robot.setDrivePower(powers[0], powers[1], powers[2], powers[3]);
|
||||
} else {
|
||||
robot.setDrivePower(0,0,0,0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,122 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Launch;
|
||||
import org.timecrafters.UltimateGoal.Competition.Pause;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
import org.timecrafters.UltimateGoal.Competition.WobbleArm;
|
||||
import org.timecrafters.UltimateGoal.Competition.WobbleGrab;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
@Disabled
|
||||
@Autonomous(name = "Autonomous")
|
||||
public class AutoEngine extends CyberarmEngine {
|
||||
|
||||
private Robot robot;
|
||||
private TensorFlowCheck tensorFlowCheck;
|
||||
|
||||
private double launchTolerance;
|
||||
private double launchPower;
|
||||
private long launchBrakeTime;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
robot = new Robot(hardwareMap);
|
||||
robot.initHardware();
|
||||
robot.webCamServo.setPosition(Robot.CAM_SERVO_DOWN);
|
||||
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
|
||||
robot.wobbleArmMotor.setTargetPosition(0);
|
||||
robot.wobbleArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
// since we've preloaded three rings, the ring belt stage is set to account for this;
|
||||
robot.ringBeltStage = 3;
|
||||
|
||||
//Autonomous specific 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());
|
||||
robot.setLocalization(rotation,locationX,locationY);
|
||||
|
||||
launchTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto","04_0","tolPos").value());
|
||||
launchPower = robot.stateConfiguration.variable("auto","04_0","power").value();
|
||||
launchBrakeTime = robot.stateConfiguration.variable("auto","04_0","brakeMS").value();
|
||||
|
||||
super.init();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
//drive to view
|
||||
addState(new DriveToCoordinates(robot, "auto", "01_0"));
|
||||
|
||||
//face ring
|
||||
addState(new Face(robot, "auto", "01_1"));
|
||||
|
||||
//select scoring area
|
||||
tensorFlowCheck = new TensorFlowCheck(robot, "auto", "02_0");
|
||||
addState(tensorFlowCheck);
|
||||
|
||||
//drive around ring stack
|
||||
addState(new DriveToCoordinates(robot, "auto", "03_0"));
|
||||
|
||||
//drive to launch position
|
||||
addState(new DriveToCoordinates(robot, robot.launchPositionX,robot.launchPositionY,robot.launchRotation,launchTolerance,launchPower,launchBrakeTime));
|
||||
|
||||
//aligns to goal
|
||||
addState(new Face(robot, "auto", "04_1"));
|
||||
|
||||
//launch rings and drive to scoreing area. LaunchDriveControl allows makes the robot begin
|
||||
//driving while the belt is resetting
|
||||
CyberarmState launchState = new Launch(robot, "auto", "04_2");
|
||||
|
||||
CyberarmState driveState = new DriveToCoordinates(robot, "auto", "05_0", true);
|
||||
addState(new LaunchDriveControl(robot,launchState,driveState));
|
||||
|
||||
//turn arm towards scoreing area.
|
||||
ArrayList<CyberarmState> threadStates0 = new ArrayList<>();
|
||||
threadStates0.add(new Face(robot, "auto", "05_1"));
|
||||
threadStates0.add(new WobbleArm(robot, "auto", "05_2",robot.wobbleDownPos));
|
||||
addState(new ThreadStates(threadStates0));
|
||||
|
||||
//open the wobble grab arm
|
||||
addState(new WobbleGrab(robot, "auto", "06_0", true));
|
||||
|
||||
//drive to second wobble
|
||||
addState(new DriveToCoordinates(robot, "auto","06_1"));
|
||||
|
||||
addState(new DriveToCoordinates(robot, "auto", "07_0"));
|
||||
addState(new Face(robot,"auto","07_1"));
|
||||
|
||||
addState(new FindWobbleGoal(robot, "auto", "08_0"));
|
||||
addState(new Pause(robot,"auto","09_0"));
|
||||
|
||||
//drive to scoring area
|
||||
ArrayList<CyberarmState> threadStates1 = new ArrayList<>();
|
||||
// threadStates1.add(new Face(robot, "auto", "09_0"));
|
||||
threadStates1.add(new WobbleArm(robot, "auto", "09_1",robot.wobbleUpPos));
|
||||
threadStates1.add(new DriveToCoordinates(robot, "auto", "10_0", true));
|
||||
addState(new ThreadStates(threadStates1));
|
||||
|
||||
|
||||
|
||||
ArrayList<CyberarmState> threadStates2 = new ArrayList<>();
|
||||
threadStates2.add(new Face(robot, "auto", "11_0"));
|
||||
threadStates2.add(new WobbleArm(robot, "auto", "11_1",robot.wobbleDownPos));
|
||||
addState(new ThreadStates(threadStates2));
|
||||
|
||||
//release wobble goal 2
|
||||
addState(new WobbleGrab(robot, "auto", "12_0", true));
|
||||
|
||||
//drive to park
|
||||
ArrayList<CyberarmState> threadStates3 = new ArrayList<>();
|
||||
threadStates3.add(new WobbleArm(robot, "auto", "12_2", 0));
|
||||
threadStates3.add(new Park(robot,"auto","13_0"));
|
||||
addState(new ThreadStates(threadStates3));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class DriveToCoordinates extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName = "manual";
|
||||
private String actionName;
|
||||
private double xTarget;
|
||||
private double yTarget;
|
||||
private float faceAngle;
|
||||
private double tolerancePos;
|
||||
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) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
public DriveToCoordinates(Robot robot, String groupName, String actionName, boolean scoringArea) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
this.scoringArea = scoringArea;
|
||||
}
|
||||
|
||||
public DriveToCoordinates(Robot robot, double xTarget, double yTarget, float faceAngle, double tolerance, double power, long brakeTime) {
|
||||
this.robot = robot;
|
||||
this.xTarget = xTarget;
|
||||
this.yTarget = yTarget;
|
||||
this.faceAngle = faceAngle;
|
||||
this.tolerancePos = tolerance;
|
||||
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());
|
||||
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 (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
|
||||
public void start() {
|
||||
if (!groupName.equals("manual")) {
|
||||
setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
|
||||
|
||||
//used to navigate towards the randomly generated scoreing area. the original target
|
||||
//becomes an offset of the scoring area position.
|
||||
if (scoringArea) {
|
||||
xTarget += robot.wobbleScoreX;
|
||||
yTarget += robot.wobbleScoreY;
|
||||
}
|
||||
|
||||
if (faceAngle == 360) {
|
||||
faceAngle = robot.getRelativeAngle(180, robot.getAngleToPosition(xTarget,yTarget));
|
||||
}
|
||||
}
|
||||
|
||||
if (autoFace) {
|
||||
faceAngle = robot.getAngleToPosition(xTarget,yTarget);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.updateLocation();
|
||||
|
||||
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 {
|
||||
long currentTime = System.currentTimeMillis();
|
||||
if (!braking) {
|
||||
breakStartTime = currentTime;
|
||||
braking = true;
|
||||
}
|
||||
if (currentTime - breakStartTime >= brakeTime) {
|
||||
robot.setDrivePower(0,0,0,0);
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,46 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class DriveWithColorSensor extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName;
|
||||
private String actionName;
|
||||
private double power;
|
||||
private double minimum;
|
||||
private double maximum;
|
||||
|
||||
public DriveWithColorSensor(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
|
||||
minimum = robot.stateConfiguration.variable(groupName,actionName,"min").value();
|
||||
maximum = robot.stateConfiguration.variable(groupName,actionName,"max").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
double sensorValue = robot.wobbleColorSensor.getDistance(DistanceUnit.MM);
|
||||
if (sensorValue < minimum) {
|
||||
robot.getMecanumPowers(180,power,0);
|
||||
} else if (sensorValue > maximum) {
|
||||
robot.getMecanumPowers(0,power,0);
|
||||
} else {
|
||||
robot.setDrivePower(0,0,0,0);
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class Face extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName = "manual";
|
||||
private String actionName;
|
||||
private float faceAngle;
|
||||
private float toleranceFace;
|
||||
private double power;
|
||||
private boolean braking;
|
||||
private long breakStartTime;
|
||||
private long breakTime;
|
||||
private boolean autoFace;
|
||||
|
||||
public Face(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
public Face(Robot robot, float faceAngle, float toleranceFace, double power, long breakTime) {
|
||||
this.robot = robot;
|
||||
this.faceAngle = faceAngle;
|
||||
this.toleranceFace = toleranceFace;
|
||||
this.power = power;
|
||||
this.breakTime = breakTime;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
if (!groupName.equals("manual")) {
|
||||
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
|
||||
toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
|
||||
breakTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
|
||||
|
||||
try {
|
||||
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "faceAngle").value();
|
||||
} catch (RuntimeException e) {
|
||||
autoFace = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
|
||||
if (!groupName.equals("manual")) {
|
||||
setHasFinished(!robot.stateConfiguration.action(groupName, actionName).enabled);
|
||||
|
||||
|
||||
if (autoFace) {
|
||||
double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value());
|
||||
double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
|
||||
faceAngle = robot.getAngleToPosition(x, y);
|
||||
}
|
||||
|
||||
if (faceAngle == 360) {
|
||||
double x = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceX").value());
|
||||
double y = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "faceY").value());
|
||||
faceAngle = robot.getRelativeAngle(180,robot.getAngleToPosition(x, y));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.updateLocation();
|
||||
double[] powers = robot.getFacePowers(faceAngle,power);
|
||||
robot.setDrivePower(powers[0], powers[1],powers[0],powers[1]);
|
||||
if (Math.abs(robot.getRelativeAngle(faceAngle,robot.getRotation())) > toleranceFace) {
|
||||
braking = false;
|
||||
} else {
|
||||
long currentTime = System.currentTimeMillis();
|
||||
if (!braking) {
|
||||
breakStartTime = currentTime;
|
||||
braking = true;
|
||||
}
|
||||
if (currentTime - breakStartTime >= breakTime) {
|
||||
setHasFinished(true);
|
||||
robot.setDrivePower(0,0,0,0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("target angle", faceAngle);
|
||||
engine.telemetry.addData("current angle", robot.getRotation());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class FaceAndDrive extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName = "manual";
|
||||
private String actionName;
|
||||
private double xTarget;
|
||||
private double yTarget;
|
||||
private float faceAngle;
|
||||
private double tolerancePos;
|
||||
private double power;
|
||||
private long brakeTime;
|
||||
private float toleranceFace;
|
||||
|
||||
public FaceAndDrive(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
xTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "xPos").value());
|
||||
yTarget = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "yPos").value());
|
||||
power = robot.stateConfiguration.variable(groupName, actionName, "power").value();
|
||||
tolerancePos = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName, actionName, "tolPos").value());
|
||||
toleranceFace = robot.stateConfiguration.variable(groupName, actionName, "tolFace").value();
|
||||
brakeTime = robot.stateConfiguration.variable(groupName, actionName, "brakeMS").value();
|
||||
|
||||
try {
|
||||
faceAngle = robot.stateConfiguration.variable(groupName, actionName, "face").value();
|
||||
} catch (NullPointerException e) {
|
||||
faceAngle = robot.getAngleToPosition(xTarget,yTarget);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
addState(new Face(robot, faceAngle, toleranceFace, power, brakeTime));
|
||||
addState(new DriveToCoordinates(robot,xTarget,yTarget,faceAngle,tolerancePos,power,brakeTime));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
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;
|
||||
|
||||
public class FindWobbleGoal extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName;
|
||||
private String actionName;
|
||||
private double power;
|
||||
private double turnCheck;
|
||||
private double driveCheck;
|
||||
private double range;
|
||||
private boolean cCheckPrev;
|
||||
private boolean ccCheckPrev;
|
||||
private float startRotation;
|
||||
private boolean foundGoalRotation;
|
||||
private float wobbleGoalRotation;
|
||||
private long timeLimit;
|
||||
|
||||
public FindWobbleGoal(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
power = robot.stateConfiguration.variable(groupName,actionName,"power").value();
|
||||
turnCheck = robot.stateConfiguration.variable(groupName,actionName,"max").value();
|
||||
driveCheck = robot.stateConfiguration.variable(groupName,actionName,"min").value();
|
||||
range = robot.stateConfiguration.variable(groupName,actionName,"r").value();
|
||||
timeLimit = robot.stateConfiguration.variable(groupName,actionName,"limit").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
setHasFinished(!robot.stateConfiguration.action(groupName,actionName).enabled);
|
||||
startRotation = robot.getRotation();
|
||||
robot.setDrivePower(power,-power,power,-power);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
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());
|
||||
|
||||
boolean cCheck = rotation > range;
|
||||
boolean ccCheck = rotation < -range;
|
||||
|
||||
if (cCheck && !cCheckPrev) {
|
||||
robot.setDrivePower(-power, power, -power, power);
|
||||
}
|
||||
|
||||
if (ccCheck && !ccCheckPrev) {
|
||||
robot.setDrivePower(power, -power, power, -power);
|
||||
}
|
||||
cCheckPrev = cCheck;
|
||||
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);
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
private void endSearch() {
|
||||
robot.setDrivePower(0,0,0,0);
|
||||
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
|
||||
setHasFinished(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("sensor", robot.wobbleColorSensor.getDistance(DistanceUnit.MM));
|
||||
engine.telemetry.addData("red", robot.wobbleColorSensor.red());
|
||||
engine.telemetry.addData("green", robot.wobbleColorSensor.green());
|
||||
engine.telemetry.addData("blue", robot.wobbleColorSensor.blue());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class LaunchDriveControl extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private CyberarmState launchState;
|
||||
private CyberarmState driveState;
|
||||
private boolean checkConditionPrev;
|
||||
|
||||
public LaunchDriveControl(Robot robot, CyberarmState launchState, CyberarmState driveState) {
|
||||
this.robot = robot;
|
||||
this.launchState = launchState;
|
||||
this.driveState = driveState;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
addParallelState(launchState);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
boolean checkCondition = (robot.ringBeltStage > 3);
|
||||
if (checkCondition && !checkConditionPrev) {
|
||||
addParallelState(driveState);
|
||||
}
|
||||
checkConditionPrev = checkCondition;
|
||||
|
||||
setHasFinished(childrenHaveFinished());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("ring belt stage", robot.ringBeltStage);
|
||||
engine.telemetry.addData("check prev", checkConditionPrev);
|
||||
engine.telemetry.addData("drive prev", checkConditionPrev);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class Park extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName;
|
||||
private String actionName;
|
||||
double parkY;
|
||||
float parkFaceAngle;
|
||||
double parkTolerance;
|
||||
double parkPower;
|
||||
long parkBrakeTime;
|
||||
double parkDecelRange;
|
||||
double parkDecelMin;
|
||||
|
||||
public Park(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
parkY = robot.inchesToTicks((double) robot.stateConfiguration.variable(groupName,actionName,"yPos").value());
|
||||
parkFaceAngle = robot.stateConfiguration.variable(groupName,actionName,"face").value();
|
||||
parkTolerance = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto",actionName,"tolPos").value());
|
||||
parkPower = robot.stateConfiguration.variable(groupName,actionName,"power").value();
|
||||
parkBrakeTime = robot.stateConfiguration.variable(groupName,actionName,"brakeMS").value();
|
||||
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,parkDecelRange,parkDecelMin));
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
setHasFinished(childrenHaveFinished());
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
public class TensorFlowCheck extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String group;
|
||||
private String action;
|
||||
private List<Recognition> recognitions;
|
||||
private int path = 0;
|
||||
private long checkTime;
|
||||
public double wobblePosX;
|
||||
public double wobblePosY;
|
||||
private int manualPath;
|
||||
private String status;
|
||||
|
||||
public TensorFlowCheck(Robot robot, String group, String action) {
|
||||
this.robot = robot;
|
||||
this.group = group;
|
||||
this.action = action;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
checkTime = robot.stateConfiguration.variable(group,action,"time").value();
|
||||
manualPath = robot.stateConfiguration.variable(group,action,"path").value();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
if (!robot.stateConfiguration.action(group,action).enabled) {
|
||||
manualPath = -1;
|
||||
} else {
|
||||
robot.tfObjectDetector.activate();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
if (runTime() < checkTime) {
|
||||
if (manualPath != -1) {
|
||||
recognitions = robot.tfObjectDetector.getUpdatedRecognitions();
|
||||
|
||||
if (recognitions != null) {
|
||||
if (recognitions.size() == 1) {
|
||||
String label = recognitions.get(0).getLabel();
|
||||
if (label.equals("Single")) {
|
||||
path = 1;
|
||||
} else if (label.equals("Quad")) {
|
||||
path = 2;
|
||||
}
|
||||
} else if (recognitions.size() > 1) {
|
||||
path = 1;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
path = manualPath;
|
||||
}
|
||||
} else {
|
||||
robot.tfObjectDetector.deactivate();
|
||||
|
||||
if (path == 0) {
|
||||
robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","x").value());
|
||||
robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos0","y").value());
|
||||
}
|
||||
if (path == 1) {
|
||||
robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","x").value());
|
||||
robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos1","y").value());
|
||||
}
|
||||
if (path == 2) {
|
||||
robot.wobbleScoreX = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","x").value());
|
||||
robot.wobbleScoreY = robot.inchesToTicks((double) robot.stateConfiguration.variable("auto", "_goalPos2","y").value());
|
||||
}
|
||||
// make the servo look up once we're done using tensorFlow
|
||||
robot.webCamServo.setPosition(0);
|
||||
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
|
||||
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Chosen Path", path);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.Autonomous;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class ThreadStates extends CyberarmState {
|
||||
|
||||
private ArrayList<CyberarmState> states = new ArrayList<CyberarmState>();
|
||||
|
||||
public ThreadStates(ArrayList<CyberarmState> states) {
|
||||
this.states = states;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
for (CyberarmState state : states) {
|
||||
addParallelState(state);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
int finishedStates = 0;
|
||||
for (CyberarmState state : states) {
|
||||
if (state.getHasFinished()) {
|
||||
finishedStates += 1;
|
||||
}
|
||||
}
|
||||
setHasFinished(finishedStates == states.size());
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addLine("Threaded States");
|
||||
for (CyberarmState child: children) {
|
||||
engine.telemetry.addLine(""+child.getClass());
|
||||
if (child.getHasFinished()) {
|
||||
engine.telemetry.addLine("finished");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
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: Game",group = "demo")
|
||||
public class DemoEngine 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 DemoState(robot));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stop() {
|
||||
robot.deactivateVuforia();
|
||||
robot.saveRecording();
|
||||
super.stop();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
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;
|
||||
|
||||
@Disabled
|
||||
@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();
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
package org.timecrafters.UltimateGoal.Competition;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
|
||||
|
||||
public class DriveMotor {
|
||||
|
||||
public DcMotor motor;
|
||||
private int brakePosition;
|
||||
private MotorConfigurationType motorType;
|
||||
private DcMotorSimple.Direction direction;
|
||||
static final double FINE_CORRECTION = 0.05;
|
||||
static final double LARGE_CORRECTION = 0.03;
|
||||
|
||||
public DriveMotor(DcMotor motor, MotorConfigurationType motorType, DcMotorSimple.Direction direction) {
|
||||
this.motor = motor;
|
||||
this.motorType = motorType;
|
||||
this.direction = direction;
|
||||
}
|
||||
|
||||
public void init() {
|
||||
// motor.setMotorType(motorType);
|
||||
// motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
// motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
motor.setDirection(direction);
|
||||
}
|
||||
|
||||
public void setPower(double power) {
|
||||
motor.setPower(power);
|
||||
}
|
||||
|
||||
public void setBrakePosition() {
|
||||
brakePosition = motor.getCurrentPosition();
|
||||
}
|
||||
|
||||
public void brake() {
|
||||
double currentPosition = motor.getCurrentPosition();
|
||||
double relativePosition = brakePosition - currentPosition;
|
||||
double breakPower = Math.pow(LARGE_CORRECTION * relativePosition, 3) + FINE_CORRECTION * relativePosition;
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
package org.timecrafters.UltimateGoal.Competition;
|
||||
|
||||
/*
|
||||
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;
|
||||
|
||||
public class Launch extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName;
|
||||
private String actionName;
|
||||
boolean hasCycled;
|
||||
boolean detectedPass;
|
||||
private boolean reduceConditionPrev;
|
||||
private double reducePos;
|
||||
private long stuckStartTime;
|
||||
|
||||
public Launch(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
public Launch(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
try {
|
||||
if (robot.stateConfiguration.action(groupName, actionName).enabled) {
|
||||
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
|
||||
|
||||
} else {
|
||||
setHasFinished(true);
|
||||
}
|
||||
} catch (NullPointerException e){
|
||||
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
//jam counter measures
|
||||
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;
|
||||
}
|
||||
|
||||
//detect when limit switch is initially triggered
|
||||
boolean detectingPass = robot.magnetSensor.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.
|
||||
|
||||
if (hasCycled) {
|
||||
robot.ringBeltStage = 0;
|
||||
robot.ringBeltMotor.setTargetPosition(beltPos);
|
||||
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
if (!robot.initLauncher) {
|
||||
robot.launchMotor.setPower(0);
|
||||
}
|
||||
|
||||
setHasFinished(true);
|
||||
} else {
|
||||
hasCycled = true;
|
||||
reducePos = beltPos + (robot.reduceLaunchPos);
|
||||
}
|
||||
}
|
||||
detectedPass = detectingPass;
|
||||
|
||||
boolean reduceCondition = (hasCycled && 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.
|
||||
|
||||
robot.ringBeltStage += 1;
|
||||
}
|
||||
reduceConditionPrev = reduceCondition;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("hasCycled", hasCycled);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
package org.timecrafters.UltimateGoal.Competition;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class Pause extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName;
|
||||
private String actionName;
|
||||
private long waitTime = -1;
|
||||
private boolean enabled;
|
||||
|
||||
public Pause(Robot robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
}
|
||||
|
||||
public Pause(Robot robot, long waitTime) {
|
||||
this.robot = robot;
|
||||
this.waitTime = waitTime;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
if (waitTime == -1) {
|
||||
enabled = robot.stateConfiguration.action(groupName, actionName).enabled;
|
||||
waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
if (!enabled) {
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
setHasFinished(runTime() > waitTime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("runTime", runTime());
|
||||
engine.telemetry.addData("wait", waitTime);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.PreInit;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class FindLimitSwitch extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
|
||||
public FindLimitSwitch(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
robot.ringBeltMotor.setPower(Robot.RING_BELT_SLOW_POWER);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
if (robot.magnetSensor.isPressed()) {
|
||||
robot.ringBeltMotor.setPower(0);
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.PreInit;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class LoadRings extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private ProgressRingBelt ringBeltState;
|
||||
private int ringCount = 0;
|
||||
|
||||
public LoadRings(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
robot.collectionMotor.setPower(1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
ringBeltState = new ProgressRingBelt(robot);
|
||||
if (engine.gamepad1.x && childrenHaveFinished()) {
|
||||
addParallelState(ringBeltState);
|
||||
|
||||
}
|
||||
|
||||
if (robot.ringBeltStage > 2) {
|
||||
robot.launchMotor.setPower(0);
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
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 {
|
||||
|
||||
private Robot robot;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
robot = new Robot(hardwareMap);
|
||||
robot.initHardware();
|
||||
super.init();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
addState(new FindLimitSwitch(robot));
|
||||
addState(new LoadRings(robot));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
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 {
|
||||
|
||||
private Robot robot;
|
||||
private int targetPos;
|
||||
private boolean prepLaunch;
|
||||
private long stuckStartTime;
|
||||
|
||||
public ProgressRingBelt(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
private void prep(){
|
||||
robot.ringBeltMotor.setTargetPosition(targetPos);
|
||||
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
robot.ringBeltMotor.setPower(Robot.RING_BELT_NORMAL_POWER);
|
||||
robot.ringBeltStage += 1;
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
//finished state when the target position is reached
|
||||
int currentPos = robot.ringBeltMotor.getCurrentPosition();
|
||||
if (currentPos >= targetPos) {
|
||||
if(prepLaunch) {
|
||||
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
|
||||
}
|
||||
|
||||
setHasFinished(true);
|
||||
}
|
||||
|
||||
//belt jam mitigation
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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.magnetSensor.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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,757 @@
|
||||
package org.timecrafters.UltimateGoal.Competition;
|
||||
|
||||
/*
|
||||
The robot object contains all the hardware and functions that are used in both teleOp and
|
||||
Autonomous. This includes drive functions, localization functions, shared constants, and a few
|
||||
general calculations and debugging tools.
|
||||
*/
|
||||
|
||||
import android.os.Environment;
|
||||
import android.util.Log;
|
||||
|
||||
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.library.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 Robot {
|
||||
|
||||
private HardwareMap hardwareMap;
|
||||
|
||||
public Robot(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
|
||||
|
||||
//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;
|
||||
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;
|
||||
|
||||
//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 magnetSensor;
|
||||
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 forwardVector;
|
||||
public double sidewaysVector;
|
||||
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);
|
||||
|
||||
wobbleUpPos = stateConfiguration.variable(
|
||||
"system","arm", "up").value();
|
||||
wobbleDownPos = stateConfiguration.variable(
|
||||
"system","arm", "down").value();
|
||||
wobbleDropPos = stateConfiguration.variable(
|
||||
"system","arm", "drop").value();
|
||||
|
||||
wobbleGrabServo = hardwareMap.servo.get("wobbleGrab");
|
||||
|
||||
wobbleColorSensor = hardwareMap.get(RevColorSensorV3.class, "color");
|
||||
wobbleTouchSensor = hardwareMap.get(RevTouchSensor.class, "touch");
|
||||
|
||||
|
||||
//init 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);
|
||||
|
||||
magnetSensor = 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");
|
||||
|
||||
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 Localization
|
||||
webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
|
||||
initVuforia();
|
||||
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();
|
||||
|
||||
//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);
|
||||
|
||||
//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;
|
||||
long launchAccelStart = System.currentTimeMillis();
|
||||
while (launcherPower < LAUNCH_POWER) {
|
||||
launcherPower = (double) ((System.currentTimeMillis() - launchAccelStart) / LAUNCH_ACCEL_TIME) * LAUNCH_POWER;
|
||||
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();
|
||||
|
||||
initTensorFlow();
|
||||
|
||||
ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds");
|
||||
}
|
||||
|
||||
private void initVuforia() {
|
||||
|
||||
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;
|
||||
parameters.useExtendedTracking = false;
|
||||
|
||||
|
||||
vuforia = ClassFactory.getInstance().createVuforia(parameters);
|
||||
|
||||
targetsUltimateGoal = this.vuforia.loadTrackablesFromAsset("UltimateGoal");
|
||||
VuforiaTrackable redTowerGoalTarget = targetsUltimateGoal.get(1);
|
||||
redTowerGoalTarget.setName("Red Tower Goal Target");
|
||||
VuforiaTrackable redAllianceTarget = targetsUltimateGoal.get(2);
|
||||
redAllianceTarget.setName("Red Alliance Target");
|
||||
VuforiaTrackable frontWallTarget = targetsUltimateGoal.get(4);
|
||||
frontWallTarget.setName("Front Wall Target");
|
||||
|
||||
trackables.addAll(targetsUltimateGoal);
|
||||
|
||||
final float mmTargetHeight = (6) * mmPerInch;
|
||||
|
||||
final float halfField = 72 * mmPerInch;
|
||||
final float quadField = 36 * mmPerInch;
|
||||
|
||||
redAllianceTarget.setLocation(OpenGLMatrix
|
||||
.translation(0, -halfField, mmTargetHeight)
|
||||
.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)));
|
||||
redTowerGoalTarget.setLocation(OpenGLMatrix
|
||||
.translation(halfField, -quadField, mmTargetHeight)
|
||||
.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));
|
||||
|
||||
for (VuforiaTrackable trackable : trackables) {
|
||||
((VuforiaTrackableDefaultListener) trackable.getListener())
|
||||
.setPhoneInformation(robotFromCamera, parameters.cameraDirection);
|
||||
}
|
||||
|
||||
targetsUltimateGoal.activate();
|
||||
}
|
||||
|
||||
public void deactivateVuforia() {
|
||||
targetsUltimateGoal.deactivate();
|
||||
}
|
||||
|
||||
private void initTensorFlow() {
|
||||
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();
|
||||
tfObjectDetector = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
|
||||
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(){
|
||||
|
||||
// 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);
|
||||
|
||||
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 xChange = magnitude * (Math.sin(direction));
|
||||
double yChange = magnitude * (Math.cos(direction));
|
||||
|
||||
locationX += xChange;
|
||||
locationY += yChange;
|
||||
|
||||
Robot.rotation += rotationChange;
|
||||
Robot.rotation = scaleAngleRange(Robot.rotation);
|
||||
|
||||
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();
|
||||
|
||||
if (timeStartZeroVelocity == 0) {
|
||||
timeStartZeroVelocity = timeCurrent;
|
||||
} else if (timeCurrent - timeStartZeroVelocity >= minCheckDurationMs ) {
|
||||
syncWithVuforia();
|
||||
}
|
||||
|
||||
} else {
|
||||
timeStartZeroVelocity = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public void syncWithVuforia() {
|
||||
trackableVisible = false;
|
||||
for (VuforiaTrackable trackable : trackables) {
|
||||
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
|
||||
OpenGLMatrix robotLocation =
|
||||
((VuforiaTrackableDefaultListener)trackable.getListener())
|
||||
.getUpdatedRobotLocation();
|
||||
|
||||
//this is used for debugging purposes.
|
||||
trackableVisible = true;
|
||||
|
||||
if (robotLocation != null) {
|
||||
lastConfirmendLocation = robotLocation;
|
||||
}
|
||||
|
||||
//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);
|
||||
float vuforiaRotation = 90-orientation.thirdAngle;
|
||||
|
||||
if (vuforiaRotation > 180) {
|
||||
vuforiaRotation -= -180;
|
||||
}
|
||||
|
||||
if (Math.abs(rotation - vuforiaRotation) < vuforiaRotationCull) {
|
||||
rotation = vuforiaRotation;
|
||||
|
||||
VectorF translation = lastConfirmendLocation.getTranslation();
|
||||
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);
|
||||
|
||||
locationX = inchesToTicks(camX - displaceX);
|
||||
locationY = inchesToTicks(camY - displaceY);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public float getRotation() {
|
||||
return Robot.rotation;
|
||||
}
|
||||
|
||||
public double getLocationX() {
|
||||
return Robot.locationX;
|
||||
}
|
||||
|
||||
public double getLocationY() {
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
//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);
|
||||
}
|
||||
|
||||
//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, Robot.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, Robot.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;
|
||||
}
|
||||
|
||||
//Used to for automated jam countermeasures
|
||||
public boolean beltIsStuck() {
|
||||
int ringBeltPos = ringBeltMotor.getCurrentPosition();
|
||||
boolean notMoved = (ringBeltPos - ringBeltPrev <= beltMaxStopTicks);
|
||||
ringBeltPrev = ringBeltPos;
|
||||
return notMoved;
|
||||
}
|
||||
|
||||
public float scaleAngleRange(float angle) {
|
||||
if (angle < -180) {
|
||||
angle += 360;
|
||||
}
|
||||
if (angle > 180) {
|
||||
angle -= 360;
|
||||
}
|
||||
return angle;
|
||||
}
|
||||
|
||||
//Debugging tools
|
||||
public void record(String record) {
|
||||
TestingRecord+="\n"+record;
|
||||
}
|
||||
|
||||
public void saveRecording() {
|
||||
writeToFile(Environment.getExternalStorageDirectory().getAbsolutePath()+File.separator+"TimeCrafters_TestingRecord"+File.separator+"RobotTestingRecord.txt", TestingRecord);
|
||||
}
|
||||
|
||||
public boolean writeToFile(String filePath, String content) {
|
||||
try {
|
||||
|
||||
FileWriter writer = new FileWriter(filePath);
|
||||
writer.write(content);
|
||||
writer.close();
|
||||
|
||||
return true;
|
||||
|
||||
} catch (IOException e) {
|
||||
Log.e("RecordTest", e.getLocalizedMessage());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
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() {
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,260 @@
|
||||
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;
|
||||
import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
|
||||
import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class Player1 extends CyberarmState {
|
||||
private Robot robot;
|
||||
|
||||
//normal drive control
|
||||
private float leftJoystickDegrees;
|
||||
private double leftJoystickMagnitude;
|
||||
private float rightJoystickDegrees;
|
||||
private double rightJoystickMagnitude;
|
||||
private double rightJoystickMagnitudePrevious;
|
||||
|
||||
private double faceControlThreshold;
|
||||
private float cardinalSnapping;
|
||||
private float pairSnapping;
|
||||
|
||||
private float faceDirection = 0;
|
||||
private double[] powers = {0,0,0,0};
|
||||
private double drivePower = 1;
|
||||
private boolean lbPrev;
|
||||
|
||||
//find wobble goal control
|
||||
private FindWobbleGoal findWobbleGoal;
|
||||
private boolean runNextFindWobble;
|
||||
private boolean findWobbleInputPrev;
|
||||
private boolean aPrev;
|
||||
|
||||
//Drive to launch control
|
||||
private powerShotsControl powerShots;
|
||||
private boolean runNextDriveToLaunch;
|
||||
private boolean driveToLaunchInputPrev;
|
||||
|
||||
private double endGameX;
|
||||
private double endGameY;
|
||||
private float endGameRot;
|
||||
private int loopCount;
|
||||
|
||||
private double refinePower;
|
||||
|
||||
public Player1(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();
|
||||
|
||||
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
|
||||
public void start() {
|
||||
faceDirection = robot.getRotation();
|
||||
}
|
||||
|
||||
@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) {
|
||||
drivePower = 0.5;
|
||||
} else {
|
||||
drivePower = 1;
|
||||
}
|
||||
}
|
||||
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;
|
||||
if (findWobbleInput) {
|
||||
//if the claw is open, run FindWobbleGoal
|
||||
if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
|
||||
|
||||
faceDirection = robot.getRotation();
|
||||
|
||||
if (runNextFindWobble && !findWobbleInputPrev) {
|
||||
findWobbleGoal =
|
||||
new FindWobbleGoal(robot, "auto", "08_0");
|
||||
addParallelState(findWobbleGoal);
|
||||
}
|
||||
//if the claw is closed, open the claw.
|
||||
} else if (!findWobbleInputPrev) {
|
||||
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
|
||||
}
|
||||
//if the button is released cancel the search
|
||||
} else if (!runNextFindWobble) {
|
||||
findWobbleGoal.setHasFinished(true);
|
||||
}
|
||||
findWobbleInputPrev = findWobbleInput;
|
||||
|
||||
//toggles wobble grabber open and closed
|
||||
boolean a = engine.gamepad1.a;
|
||||
if (a && !aPrev) {
|
||||
if (robot.wobbleGrabServo.getPosition() == Robot.WOBBLE_SERVO_OPEN) {
|
||||
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
|
||||
} else {
|
||||
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
|
||||
}
|
||||
}
|
||||
aPrev = a;
|
||||
|
||||
//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;
|
||||
if (driveToLaunchInput) {
|
||||
if (runNextDriveToLaunch && !driveToLaunchInputPrev) {
|
||||
powerShots = new powerShotsControl(robot);
|
||||
addParallelState(powerShots);
|
||||
}
|
||||
faceDirection = robot.getRotation();
|
||||
} else if (!runNextDriveToLaunch) {
|
||||
powerShots.setHasFinished(true);
|
||||
}
|
||||
driveToLaunchInputPrev = driveToLaunchInput;
|
||||
|
||||
//Normal Driver Controls
|
||||
if (childrenHaveFinished()) {
|
||||
|
||||
|
||||
double leftJoystickX = engine.gamepad1.left_stick_x;
|
||||
double leftJoystickY = engine.gamepad1.left_stick_y;
|
||||
|
||||
leftJoystickDegrees = robot.getRelativeAngle(90,
|
||||
(float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY)));
|
||||
leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
|
||||
|
||||
double rightJoystickX = engine.gamepad1.right_stick_x;
|
||||
double rightJoystickY = engine.gamepad1.right_stick_y;
|
||||
|
||||
rightJoystickDegrees = robot.getRelativeAngle(90,
|
||||
(float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY)));
|
||||
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
|
||||
|
||||
//allows the the driver to indicate which direction the robot is currently looking
|
||||
//so that the controller and robot can be re-synced in the event of a bad initial
|
||||
//rotation.
|
||||
if (engine.gamepad1.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 (engine.gamepad1.dpad_right) {
|
||||
powers = new double[]{refinePower, -refinePower, refinePower, -refinePower};
|
||||
faceDirection = robot.getRotation();
|
||||
} else if (engine.gamepad1.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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,189 @@
|
||||
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;
|
||||
|
||||
public class Player2 extends CyberarmState {
|
||||
private Robot robot;
|
||||
|
||||
private boolean rbPrev;
|
||||
private boolean yPrev;
|
||||
private boolean xPrev;
|
||||
private boolean bPrev;
|
||||
private boolean wobbleArmUp = false;
|
||||
private boolean wobbleGrabOpen = false;
|
||||
private boolean aPrev;
|
||||
private double beltPowerPrev;
|
||||
private DcMotor.RunMode runModePrev;
|
||||
private boolean lbPrev;
|
||||
private boolean manualArmHold;
|
||||
private int loops = 0;
|
||||
|
||||
|
||||
|
||||
private boolean launchInput = false;
|
||||
|
||||
public Player2(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
robot.wobbleArmMotor.setTargetPosition(0);
|
||||
robot.wobbleArmMotor.setPower(0.5);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
// 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) {
|
||||
robot.collectionMotor.setPower(-lt);
|
||||
} else if (childrenHaveFinished()) {
|
||||
robot.collectionMotor.setPower(rt);
|
||||
} else {
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
//manually control the wobble arm for when it's initialized in an unexpected position.
|
||||
double leftStickY = engine.gamepad2.left_stick_y;
|
||||
|
||||
if (engine.gamepad2.dpad_right) {
|
||||
if (!robot.wobbleTouchSensor.isPressed()) {
|
||||
setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
robot.wobbleArmMotor.setPower(-0.2);
|
||||
robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
|
||||
} else {
|
||||
setArmMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.wobbleArmMotor.setTargetPosition(0);
|
||||
setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
}
|
||||
} else if (leftStickY != 0 ) {
|
||||
setArmMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
robot.wobbleArmMotor.setPower(0.15 * leftStickY);
|
||||
robot.wobbleArmMotor.setTargetPosition(robot.wobbleArmMotor.getCurrentPosition());
|
||||
} else {
|
||||
setArmMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
robot.wobbleArmMotor.setPower(0.5);
|
||||
if (engine.gamepad2.dpad_up) {
|
||||
robot.wobbleArmMotor.setTargetPosition(robot.wobbleUpPos);
|
||||
} else if (engine.gamepad2.dpad_down) {
|
||||
robot.wobbleArmMotor.setTargetPosition(robot.wobbleDownPos);
|
||||
} else if (engine.gamepad2.dpad_left) {
|
||||
robot.wobbleArmMotor.setTargetPosition(robot.wobbleDropPos );
|
||||
}
|
||||
}
|
||||
|
||||
//manually toggle the launch wheel for emergencies
|
||||
boolean a = engine.gamepad2.a;
|
||||
if (a && !aPrev) {
|
||||
if (robot.launchMotor.getPower() == 0) {
|
||||
robot.launchMotor.setPower(Robot.LAUNCH_POWER);
|
||||
} else {
|
||||
robot.launchMotor.setPower(0);
|
||||
}
|
||||
}
|
||||
aPrev = a;
|
||||
|
||||
|
||||
|
||||
|
||||
//allows the driver to revers the belt in the event of a jam
|
||||
boolean lb = engine.gamepad2.left_bumper;
|
||||
if (lb && !lbPrev) {
|
||||
runModePrev = robot.ringBeltMotor.getMode();
|
||||
beltPowerPrev = robot.ringBeltMotor.getPower();
|
||||
robot.ringBeltMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
|
||||
}
|
||||
|
||||
if (!lb && lbPrev) {
|
||||
robot.ringBeltMotor.setPower(beltPowerPrev);
|
||||
robot.ringBeltMotor.setMode(runModePrev);
|
||||
}
|
||||
lbPrev = lb;
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
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 {
|
||||
|
||||
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 TeleOpState(robot));
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stop() {
|
||||
robot.deactivateVuforia();
|
||||
robot.saveRecording();
|
||||
super.stop();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
package org.timecrafters.UltimateGoal.Competition.TeleOp;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.UltimateGoal.Competition.Autonomous.DriveToCoordinates;
|
||||
import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
|
||||
import org.timecrafters.UltimateGoal.Competition.Launch;
|
||||
import org.timecrafters.UltimateGoal.Competition.ProgressRingBelt;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
public class TeleOpState extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
|
||||
public TeleOpState(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
addParallelState(new Player1(robot));
|
||||
addParallelState(new Player2(robot));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
// setHasFinished(childrenHaveFinished());
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
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;
|
||||
|
||||
public class powerShotsControl extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
|
||||
private double endGameX;
|
||||
private double endGameY;
|
||||
private float endGameRot;
|
||||
|
||||
private double endGamePower;
|
||||
|
||||
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();
|
||||
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));
|
||||
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(endGamePower);
|
||||
}
|
||||
|
||||
@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());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
package org.timecrafters.UltimateGoal.Competition;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class UnstickRingBelt extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private int targetPos;
|
||||
private double lastPower;
|
||||
private DcMotor.RunMode lastRunMode;
|
||||
|
||||
public UnstickRingBelt(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
lastPower = robot.ringBeltMotor.getPower();
|
||||
lastRunMode = robot.ringBeltMotor.getMode();
|
||||
int currentPos = robot.ringBeltMotor.getCurrentPosition();
|
||||
targetPos = currentPos - robot.beltReverseTicks;
|
||||
robot.ringBeltMotor.setPower(-Robot.RING_BELT_SLOW_POWER);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
int currentPos = robot.ringBeltMotor.getCurrentPosition();
|
||||
|
||||
if (currentPos < targetPos) {
|
||||
robot.ringBeltMotor.setPower(lastPower);
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
package org.timecrafters.UltimateGoal.Competition;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class WobbleArm extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName;
|
||||
private String actionName;
|
||||
private int suggestedPos;
|
||||
private int targetPos = -1;
|
||||
private long waitTime = -1;
|
||||
|
||||
public WobbleArm(Robot robot, String groupName, String actionName, int targetPos) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
this.suggestedPos = targetPos;
|
||||
}
|
||||
|
||||
public WobbleArm(Robot robot, int targetPos, long waitTime) {
|
||||
this.robot = robot;
|
||||
this.targetPos = targetPos;
|
||||
this.waitTime = waitTime;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
if (waitTime == -1) {
|
||||
waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
|
||||
targetPos = robot.stateConfiguration.variable(groupName,actionName,"armPos").value();
|
||||
if (targetPos == -1) {
|
||||
targetPos = suggestedPos;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
robot.wobbleArmMotor.setPower(0.5);
|
||||
robot.wobbleArmMotor.setTargetPosition(targetPos);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
setHasFinished(runTime() > waitTime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("runTime", runTime());
|
||||
engine.telemetry.addData("target", targetPos);
|
||||
engine.telemetry.addData("wait", waitTime);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
package org.timecrafters.UltimateGoal.Competition;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class WobbleGrab extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private String groupName;
|
||||
private String actionName;
|
||||
private boolean open;
|
||||
private long waitTime = -1;
|
||||
private boolean enabled;
|
||||
|
||||
public WobbleGrab(Robot robot, String groupName, String actionName, boolean open) {
|
||||
this.robot = robot;
|
||||
this.groupName = groupName;
|
||||
this.actionName = actionName;
|
||||
this.open = open;
|
||||
}
|
||||
|
||||
public WobbleGrab(Robot robot, boolean open, long waitTime) {
|
||||
this.robot = robot;
|
||||
this.open = open;
|
||||
this.waitTime = waitTime;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
if (waitTime == -1) {
|
||||
enabled = robot.stateConfiguration.action(groupName, actionName).enabled;
|
||||
waitTime = robot.stateConfiguration.variable(groupName, actionName, "wait").value();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
if (enabled) {
|
||||
if (open) {
|
||||
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_OPEN);
|
||||
} else {
|
||||
robot.wobbleGrabServo.setPosition(Robot.WOBBLE_SERVO_CLOSED);
|
||||
}
|
||||
} else {
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
setHasFinished(runTime() > waitTime);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("runTime", runTime());
|
||||
engine.telemetry.addData("wait", waitTime);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
|
||||
public class ControlHubTest extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
|
||||
private Acceleration acceleration;
|
||||
private double Vx = 0;
|
||||
private double Vy = 0;
|
||||
private double Vz = 0;
|
||||
private double aVx = 0;
|
||||
private double aVy = 0;
|
||||
private double aVz = 0;
|
||||
|
||||
|
||||
public ControlHubTest(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
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;
|
||||
|
||||
robot.imu.startAccelerationIntegration(startPosition,startVelocity, 200);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
acceleration = robot.imu.getAcceleration();
|
||||
|
||||
Vx = robot.imu.getVelocity().xVeloc;
|
||||
Vy = robot.imu.getVelocity().yVeloc;
|
||||
Vz = robot.imu.getVelocity().zVeloc;
|
||||
|
||||
aVx = robot.imu.getAngularVelocity().xRotationRate;
|
||||
aVy = robot.imu.getAngularVelocity().yRotationRate;
|
||||
aVz = robot.imu.getAngularVelocity().zRotationRate;
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addLine("Acceleration");
|
||||
engine.telemetry.addData("x", acceleration.xAccel);
|
||||
engine.telemetry.addData("y", acceleration.yAccel);
|
||||
engine.telemetry.addData("z", acceleration.zAccel);
|
||||
|
||||
engine.telemetry.addLine("Velocity");
|
||||
engine.telemetry.addData("X", Vx);
|
||||
engine.telemetry.addData("Y", Vy);
|
||||
engine.telemetry.addData("Z", Vz);
|
||||
|
||||
engine.telemetry.addLine("Angular Velocity");
|
||||
engine.telemetry.addData("X", aVx);
|
||||
engine.telemetry.addData("Y", aVy);
|
||||
engine.telemetry.addData("Z", aVz);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.ColorRangeSensor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
|
||||
public class DistanceSensorTest extends CyberarmState {
|
||||
|
||||
|
||||
private Rev2mDistanceSensor distanceSensor;
|
||||
// private RevColorSensorV3 colorSensor;
|
||||
private ColorRangeSensor colorSensor;
|
||||
private double distance = 0;
|
||||
private double color = 0;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
distanceSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "distance");
|
||||
colorSensor = engine.hardwareMap.get(ColorRangeSensor.class, "color");
|
||||
// colorSensor = engine.hardwareMap.get(RevColorSensorV3.class, "color");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
distance = distanceSensor.getDistance(DistanceUnit.MM);
|
||||
color = colorSensor.getDistance(DistanceUnit.MM);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("distance", distance);
|
||||
engine.telemetry.addData("color", color);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
|
||||
public class LEDTest extends CyberarmState {
|
||||
|
||||
RevBlinkinLedDriver leds;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
leds = engine.hardwareMap.get(RevBlinkinLedDriver.class, "leds");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
setHasFinished(false);
|
||||
if (engine.gamepad1.x) {
|
||||
leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED);
|
||||
} else {
|
||||
leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN );
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,137 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
|
||||
public class MecanumFunctionTest extends CyberarmState {
|
||||
|
||||
private Robot robot;
|
||||
private float leftJoystickDegrees;
|
||||
private double leftJoystickMagnitude;
|
||||
private float rightJoystickDegrees;
|
||||
private double rightJoystickMagnitude;
|
||||
private double powerFrontLeft=0;
|
||||
private double powerFrontRight=0;
|
||||
private double powerBackLeft=0;
|
||||
private double powerBackRight=0;
|
||||
|
||||
private double aVx = 0;
|
||||
private double aVy = 0;
|
||||
private double aVz = 0;
|
||||
|
||||
private int powerStep = 5;
|
||||
private double POWER_SCALE = 0.5 ;
|
||||
private boolean toggleSpeedInput = false;
|
||||
|
||||
public MecanumFunctionTest(Robot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
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;
|
||||
|
||||
robot.imu.startAccelerationIntegration(startPosition,startVelocity, 10);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
robot.updateLocation();
|
||||
// robot.record();/////////////
|
||||
|
||||
AngularVelocity angularVelocity = robot.imu.getAngularVelocity();
|
||||
|
||||
aVx = angularVelocity.xRotationRate;
|
||||
aVy = angularVelocity.yRotationRate;
|
||||
aVz = angularVelocity.zRotationRate;
|
||||
|
||||
double leftJoystickX = engine.gamepad1.left_stick_x;
|
||||
double leftJoystickY = engine.gamepad1.left_stick_y;
|
||||
|
||||
leftJoystickDegrees = (float) Math.toDegrees(Math.atan2(leftJoystickX, -leftJoystickY));
|
||||
leftJoystickMagnitude = Math.hypot(leftJoystickX, leftJoystickY);
|
||||
|
||||
double rightJoystickX = engine.gamepad1.right_stick_x;
|
||||
double rightJoystickY = engine.gamepad1.right_stick_y;
|
||||
|
||||
rightJoystickDegrees = (float) Math.toDegrees(Math.atan2(rightJoystickX, -rightJoystickY));
|
||||
rightJoystickMagnitude = Math.hypot(rightJoystickX, rightJoystickY);
|
||||
|
||||
powerFrontLeft = 0;
|
||||
powerFrontRight = 0;
|
||||
powerBackLeft = 0;
|
||||
powerBackRight = 0;
|
||||
|
||||
boolean a = engine.gamepad1.a;
|
||||
boolean b = engine.gamepad1.b;
|
||||
|
||||
if (a && !toggleSpeedInput && POWER_SCALE < 1) {
|
||||
powerStep += 1;
|
||||
POWER_SCALE = powerStep * 0.1;
|
||||
}
|
||||
|
||||
if (b && !toggleSpeedInput && POWER_SCALE > 0.1) {
|
||||
powerStep -= 1;
|
||||
POWER_SCALE = powerStep * 0.1;
|
||||
}
|
||||
|
||||
toggleSpeedInput = a || b;
|
||||
|
||||
if (rightJoystickMagnitude == 0) {
|
||||
if (leftJoystickMagnitude !=0) {
|
||||
double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, leftJoystickDegrees);
|
||||
powerFrontLeft = powers[0];
|
||||
powerFrontRight = powers[1];
|
||||
powerBackLeft = powers[2];
|
||||
powerBackRight = powers[3];
|
||||
|
||||
}
|
||||
} else {
|
||||
if (leftJoystickMagnitude == 0) {
|
||||
double[] powers = robot.getFacePowers(rightJoystickDegrees, POWER_SCALE * rightJoystickMagnitude);
|
||||
powerFrontLeft = powers[0];
|
||||
powerFrontRight = powers[1];
|
||||
powerBackLeft = powers[0];
|
||||
powerBackRight = powers[1];
|
||||
} else {
|
||||
double[] powers = robot.getMecanumPowers(leftJoystickDegrees, POWER_SCALE * leftJoystickMagnitude, rightJoystickDegrees);
|
||||
powerFrontLeft = powers[0];
|
||||
powerFrontRight = powers[1];
|
||||
powerBackLeft = powers[2];
|
||||
powerBackRight = powers[3];
|
||||
}
|
||||
}
|
||||
|
||||
robot.setDrivePower(powerFrontLeft,powerFrontRight,powerBackLeft,powerBackRight);
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Scale", POWER_SCALE);
|
||||
|
||||
engine.telemetry.addLine("Angular Velocity");
|
||||
engine.telemetry.addData("X", aVx);
|
||||
engine.telemetry.addData("Y", aVy);
|
||||
engine.telemetry.addData("Z", aVz);
|
||||
|
||||
engine.telemetry.addLine("Powers");
|
||||
engine.telemetry.addData("FL", robot.driveFrontLeft.getPower());
|
||||
engine.telemetry.addData("FR", robot.driveFrontRight.getPower());
|
||||
engine.telemetry.addData("BL", robot.driveBackLeft.getPower());
|
||||
engine.telemetry.addData("BR", robot.driveBackRight.getPower());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
|
||||
public class ServoPosTest extends CyberarmState {
|
||||
|
||||
private Servo servo;
|
||||
private double servoPos;
|
||||
|
||||
public ServoPosTest() {
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
servo = engine.hardwareMap.servo.get("look");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
servoPos = engine.gamepad1.right_stick_y;
|
||||
servo.setPosition(servoPos);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Position", servoPos);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.UltimateGoal.Competition.Autonomous.FindWobbleGoal;
|
||||
import org.timecrafters.UltimateGoal.Competition.Robot;
|
||||
|
||||
@TeleOp (name = "Hardware test", group = "test")
|
||||
public class TestingEngine extends CyberarmEngine {
|
||||
|
||||
private Robot robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
addState(new ServoPosTest());
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
package org.timecrafters.UltimateGoal.HardwareTesting;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
|
||||
public class WobbleArmTest extends CyberarmState {
|
||||
|
||||
private int currentPos;
|
||||
private DcMotor motor;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
motor = engine.hardwareMap.dcMotor.get("wobbleArm");
|
||||
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
currentPos = motor.getCurrentPosition();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("Position", currentPos);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user