auto> robots = reflections.getSubTypesOf(Alloy.class);
+ for (Class robot : robots) {
+ try {
+ if (!Modifier.isAbstract(robot.getMethod("robotSetup").getModifiers())
+ || robot.getMethod("robotSetup") == null) {
+ robot.getMethod("robotSetup").invoke(robot.newInstance());
+ }
+ } catch (NoSuchMethodException
+ | IllegalAccessException
+ | InstantiationException
+ | InvocationTargetException e) {
+ e.printStackTrace();
+ }
+ }
+
+ new RobotCore(telemetry, hardwareMap, gamepad1, gamepad2);
+ setup();
}
+ /**
+ * The user loop can be overridden in the auto mode and is called every loop before the state is
+ * run. This can be useful for updating information used in states.
+ */
+ public void userLoop() {}
+
+ /** the Loop method takes care of running the state machine */
@Override
public void loop() {
- // This code will be run once a loop while the auto mode is running
+ userLoop();
+ if (running) {
+ auto.run(); // Run the state machine, which will in turn run the states
+ }
+ if (auto.isDone()) { // check if the state machine has finished (Last state achieved)
+ running = false; // stop the state machine
+ auto.stop(); // Finally stop the state machine
+ }
+ }
+
+ /**
+ * A wrapper around the add state method to allow the user to add states to the auto mode
+ *
+ * @param state the state to be added
+ */
+ public void newState(State state) {
+ auto.addState(state);
}
}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/README.md b/src/main/java/org/montclairrobotics/alloy/auto/README.md
index 91d5243..c6c4e6c 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/README.md
+++ b/src/main/java/org/montclairrobotics/alloy/auto/README.md
@@ -17,3 +17,6 @@ when they have finished running
3. Alloy Autonomus - This is the basic framework for alloy auto's. An alloy autonomous takes care of running the state machine
that the auto is using so all the user has to do is create the state machine and define functionality.
You can read more about how to use an Alloy Autonomous [Here](https://github.com/GarrettBurroughs/Alloy/wiki/Creating-An-Auto-Mode)
+
+4. Simple Autonomous - A simple autonomous mode further abstracts away auto mode creation allowing
+for command like auto mode programming. You can read more about how to use a Simple autonomous [Here](https://github.com/GarrettBurroughs/Alloy/wiki/Creating-An-Auto-Mode)
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
new file mode 100644
index 0000000..81e4f4c
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/SimpleAutonomous.java
@@ -0,0 +1,62 @@
+/*
+MIT License
+
+Copyright (c) 2018 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+package org.montclairrobotics.alloy.auto;
+
+import org.montclairrobotics.alloy.auto.States.ConditionalState;
+import org.montclairrobotics.alloy.auto.States.Drive;
+import org.montclairrobotics.alloy.auto.States.Turn;
+import org.montclairrobotics.alloy.utils.Input;
+import org.montclairrobotics.alloy.vector.Angle;
+
+/**
+ * Created by MHS Robotics on 1/26/2018.
+ *
+ * SimpleAutonomous abstracts away the concept of adding states to a state machine, so that
+ * states can be added as if they were commands to be executed by the auto mode. This class only has
+ * the pre-written states included, so another class extending this one would need to be created to
+ * add any custom states
+ *
+ * @author Garrett Burroughs
+ * @since
+ */
+public class SimpleAutonomous extends AlloyAutonomous {
+ @Override
+ public void setup() {}
+
+ public void drive(double speed, double distance) {
+ addState(new Drive(speed, distance));
+ }
+
+ public void turn(double speed, Angle angle) {
+ addState(new Turn(speed, angle));
+ }
+
+ public void contition(Input condition, State state){
+ addState(new ConditionalState(condition, state));
+ }
+
+ public void addState(State state){
+ auto.addState(state);
+ }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/State.java b/src/main/java/org/montclairrobotics/alloy/auto/State.java
index 7887b13..d19ef5f 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/State.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/State.java
@@ -38,6 +38,8 @@ of this software and associated documentation files (the "Software"), to deal
public abstract class State {
private Integer nextState = null;
+ protected String debug = "Running State: ";
+ protected String description = "None";
/** The start method is the first thing called when the state is run */
public abstract void start();
@@ -55,10 +57,33 @@ public abstract class State {
*/
public abstract boolean isDone();
+ /**
+ * In order to have a non linear state machine, the state machine must know what state to go to
+ * when it is done with the previous state. The state also sometimes needs to know the current
+ * state for example if it just wanted to increment the state by one.
+ *
+ * @param currentState the state the state machine is currently running
+ * @return the state the state machine should go to
+ */
public int getNextState(int currentState) {
if (nextState != null) {
return nextState;
}
return currentState + 1;
}
+
+ /**
+ * When a state machine is running, it will debug out information about the state it is running.
+ * It will debug the result of debugInfo
+ *
+ * @param currentState the current state so that it can be used in the debug
+ * @return debug information about the state
+ */
+ public String debugInfo(int currentState) {
+ return debug + currentState;
+ }
+
+ public String verboseDebug() {
+ return description;
+ }
}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
index 8bd67f0..973d063 100644
--- a/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
+++ b/src/main/java/org/montclairrobotics/alloy/auto/StateMachine.java
@@ -23,8 +23,10 @@ of this software and associated documentation files (the "Software"), to deal
*/
package org.montclairrobotics.alloy.auto;
+import com.qualcomm.robotcore.util.ElapsedTime;
import java.util.ArrayList;
import java.util.Arrays;
+import org.montclairrobotics.alloy.components.Component;
/**
* Created by MHS Robotics on 12/16/2017.
@@ -36,12 +38,17 @@ of this software and associated documentation files (the "Software"), to deal
* State machines can be used for controlling auto modes, but can also be ran in teleop modes for
* pre coded instructions that make driving easier.
*
+ * State machines are also states themselves so a state machine can run another state machine.
+ * This allows for the reuse of auto code
+ *
* @author Garrett Burroughs
* @since 0.1
+ * @version 0.1
*/
public class StateMachine extends State {
- private ArrayList states;
+ /** The states list keeps track of all of the states that will be run in the state machine */
+ ArrayList states;
/**
* Since state machines can run in a non linear fashion, the last state in the array may not be
@@ -54,33 +61,129 @@ public class StateMachine extends State {
* Note: even if your state machine is running in a linear fashion, it needs to have a final
* state.
*/
- private Integer finalState;
+ Integer finalState;
+
+ /** The name of the state machine, this is purely for debugging purposes */
+ String name;
+
+ /**
+ * A boolean that keeps track of weather or not the current state has been run yet, this is used
+ * to determine it the start() method of the state should be run
+ */
+ boolean stateStarted;
+
+ /** the time spent in the state that has most recently finished */
+ double timeInLastState;
- public StateMachine(State... states) {
- this.states = (ArrayList) Arrays.asList(states);
+ /** A timer object to keep track of the time in states. */
+ private ElapsedTime timer;
+
+ /** Keeps track of weather or not the state machine has finished, True if it has */
+ boolean done = false;
+
+ /** Keeps track of the current state */
+ int state;
+
+ public StateMachine(String name, int finalState, State... states) {
+ this.name = name;
+ this.states = new ArrayList<>(Arrays.asList(states));
+ this.finalState = finalState;
+ description = "";
}
+ public StateMachine(State ... states){
+ this("State Machine", 0, states);
+ }
+
+ public StateMachine(String name, String description, int finalState, State... states) {
+ this(name, finalState, states);
+ super.description = description;
+ }
+
+ public void setFinalState(Integer finalState) {
+ this.finalState = finalState;
+ }
+
+ public void setName(String name) {
+ this.name = name;
+ }
+
+ /** Read out that the state has started and reset the timer */
@Override
public void start() {
- // Called when the state machine starts
+ Component.debugger.driverInfo("Running", name);
+ timer.reset();
}
+ /** The run method takes care of actually running the states */
@Override
public void run() {
- // Called every loop while the state machine is running
+ State currentState = states.get(state);
+
+ //Check If the state has started, if it hasn't run the 'start()' method
+ if (!stateStarted) {
+ timer.reset();
+ currentState.start();
+ stateStarted = true; // State has been started
+ }
+
+ currentState.run(); //run the state
+
+ if (currentState.isDone()) { //check if the state has finished
+ timeInLastState = timer.nanoseconds(); // Update the last state time
+ currentState.stop(); // Run the stop() method on the state
+ if (currentState.getNextState(state)
+ == finalState) { // Check if the state machine is done
+ done = true; // set the state machine as done
+ return; // return (exit) out of the run() method
+ }
+ if (currentState.getNextState(state)
+ < states.size()) { // make sure there is a next state to go to
+ state = currentState.getNextState(state); // go to the next state
+ } else {
+ Component.debugger.error(
+ "STATE MACHINE OUT OF BOUNDS"); // Give the user an error if there is no next state to go to
+ done = true; // stop the state machine
+ return; // exit the run method to ensure nothing else runs
+ }
+ stateStarted = false; // The next state has not started yet.
+ }
+ Component.debugger.test(name + " | State: ", state); // Debug info about the state
+ Component.debugger.debug(currentState.debugInfo(state), currentState.verboseDebug());
}
+ /** When the state machine is finished, read out it has finished */
@Override
public void stop() {
- // Called once when the state machine stops
+ Component.debugger.test("Status", name + ", Finished ");
}
+ /**
+ * determine if the state machine is done
+ *
+ * @return true if the state machine is done
+ */
@Override
public boolean isDone() {
- return false;
+ return done;
}
+ /**
+ * Adds a state to the state machine. States can be added to a state machine but should not be
+ * added after the state machine
+ *
+ * @param state state that will be added to the state machine
+ */
public void addState(State state) {
states.add(state);
}
+
+ /**
+ * Gets the time in the most recently completed state
+ *
+ * @return The time in last state
+ */
+ public double getTimeInLastState() {
+ return timeInLastState;
+ }
}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java b/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
new file mode 100644
index 0000000..d92e0a4
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/ConditionalState.java
@@ -0,0 +1,80 @@
+/*
+MIT License
+
+Copyright (c) 2019 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+package org.montclairrobotics.alloy.auto.States;
+
+import org.montclairrobotics.alloy.auto.State;
+import org.montclairrobotics.alloy.utils.Input;
+
+/**
+ * A conditional state runs a state if the condition is true
+ *
+ * A conditional state takes in a boolean input, and evaluates it when the state starts. If the
+ * input is true when the state starts, the passed in state will execute.
+ */
+public class ConditionalState extends State {
+ /** The boolean input that will determine if the state is run */
+ Input condition;
+
+ /** The state that will be run, if the input is true */
+ State state;
+
+ /** A variable to keep track whether or not the state should be run */
+ boolean run;
+
+ public ConditionalState(Input condition, State state) {
+ this.condition = condition;
+ this.state = state;
+ }
+
+ @Override
+ public void start() {
+ run = condition.get(); // Evaluate the condition
+ if (run) {
+ state.start();
+ }
+ }
+
+ @Override
+ public void run() {
+ if (run) {
+ state.run();
+ }
+ }
+
+ @Override
+ public void stop() {
+ if (run) {
+ state.stop();
+ }
+ }
+
+ @Override
+ public boolean isDone() {
+ if (run) {
+ return state.isDone();
+ } else {
+ return true;
+ }
+ }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java b/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
new file mode 100644
index 0000000..9d0e114
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/Drive.java
@@ -0,0 +1,90 @@
+/*
+MIT License
+
+Copyright (c) 2018 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+package org.montclairrobotics.alloy.auto.States;
+
+import org.montclairrobotics.alloy.auto.State;
+import org.montclairrobotics.alloy.core.Alloy;
+import org.montclairrobotics.alloy.drive.DTInput;
+import org.montclairrobotics.alloy.drive.DriveTrain;
+import org.montclairrobotics.alloy.utils.ConstantInput;
+import org.montclairrobotics.alloy.vector.Angle;
+import org.montclairrobotics.alloy.vector.Polar;
+import org.montclairrobotics.alloy.vector.Vector;
+
+/**
+ * Created by MHS Robotics on 1/26/2018.
+ *
+ * @author Garrett Burroughs
+ * @since
+ */
+public class Drive extends State {
+
+ DriveTrain driveTrain;
+ private double speed;
+ private double distance;
+ private int[] startValues;
+ private static int tolerance = 5;
+
+ public static void setTolerance(int tolerance) {
+ Drive.tolerance = tolerance;
+ }
+
+ public Drive(double speed, double distance) {
+ driveTrain = Alloy.getDriveTrain();
+ this.speed = speed;
+ this.distance = distance;
+ }
+
+ public Drive setDriveTrain(DriveTrain driveTrain) {
+ this.driveTrain = driveTrain;
+ return this;
+ }
+
+ @Override
+ public void start() {
+ startValues = driveTrain.getEncoderValues();
+ }
+
+ @Override
+ public void run() {
+ driveTrain.setInput(
+ new ConstantInput(new DTInput(new Polar(speed, Angle.ZERO), Angle.ZERO)));
+ }
+
+ @Override
+ public void stop() {
+ driveTrain.setInput(new ConstantInput(new DTInput(Vector.ZERO, Angle.ZERO)));
+ }
+
+ @Override
+ public boolean isDone() {
+ int[] currentValues = driveTrain.getEncoderValues();
+ double total = 0;
+ for (int i = 0; i < startValues.length; i++) {
+ total += currentValues[i] - startValues[i];
+ }
+ double average = total / (double) currentValues.length;
+ return Math.abs(distance - average) < tolerance;
+ }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
new file mode 100644
index 0000000..44c633c
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/auto/States/Turn.java
@@ -0,0 +1,106 @@
+/*
+MIT License
+
+Copyright (c) 2018 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+package org.montclairrobotics.alloy.auto.States;
+
+import org.montclairrobotics.alloy.auto.State;
+import org.montclairrobotics.alloy.core.Alloy;
+import org.montclairrobotics.alloy.drive.DTInput;
+import org.montclairrobotics.alloy.drive.DriveTrain;
+import org.montclairrobotics.alloy.utils.ConstantInput;
+import org.montclairrobotics.alloy.vector.Angle;
+import org.montclairrobotics.alloy.vector.Vector;
+
+/**
+ * Created by MHS Robotics on 1/26/2018.
+ *
+ * @author Garrett Burroughs
+ * @since
+ */
+public class Turn extends State {
+
+ private double speed;
+ private double degrees;
+ private int[] rightSideStartValues;
+ private int[] leftSideStartValues;
+ private DriveTrain driveTrain;
+
+ private static double ticksPerDegree = 1;
+
+ public static void setTicksPerDegree(double ticksPerDegree) {
+ Turn.ticksPerDegree = ticksPerDegree;
+ }
+
+ private static double tolerance = 5;
+
+ public static void setTolerance(double tolerance) {
+ Turn.tolerance = tolerance;
+ }
+
+ public Turn(double speed, Angle angle) {
+ this.speed = speed;
+ this.degrees = angle.getDegrees();
+ driveTrain = Alloy.getDriveTrain();
+ }
+
+ @Override
+ public void start() {
+ rightSideStartValues = Alloy.getDriveTrain().getRightEncoderValues();
+ leftSideStartValues = Alloy.getDriveTrain().getLeftEncoderValues();
+ }
+
+ @Override
+ public void run() {
+ if (degrees > 0) {
+ driveTrain.setInput(
+ new ConstantInput(new DTInput(Vector.ZERO, new Angle(speed))));
+ } else {
+ driveTrain.setInput(
+ new ConstantInput(new DTInput(Vector.ZERO, new Angle(-speed))));
+ }
+ }
+
+ @Override
+ public void stop() {
+ driveTrain.setInput(new ConstantInput(new DTInput(Vector.ZERO, Angle.ZERO)));
+ }
+
+ @Override
+ public boolean isDone() {
+ int rightTicks = 0;
+ int[] currentRight = driveTrain.getRightEncoderValues();
+ for (int i = 0; i < rightSideStartValues.length; i++) {
+ rightTicks += Math.abs(currentRight[i] - rightSideStartValues[i]);
+ }
+
+ int leftTicks = 0;
+ int[] currentLeft = driveTrain.getLeftEncoderValues();
+ for (int i = 0; i < leftSideStartValues.length; i++) {
+ leftTicks += Math.abs(currentLeft[i] - leftSideStartValues[i]);
+ }
+
+ int ticksMoved = rightTicks + leftTicks;
+
+ return Math.abs(ticksMoved - degrees * ticksPerDegree) < tolerance;
+ }
+}
diff --git a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
index 3c6bb9a..1ec9c93 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Alloy.java
@@ -82,6 +82,7 @@ public void init() {
robotSetup();
initialization();
+ driveTrain.setDefaultInput();
}
@Override
diff --git a/src/main/java/org/montclairrobotics/alloy/core/Encoder.java b/src/main/java/org/montclairrobotics/alloy/core/Encoder.java
index f3bc16d..16e3fe3 100644
--- a/src/main/java/org/montclairrobotics/alloy/core/Encoder.java
+++ b/src/main/java/org/montclairrobotics/alloy/core/Encoder.java
@@ -61,12 +61,25 @@ public abstract class Encoder extends InputComponent {
/** Max speed that the motor can go Measured in Ticks per Second */
private double maxSpeed;
+ /** Keeps track of the ticks when the encoder was "reset" */
+ private int ticksAtReset;
+
/**
* A method that should be overridden by the encoder
*
* @return the raw value of encoder ticks that the encoder reads
*/
- public abstract int getTicks();
+ public abstract int getRawTicks();
+
+ /** Gets the ticks, this value is affected by encoder resets */
+ public int getTicks() {
+ return getRawTicks() - ticksAtReset;
+ }
+
+ /** soft reset the encoder, setting the current value as 0 ticks */
+ public void reset() {
+ ticksAtReset = getRawTicks();
+ }
public Encoder(double distancePerTick, double maxSpeed) {
this.distancePerTick = distancePerTick;
diff --git a/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java b/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java
index 39bd492..dde70ba 100644
--- a/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java
+++ b/src/main/java/org/montclairrobotics/alloy/drive/DriveTrain.java
@@ -23,10 +23,11 @@ of this software and associated documentation files (the "Software"), to deal
*/
package org.montclairrobotics.alloy.drive;
-import org.montclairrobotics.alloy.components.InputComponent;
+import java.util.ArrayList;
import org.montclairrobotics.alloy.motor.Mapper;
import org.montclairrobotics.alloy.motor.MotorGroup;
import org.montclairrobotics.alloy.motor.MotorModule;
+import org.montclairrobotics.alloy.utils.Input;
/**
* Created by MHS Robotics on 12/16/2017.
@@ -35,17 +36,57 @@ of this software and associated documentation files (the "Software"), to deal
* @since 0.1
*/
public class DriveTrain extends MotorGroup {
- public DriveTrain(InputComponent input, Mapper mapper, MotorModule... modules) {
+ Input defaultInput;
+
+ public DriveTrain(Input input, Mapper mapper, MotorModule... modules) {
super(input, mapper, modules);
+ defaultInput = input;
+ }
+
+ public void setDefaultInput() {
+ setInput(defaultInput);
+ }
+
+ public int[] getEncoderValues() {
+ ArrayList modules = getModules();
+ int[] values = new int[modules.size()];
+ for (int i = 0; i < values.length; i++) {
+ values[i] = modules.get(i).getEncoder().getTicks();
+ }
+ return values;
}
- public DriveTrain setInput(InputComponent input) {
- this.input = input;
- return this;
+ public int[] getRightEncoderValues() {
+ ArrayList modules = getModules();
+ int total = 0;
+ for (MotorModule m : modules) {
+ if (m.getOffset().getX() > 0) {
+ total++;
+ }
+ }
+ int[] values = new int[total];
+ for (int i = 0; i < values.length; i++) {
+ if (modules.get(i).getOffset().getX() > 0) {
+ values[i] = modules.get(i).getEncoder().getTicks();
+ }
+ }
+ return values;
}
- public DriveTrain setMapper(Mapper mapper) {
- this.mapper = mapper;
- return this;
+ public int[] getLeftEncoderValues() {
+ ArrayList modules = getModules();
+ int total = 0;
+ for (MotorModule m : modules) {
+ if (m.getOffset().getX() < 0) {
+ total++;
+ }
+ }
+ int[] values = new int[total];
+ for (int i = 0; i < values.length; i++) {
+ if (modules.get(i).getOffset().getX() < 0) {
+ values[i] = modules.get(i).getEncoder().getTicks();
+ }
+ }
+ return values;
}
}
diff --git a/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java b/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java
index 12dcf39..f76f548 100644
--- a/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java
+++ b/src/main/java/org/montclairrobotics/alloy/ftc/FTCMotor.java
@@ -103,7 +103,7 @@ public boolean getInverted() {
public Encoder getEncoder() {
return new Encoder() {
@Override
- public int getTicks() {
+ public int getRawTicks() {
return motor.getCurrentPosition();
}
};
diff --git a/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java b/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java
index 97149ad..a667d80 100644
--- a/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java
+++ b/src/main/java/org/montclairrobotics/alloy/ftc/FTCTargetMotor.java
@@ -208,7 +208,7 @@ public Mode getRunmode() {
public Encoder getEncoder() {
return new Encoder() {
@Override
- public int getTicks() {
+ public int getRawTicks() {
return getPosition();
}
};
diff --git a/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java b/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java
index 840a5b1..f676b2f 100644
--- a/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java
+++ b/src/main/java/org/montclairrobotics/alloy/motor/MotorGroup.java
@@ -26,8 +26,8 @@ of this software and associated documentation files (the "Software"), to deal
import java.util.ArrayList;
import java.util.Arrays;
import org.montclairrobotics.alloy.components.Component;
-import org.montclairrobotics.alloy.components.InputComponent;
import org.montclairrobotics.alloy.update.Update;
+import org.montclairrobotics.alloy.utils.Input;
/**
* A motor group is a group of motor modules that run together
@@ -43,15 +43,15 @@ of this software and associated documentation files (the "Software"), to deal
public class MotorGroup extends Component {
/** The mapper that will be used for power assignment and calculation */
- public Mapper mapper;
+ private Mapper mapper;
/** The modules contained in the motor group */
- public ArrayList modules;
+ private ArrayList modules;
/** The control input for the motor group */
- public InputComponent input;
+ private Input input;
- public MotorGroup(InputComponent input, Mapper mapper, MotorModule... modules) {
+ public MotorGroup(Input input, Mapper mapper, MotorModule... modules) {
this.input = input;
this.mapper = mapper;
this.modules = (ArrayList) Arrays.asList(modules);
@@ -63,7 +63,7 @@ public MotorGroup(InputComponent input, Mapper mapper, MotorModule... modules) {
* @param input
* @param modules
*/
- public MotorGroup(InputComponent input, MotorModule... modules) {
+ public MotorGroup(Input input, MotorModule... modules) {
this(input, new DefaultMapper(), modules);
}
@@ -71,4 +71,24 @@ public MotorGroup(InputComponent input, MotorModule... modules) {
public void controlPower() {
mapper.map(input, modules.toArray(new MotorModule[modules.size()]));
}
+
+ public Mapper getMapper() {
+ return mapper;
+ }
+
+ public void setMapper(Mapper mapper) {
+ this.mapper = mapper;
+ }
+
+ public Input getInput() {
+ return input;
+ }
+
+ public void setInput(Input input) {
+ this.input = input;
+ }
+
+ public ArrayList getModules() {
+ return modules;
+ }
}
diff --git a/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java b/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java
index b3eae92..72567af 100644
--- a/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java
+++ b/src/main/java/org/montclairrobotics/alloy/motor/MotorModule.java
@@ -30,7 +30,9 @@ of this software and associated documentation files (the "Software"), to deal
import org.montclairrobotics.alloy.components.Step;
import org.montclairrobotics.alloy.core.Encoder;
import org.montclairrobotics.alloy.core.Motor;
+import org.montclairrobotics.alloy.exceptions.InvalidConfigurationException;
import org.montclairrobotics.alloy.update.Update;
+import org.montclairrobotics.alloy.utils.BangBang;
import org.montclairrobotics.alloy.utils.ConstantInput;
import org.montclairrobotics.alloy.utils.ErrorCorrection;
import org.montclairrobotics.alloy.vector.Vector;
@@ -53,24 +55,41 @@ of this software and associated documentation files (the "Software"), to deal
*/
public class MotorModule extends Component {
/** The motors that the module will control */
- public ArrayList motors;
+ private ArrayList motors;
/** The direction that the modules run for use in a motor group */
- public Vector direction;
+ private Vector direction;
/** the position of the motor, relative to the center of the motor group */
- public Vector offset;
+ private Vector offset;
/** An error correction that will control the power */
- public ErrorCorrection powerControl;
+ private ErrorCorrection powerControl;
/** The encoder that keeps track of the position and controls the motors */
- public Encoder encoder;
+ private Encoder encoder;
/** how fast the module should be running */
- public double targetPower;
+ private double targetPower;
- public InputComponent modifier;
+ /**
+ * An input component, that takes in the motor input, and runs it to the output (setting the
+ * motor powers) this way, steps can be added to this modifier, and the input of the module can
+ * be changed
+ */
+ private InputComponent modifier;
+
+ /** True when the module is being set to a position */
+ private boolean toPotsition = false;
+
+ /** If running to a position, the target position the motor will aim to go to */
+ private double targetPosition;
+
+ /** Error Corrector to get to the right position */
+ private ErrorCorrection positionCorrection;
+
+ /** How fast the motors move when they are having their position be set */
+ private double movementSpeed;
/**
* Create a fully functioning motor module
@@ -99,6 +118,10 @@ public MotorModule(
}
modifier = new InputComponent() {};
+
+ positionCorrection = new BangBang(1);
+
+ positionCorrection.setInput(encoder);
}
/**
@@ -134,14 +157,28 @@ public MotorModule setErrorCorrection(ErrorCorrection powerControl) {
return this;
}
+ public MotorModule setPositionCorrection(ErrorCorrection positionCorrection) {
+ this.positionCorrection = positionCorrection;
+ return this;
+ }
+
@Update
public void powerCorrection() {
if (status.isEnabled()) { // Check if its enabled
- for (Motor m : motors) {
- if (powerControl != null) {
- m.setMotorPower(targetPower + powerControl.getCorrection());
- } else {
- m.setMotorPower(targetPower);
+ if (!toPotsition) { // Manual operation
+ for (Motor m : motors) {
+ if (powerControl != null) {
+ m.setMotorPower(targetPower + powerControl.getCorrection());
+ } else {
+ m.setMotorPower(targetPower);
+ }
+ }
+ } else { // Autonomous operation
+ for (Motor m : motors) {
+ if (encoder == null)
+ throw new InvalidConfigurationException(
+ "Modules must have an encoder to set their position");
+ m.setMotorPower(positionCorrection.getCorrection() * movementSpeed);
}
}
} else { // If disabled, set the power to 0
@@ -161,6 +198,16 @@ public void setPower(double power) {
modifier.applySteps();
targetPower = modifier.get();
powerControl.setTarget(modifier.get());
+ toPotsition = false;
+ }
+
+ public void setMovementSpeed(double speed) {
+ this.movementSpeed = speed;
+ }
+
+ public void setTargetPosition(double targetPosition) {
+ positionCorrection.setTarget(targetPosition);
+ toPotsition = true;
}
public MotorModule setPowerModifier(InputComponent modifier) {
@@ -181,6 +228,8 @@ public void disableAction() {
}
}
+ // Autonomous operation
+
/** @return the motor arraylist in the module */
public ArrayList getMotors() {
return motors;
@@ -215,4 +264,16 @@ public InputComponent getModifier() {
public Vector getOffset() {
return offset;
}
+
+ public boolean atTargetPosition(double tolerance) {
+ return Math.abs(encoder.getTicks() - targetPosition) < tolerance;
+ }
+
+ public void toPosition() {
+ toPotsition = true;
+ }
+
+ public void usingPower() {
+ toPotsition = false;
+ }
}
diff --git a/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java b/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java
index f0e49a5..1dc7e68 100644
--- a/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java
+++ b/src/main/java/org/montclairrobotics/alloy/test/IntakeMapper.java
@@ -32,10 +32,10 @@ public class IntakeMapper implements Mapper {
@Override
public void map(Vector input, MotorModule... modules) {
for (MotorModule m : modules) {
- if (m.offset.getX() > 0) {
- m.setPower((input.getY() + input.getX()) * Utils.sign(m.direction.getY()));
+ if (m.getOffset().getX() > 0) {
+ m.setPower((input.getY() + input.getX()) * Utils.sign(m.getDirection().getY()));
} else {
- m.setPower((input.getY() - input.getX()) * Utils.sign(m.direction.getY()));
+ m.setPower((input.getY() - input.getX()) * Utils.sign(m.getDirection().getY()));
}
}
}
diff --git a/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java b/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java
new file mode 100644
index 0000000..728af90
--- /dev/null
+++ b/src/main/java/org/montclairrobotics/alloy/test/SquareAuto.java
@@ -0,0 +1,54 @@
+/*
+MIT License
+
+Copyright (c) 2019 Garrett Burroughs
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+package org.montclairrobotics.alloy.test;
+
+import org.montclairrobotics.alloy.auto.SimpleAutonomous;
+import org.montclairrobotics.alloy.auto.States.Turn;
+import org.montclairrobotics.alloy.vector.Angle;
+
+public class SquareAuto extends SimpleAutonomous {
+
+ @Override
+ public void setup() {
+ Turn.setTicksPerDegree(10); // This is a dummy value, this value will need to be tested for
+
+ /*
+ Keep in mind, even though states can be added like this, because they are created by methods inherited from
+ Simple autonomous, they are not actually run right here.
+ This means that you can not conditional add states here, for example
+
+ if(sensor.value > 5){
+ drive(1, 5);
+ }
+ a
+ would not work, because it would be reading the sensor value on startup.
+ If you want to run states conditionally, use ConditionalState
+ */
+ for (int i = 0; i < 4; i++) {
+ // These functions come from SimpleAutonomous, they just add a new state to the state machine
+ drive(1, 100);
+ turn(1, new Angle(90));
+ }
+ }
+}