();
+ sideChooser.addDefault("Red", Side.Left);
+ sideChooser.addObject("Blue", Side.Right);
+ SmartDashboard.putData("Side Selection", sideChooser);
+
+// SmartDashboard.putData("Reset Position", new ResetPosition());
+// SmartDashboard.putData("Gear Dropoff", new GearIntakeDropOff());
+// SmartDashboard.putData("Scaler Start", new ScalerUp(1));
+// SmartDashboard.putData("Acquire Gear", new DriveAcquireGear());
+// SmartDashboard.putData("Place Gear", new DrivePlaceGear());
+// SmartDashboard.putData("Zero Yaw", new ZeroYaw());
+//
+//// SmartDashboard.putData(new RecordVoltageData());
+//// SmartDashboard.putData(new SendSDVoltage());
+// SmartDashboard.putData(new DriveMoveDistanceProfile(100.0, 30, 30, 0).addA(new NavxAssist()));
+// SmartDashboard.putData(new DriveMoveInCircleProfile(90, 60, true, 30, 30, 30));
+// SmartDashboard.putData("Drive Test", new DriveExp(.5,0).addA(new NavxVisionAssist(Pipes.Peg)));
+// SmartDashboard.putData("Sine Profile 100", new MoveSine(100));
+ side = Side.Left;
+ prevTime = Timer.getFPGATimestamp();
+ }
+ @Override
+ public void robotPeriodic() {
+ Setting.fetchAround();
+ BooleanSetting.fetchAround();
+
+ vision.debugLog();
+ //navigation.integrate();
+ navigation.displayPosition();
+ drive.debugLog();
+ log();
+ }
+ @Override
+ public void robotContinuous() {
+// dt = Timer.getFPGATimestamp() - prevTime;
+// prevTime += dt;
+// SmartDashboard.putNumber("dt", dt);
+ navigation.integrate();
+ }
+
+
+
+ @Override
+ public void autonomousInit() {
+
+ navigation.resetPosition();
+ retrieveSide();
+ Location.changeSide(side, Dms.Field.LENGTH);
+ autonomousCommand = autonomousChooser.getSelected();
+ if (autonomousCommand != null) autonomousCommand.start();
+
+ }
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ }
+ @Override
+ public void autonomousContinuous() {
+
+ }
+
+
+
+ @Override
+ public void teleopInit() {
+
+
+ }
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+// d.Arm();
+// d.Pivot();
+
+ }
+ @Override
+ public void teleopContinuous() {
+
+ }
+
+
+
+ @Override
+ public void testPeriodic() {
+ LiveWindow.run();
+
+ }
+ @Override
+ public void disabledInit() {
+ if (autonomousCommand != null)
+ autonomousCommand.cancel();
+ }
+ @Override
+ public void disabledPeriodic() {
+ Scheduler.getInstance().run();
+
+ }
+ @Override
+ public void disabledContinuous() {
+
+ }
+
+
+
+ public void log(){
+ scaler.log();
+ drive.log();
+ //shooter.log();
+ //ballIntake.log();
+ gearIntake.log();
+ navigation.log();
+ //ultra.log();
+ vision.log();
+ }
+
+ public void debugLog(){
+ scaler.debugLog();
+ drive.debugLog();
+ //shooter.debugLog();
+ //ballIntake.debugLog();
+ gearIntake.debugLog();
+ navigation.debugLog();
+ //ultra.debugLog();
+ vision.debugLog();
+ }
+
+
+
+
+
+
+ public static void retrieveSide(){
+ if(sideChooser.getSelected() != null){
+ side = sideChooser.getSelected();
+ }else{
+ System.out.println("Error : sideChooser was found null when retrieving side.");
+ };
+ }
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/assists/NavxAssist.java b/src/org/usfirst/frc/team3735/robot/assists/NavxAssist.java
new file mode 100644
index 0000000..929e140
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/assists/NavxAssist.java
@@ -0,0 +1,32 @@
+package org.usfirst.frc.team3735.robot.assists;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.cmds.ComAssist;
+
+public class NavxAssist extends ComAssist{
+
+ Double angle;
+
+ public NavxAssist(Double yaw){
+ angle = yaw;
+ }
+
+ public NavxAssist(){
+
+ }
+
+ @Override
+ public void initialize() {
+ if(angle == null){
+ Robot.navigation.getController().setSetpoint(Robot.navigation.getYaw());
+ }else{
+ Robot.navigation.getController().setSetpoint(angle.doubleValue());
+ }
+ }
+
+ @Override
+ public void execute() {
+ Robot.drive.setNavxAssist(Robot.navigation.getController().getError());
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/assists/NavxVisionAssist.java b/src/org/usfirst/frc/team3735/robot/assists/NavxVisionAssist.java
new file mode 100644
index 0000000..713920d
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/assists/NavxVisionAssist.java
@@ -0,0 +1,55 @@
+package org.usfirst.frc.team3735.robot.assists;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.subsystems.Navigation;
+import org.usfirst.frc.team3735.robot.subsystems.Vision;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.cmds.ComAssist;
+
+public class NavxVisionAssist extends ComAssist{
+
+ private Pipes pipe;
+ private double prevWorking = 0;
+
+ public NavxVisionAssist(Pipes p){
+ this.pipe = p;
+ prevWorking = 0;
+ requires(Robot.vision);
+ requires(Robot.navigation);
+ }
+
+ @Override
+ public void initialize() {
+ Robot.vision.setMainHandler(pipe);
+ prevWorking = 0;
+ }
+
+ @Override
+ public void execute() {
+ double input = Robot.vision.getRelativeCX();
+ if(input != Vision.nullValue){
+ if(input != prevWorking){
+ Robot.navigation.getController().setSetpoint(
+ VortxMath.continuousLimit(
+ Robot.navigation.getYaw() + (input * Robot.vision.dpp.getValue()),
+ -180, 180)
+ );
+ prevWorking = input;
+ }
+ Robot.drive.setVisionAssist((Robot.navigation.getController().getError()/180.0) * Navigation.navVisCo.getValue());
+ }else{
+ Robot.drive.setVisionAssist(0);
+ }
+ }
+
+ @Override
+ public void end(){
+ Robot.drive.setVisionAssist(0);
+ }
+
+//doy mun fuh = ???
+//sing chow em = hello
+//em depp
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/assists/VisionAssist.java b/src/org/usfirst/frc/team3735/robot/assists/VisionAssist.java
new file mode 100644
index 0000000..e3dfa40
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/assists/VisionAssist.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team3735.robot.assists;
+
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.subsystems.Vision;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.util.cmds.ComAssist;
+
+public class VisionAssist extends ComAssist{
+
+ Pipes pipe;
+ private double prevWorking = 0;
+
+ public VisionAssist(Pipes p){
+ requires(Robot.vision);
+ this.pipe = p;
+ }
+
+ @Override
+ public void initialize() {
+ Robot.vision.setMainHandler(pipe);
+ }
+
+ @Override
+ public void execute() {
+ double in = Robot.vision.getRelativeCX();
+ if(in == Vision.nullValue){
+ Robot.drive.setVisionAssist(0);
+ }else{
+ prevWorking = in;
+ Robot.drive.setVisionAssist(in * .0025);
+ }
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/InterruptOperations.java b/src/org/usfirst/frc/team3735/robot/commands/InterruptOperations.java
new file mode 100644
index 0000000..14ec32e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/InterruptOperations.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.commands;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class InterruptOperations extends InstantCommand {
+
+ public InterruptOperations() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.gearIntake);
+ requires(Robot.scaler);
+
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/Wait.java b/src/org/usfirst/frc/team3735/robot/commands/Wait.java
new file mode 100644
index 0000000..a7ec005
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/Wait.java
@@ -0,0 +1,32 @@
+package org.usfirst.frc.team3735.robot.commands;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+/**
+ *
+ */
+public class Wait extends TimedCommand {
+
+ public Wait(double timeout) {
+ super(timeout);
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Called once after timeout
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonBaseline.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonBaseline.java
new file mode 100644
index 0000000..33c6dda
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonBaseline.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonBaseline extends CommandGroup {
+
+ public AutonBaseline(){
+ /* Let Move FWD Only */
+ /* All the Timing Needs Adjustment | */
+ /* This */
+ /* | */
+ //112 + 20
+ //addSequential(new DriveMoveDistanceExpNavx(132, .7)); /* Straight To Pin*/
+ addSequential(new Move(350));
+ //addSequential(new DriveMoveDistanceNavxExpNaik(132));
+ //addSequential(new DriveMoveDistance(10));
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonDoNothing.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonDoNothing.java
new file mode 100644
index 0000000..4d8a9df
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonDoNothing.java
@@ -0,0 +1,16 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonDoNothing extends CommandGroup {
+
+ public AutonDoNothing() {
+ addSequential(new DriveBrake());
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGear.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGear.java
new file mode 100644
index 0000000..cf7d07f
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGear.java
@@ -0,0 +1,54 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.assists.NavxAssist;
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveExp;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonLeftGear extends CommandGroup {
+
+ public AutonLeftGear(){
+
+// addSequential(new DriveMoveDistanceExpNavx(70,.7),2.6);
+// addSequential(new DriveBrake(),.4);
+// addSequential(new DriveTurnToAnglePIDCtrl(60),2);
+// addSequential(new DriveTurnToAnglePIDCtrl(Pipes.Peg),1.5);
+// //addSequential(new DriveMoveDistanceExpNavx(88.7),3);
+// //addSequential(new DriveMoveDistanceExpVisionBumped(89, .7, Pipes.Peg),3);
+// addSequential(new DriveMoveDistanceExpNavxBumped(89, .7),2.5);
+// addSequential(new GearIntakeDropOff(),2);
+
+// addSequential(new Move(60));
+// addSequential(new DriveBrake(),.4);
+// addSequential(new TurnTo(60),1);
+// addSequential(new TurnTo(Pipes.Peg),1.5);
+// //addSequential(new DriveMoveDistanceExpNavx(88.7),3);
+// //addSequential(new DriveMoveDistanceExpVisionBumped(89, .7, Pipes.Peg),3);
+// //addSequential(new DriveMoveDistanceExpNavxBumped(89, .7),2.5);
+// addSequential(new Move(40).addT(new Bumped(.7)));
+// addSequential(new GearIntakeDropOff(),2);
+
+ addSequential(new Move(60));
+ addSequential(new DriveBrake(),.4);
+ addSequential(new TurnTo(60),2);
+ addSequential(new TurnTo(Pipes.Peg),2);
+ //addSequential(new DriveMoveDistanceExpNavx(88.7),3);
+ //addSequential(new DriveMoveDistanceExpVisionBumped(89, .7, Pipes.Peg),3);
+ //addSequential(new DriveMoveDistanceExpNavxBumped(89, .7),2.5);
+ addSequential(new Move(45,.5).addT(new Bumped(.7)));
+ addSequential(new GearIntakeDropOff(),2);
+
+
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGearBaseline.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGearBaseline.java
new file mode 100644
index 0000000..06c38ee
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGearBaseline.java
@@ -0,0 +1,31 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.Wait;
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonLeftGearBaseline extends CommandGroup {
+
+ public AutonLeftGearBaseline(){
+// addSequential(new AutonLeftGear());
+// addSequential(new Wait(2));
+// addSequential(new DriveMoveDistanceExpNavx(-10,1),1);
+// addSequential(new DriveBrake(),.4);
+// addSequential(new TurnTo(0),2);
+// addSequential(new DriveMoveDistanceExpNavx(150,1),4);
+
+ addSequential(new AutonLeftGear());
+ addSequential(new Move(-10));
+ addSequential(new DriveBrake(),.4);
+ addSequential(new TurnTo(0),2);
+ addSequential(new Move(300).addT(new Bumped(.9)));
+ addSequential(new TurnTo(0),2);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGearHopper.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGearHopper.java
new file mode 100644
index 0000000..e442189
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonLeftGearHopper.java
@@ -0,0 +1,26 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.Wait;
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonLeftGearHopper extends CommandGroup {
+
+ public AutonLeftGearHopper() {
+ addSequential(new AutonLeftGear());
+ addSequential(new TurnTo(90));
+ addSequential(new Move(-90).addT(new Bumped(.8)));
+ addSequential(new Wait(2));
+ addSequential(new Move(10));
+ addSequential(new TurnTo(-170),2);
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonMiddleGear.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonMiddleGear.java
new file mode 100644
index 0000000..4d69f7e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonMiddleGear.java
@@ -0,0 +1,19 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonMiddleGear extends CommandGroup {
+
+ public AutonMiddleGear(){
+ addSequential(new Move(75, .6).addT(new Bumped()));
+ addSequential(new GearIntakeDropOff(),4);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGear.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGear.java
new file mode 100644
index 0000000..88822a5
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGear.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonRightGear extends CommandGroup {
+
+ public AutonRightGear(){
+// addSequential(new DriveMoveDistanceExpNavx(70,.7),2.6);
+// addSequential(new DriveBrake(),.4);
+// addSequential(new DriveTurnToAnglePIDCtrl(-60),2);
+// //addSequential(new DriveMoveDistanceNavx(88.7),3);
+// addSequential(new DriveTurnToAnglePIDCtrl(Pipes.Peg),2);
+// addSequential(new DriveMoveDistanceExpNavxBumped(89, .7),3);
+// //addSequential(new DriveMoveDistanceExpVisionBumped(89, .7, Pipes.Peg),3);
+// addSequential(new GearIntakeDropOff(),3);
+
+// addSequential(new DriveMoveDistanceExpNavx(70,.7),2.6);
+// addSequential(new DriveBrake(),.4);
+// addSequential(new TurnTo(-60),2);
+// addSequential(new TurnTo(Pipes.Peg),2);
+// //addSequential(new DriveMoveDistanceExpNavx(88.7),3);
+// //addSequential(new DriveMoveDistanceExpVisionBumped(89, .7, Pipes.Peg),3);
+// addSequential(new DriveMoveDistanceExpNavxBumped(89, .7),3);
+// addSequential(new GearIntakeDropOff(),3);
+
+ addSequential(new Move(60));
+ addSequential(new DriveBrake(),.4);
+ addSequential(new TurnTo(-60),2);
+ addSequential(new TurnTo(Pipes.Peg),2);
+ //addSequential(new DriveMoveDistanceExpNavx(88.7),3);
+ //addSequential(new DriveMoveDistanceExpVisionBumped(89, .7, Pipes.Peg),3);
+ //addSequential(new DriveMoveDistanceExpNavxBumped(89, .7),2.5);
+ addSequential(new Move(45,.5).addT(new Bumped(.7)));
+ addSequential(new GearIntakeDropOff(),2);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGearBaseline.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGearBaseline.java
new file mode 100644
index 0000000..1074cf7
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGearBaseline.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonRightGearBaseline extends CommandGroup {
+
+ public AutonRightGearBaseline(){
+// addSequential(new AutonRightGear());
+// addSequential(new DriveMoveDistanceExpNavx(-10,1),1);
+// addSequential(new DriveBrake(),.4);
+// addSequential(new TurnTo(0),2);
+// addSequential(new DriveMoveDistanceExpNavx(150,1),4);
+
+ addSequential(new AutonRightGear());
+ addSequential(new Move(-10));
+ addSequential(new DriveBrake(),.4);
+ addSequential(new TurnTo(0),2);
+ addSequential(new Move(300).addT(new Bumped(.9)));
+ addSequential(new TurnTo(0),2);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGearHopper.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGearHopper.java
new file mode 100644
index 0000000..5683bcc
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonRightGearHopper.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.commands.Wait;
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.Move;
+
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonRightGearHopper extends CommandGroup {
+
+ public AutonRightGearHopper() {
+ addSequential(new AutonRightGear());
+ addSequential(new TurnTo(-90));
+ addSequential(new Move(-90).addT(new Bumped(.8)));
+ addSequential(new Wait(2));
+ addSequential(new Move(10));
+ addSequential(new TurnTo(170),2);
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonTest.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonTest.java
new file mode 100644
index 0000000..7ab1af0
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/AutonTest.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous;
+
+import org.usfirst.frc.team3735.robot.assists.NavxAssist;
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveExp;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.triggers.Bumped;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonTest extends CommandGroup {
+
+ public AutonTest(){
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonBottomGear.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonBottomGear.java
new file mode 100644
index 0000000..a9264e3
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonBottomGear.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous.newstuff;
+
+import org.usfirst.frc.team3735.robot.commands.drive.positions.BeginAtPathY;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathPredicted;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathTight;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonBottomGear extends CommandGroup {
+
+ public AutonBottomGear() {
+ addSequential(new BeginAtPathY(Waypoints.bottomGear));
+ //addSequential(new FollowPathTight(Waypoints.bottomGear));
+ addSequential(new FollowPathPredicted(Waypoints.bottomGear));
+ addSequential(new DriveBrake(),.5);
+ addSequential(new GearIntakeDropOff());
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonBottomGearHopper.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonBottomGearHopper.java
new file mode 100644
index 0000000..9ddd74b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonBottomGearHopper.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous.newstuff;
+
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.BeginAtPathY;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathPredicted;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathTight;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonBottomGearHopper extends CommandGroup {
+
+ public AutonBottomGearHopper() {
+ addSequential(new AutonBottomGear());
+ //addSequential(new FollowPathTight(Waypoints.bottomGearHopper, true));
+ addSequential(new FollowPathPredicted(Waypoints.bottomGear));
+ addSequential(new TurnTo(0));
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonCrossField.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonCrossField.java
new file mode 100644
index 0000000..ece089a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonCrossField.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous.newstuff;
+
+import org.usfirst.frc.team3735.robot.commands.drive.positions.BeginAtPathY;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathTight;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonCrossField extends CommandGroup {
+
+ public AutonCrossField() {
+ addSequential(new BeginAtPathY(Waypoints.crossField));
+ addSequential(new FollowPathTight(Waypoints.crossField));
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonTopGear.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonTopGear.java
new file mode 100644
index 0000000..eac790b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonTopGear.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous.newstuff;
+
+import org.usfirst.frc.team3735.robot.commands.drive.positions.BeginAtPathY;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathPredicted;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathTight;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonTopGear extends CommandGroup {
+
+ public AutonTopGear() {
+ addSequential(new BeginAtPathY(Waypoints.topGear));
+ //addSequential(new FollowPathTight(Waypoints.topGear));
+ addSequential(new FollowPathPredicted(Waypoints.topGear));
+ addSequential(new DriveBrake(),.5);
+ addSequential(new GearIntakeDropOff());
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonTopGearHopper.java b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonTopGearHopper.java
new file mode 100644
index 0000000..614abaf
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/autonomous/newstuff/AutonTopGearHopper.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.commands.autonomous.newstuff;
+
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.BeginAtPathY;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathPredicted;
+import org.usfirst.frc.team3735.robot.commands.drive.positions.FollowPathTight;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveBrake;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class AutonTopGearHopper extends CommandGroup {
+
+ public AutonTopGearHopper() {
+ addSequential(new AutonTopGear());
+ //addSequential(new FollowPathTight(Waypoints.topGearHopper, true));
+ addSequential(new FollowPathPredicted(Waypoints.topGearHopper, true));
+ addSequential(new TurnTo(-180,false));
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakePivotDown.java b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakePivotDown.java
new file mode 100644
index 0000000..37c8cb7
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakePivotDown.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team3735.robot.commands.cubeintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class CubeIntakePivotDown extends Command {
+
+ public CubeIntakePivotDown() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.cubeIntake);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.cubeIntake.setPivotSpeed(-.5);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.cubeIntake.setPivotSpeed(0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakePivotUp.java b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakePivotUp.java
new file mode 100644
index 0000000..6cac7b5
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakePivotUp.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team3735.robot.commands.cubeintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class CubeIntakePivotUp extends Command {
+
+ public CubeIntakePivotUp() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.cubeIntake);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.cubeIntake.setPivotSpeed(.5);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.cubeIntake.setPivotSpeed(0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakeRollersIn.java b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakeRollersIn.java
new file mode 100644
index 0000000..15ca055
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakeRollersIn.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc.team3735.robot.commands.cubeintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class CubeIntakeRollersIn extends Command {
+
+ public CubeIntakeRollersIn() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.cubeIntake);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.cubeIntake.setRollersSpeed(.9);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.cubeIntake.setRollersSpeed(0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakeRollersOut.java b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakeRollersOut.java
new file mode 100644
index 0000000..7111b5a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/cubeintake/CubeIntakeRollersOut.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team3735.robot.commands.cubeintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class CubeIntakeRollersOut extends Command {
+
+ public CubeIntakeRollersOut() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.cubeIntake);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.cubeIntake.setRollersSpeed(-.9);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.cubeIntake.setRollersSpeed(0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/DDxDrive.java b/src/org/usfirst/frc/team3735/robot/commands/drive/DDxDrive.java
new file mode 100644
index 0000000..505492c
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/DDxDrive.java
@@ -0,0 +1,100 @@
+package org.usfirst.frc.team3735.robot.commands.drive;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.util.calc.JerkLimiter;
+import org.usfirst.frc.team3735.robot.util.calc.Range;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DDxDrive extends Command {
+
+ private JerkLimiter move;
+ private JerkLimiter turn;
+
+ private double moveMotor, turnMotor;
+// private double maxA = 2;
+// private double maxJ = 1;
+
+ private Setting maxA = new Setting("Max Acc", 10){
+ @Override
+ public double getValue(){
+ return super.getValue() / 50.0;
+ }
+ };
+
+ private Setting maxJ = new Setting("Max Jerk", 10){
+ @Override
+ public double getValue(){
+ return super.getValue() / 50.0;
+ }
+ };
+
+ private Setting maxAt = new Setting("Max Acc Turn", 10){
+ @Override
+ public double getValue(){
+ return super.getValue() / 50.0;
+ }
+ };
+
+ private Setting maxJt = new Setting("Max Jerk Turn", 10){
+ @Override
+ public double getValue(){
+ return super.getValue() / 50.0;
+ }
+ };
+
+
+ public DDxDrive() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.drive);
+
+ move = new JerkLimiter(0, new Range(maxA), new Range(maxJ));
+ turn = new JerkLimiter(0, new Range(maxAt), new Range(maxJt));
+
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ //move.reset(Robot.drive.getCurrentPercent());
+ move.reset();
+ turn.reset();
+ Robot.drive.setupDriveForSpeedControl();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+
+ moveMotor = Robot.oi.getDriveMove();
+ turnMotor = Robot.oi.getDriveTurn();
+
+ moveMotor = moveMotor * Math.pow(Math.abs(moveMotor), Drive.moveExponent.getValue() - 1);
+ turnMotor = turnMotor * Math.pow(Math.abs(turnMotor), Drive.turnExponent.getValue() - 1);
+
+ Robot.drive.normalDrive(
+ move.feed(Robot.oi.getDriveMove()) * Drive.scaledMaxMove.getValue(),
+ turn.feed(Robot.oi.getDriveTurn()) * Drive.scaledMaxTurn.getValue()
+ );
+
+
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/ExpDrive.java b/src/org/usfirst/frc/team3735/robot/commands/drive/ExpDrive.java
new file mode 100644
index 0000000..c2afe4c
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/ExpDrive.java
@@ -0,0 +1,119 @@
+package org.usfirst.frc.team3735.robot.commands.drive;
+
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.cmds.VortxCommand;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class ExpDrive extends Command {
+
+ private double moveSetValue;
+ private double turnSetValue;
+
+ private double moveMotor;
+ private double turnMotor;
+
+ private double moveMotorPrev;
+ private double turnMotorPrev;
+
+ private double fodAngle;
+ private double fodMove;
+ private double fodTurn;
+
+ private static Setting navxCo = new Setting("FOD Navx Coefficient", 2.5);
+ private static Setting navxPow = new Setting("FOD Navx Exponent", 1);
+ private static Setting fodMoveCo = new Setting("FOD Move Exponent", 1);
+
+private static Setting moveReactivity = new Setting("Move Reactivity", Constants.Drive.moveReactivity);
+ private static Setting turnReactivity = new Setting("Turn Reactivity", Constants.Drive.turnReactivity);
+
+
+ public ExpDrive() {
+ requires(Robot.drive);
+ }
+
+ protected void initialize() {
+ super.initialize();
+
+ Robot.drive.setupDriveForSpeedControl();
+ moveSetValue = Robot.drive.getCurrentPercent();
+ turnSetValue = 0.0;
+
+ moveMotor = 0.0;
+ turnMotor = 0.0;
+
+ moveMotorPrev = moveSetValue;
+ turnMotorPrev = 0.0;
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ public void execute() {
+ super.execute();
+ moveSetValue = Robot.oi.getDriveMove();
+ turnSetValue = Robot.oi.getDriveTurn();
+ if(Robot.oi.getFODMag() > .1){
+ fodMove = Math.pow(Robot.oi.getFODMag(), fodMoveCo.getValue());
+ fodAngle = Robot.oi.getFODAngle();
+ Robot.navigation.getController().setSetpoint(fodAngle);
+
+ fodTurn = fodMove * (Math.pow(Robot.navigation.getController().getError()/180.0, navxPow.getValue())) * navxCo.getValue();
+
+ }else{
+ fodMove = 0;
+ fodTurn = 0;
+ }
+
+ //moveSetValue = moveSetValue + fodMove;
+ turnSetValue = turnSetValue + fodTurn;
+
+ moveMotor = (moveSetValue-moveMotorPrev)*moveReactivity.getValue() + moveMotorPrev;
+ turnMotor = (turnSetValue-turnMotorPrev)*turnReactivity.getValue() + turnMotorPrev;
+
+ moveMotorPrev = moveMotor;
+ turnMotorPrev = turnMotor;
+
+ moveMotor = moveMotor * Math.pow(Math.abs(moveMotor), Drive.moveExponent.getValue() - 1);
+ turnMotor = turnMotor * Math.pow(Math.abs(turnMotor), Drive.turnExponent.getValue() - 1);
+
+ moveMotor = moveMotor * Drive.scaledMaxMove.getValue();
+// if(!Robot.oi.main.ls.get()){
+// turnMotor = turnMotor * Drive.scaledMaxTurn.getValue();
+// }
+ Robot.drive.normalDrive(moveMotor, turnMotor);
+ log();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ public boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ public void end() {
+ super.end();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ public void interrupted() {
+ super.interrupted();
+ end();
+ }
+
+ private void log(){
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/RecordAverageSpeed.java b/src/org/usfirst/frc/team3735/robot/commands/drive/RecordAverageSpeed.java
new file mode 100644
index 0000000..d9303f5
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/RecordAverageSpeed.java
@@ -0,0 +1,50 @@
+package org.usfirst.frc.team3735.robot.commands.drive;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.calc.RollingAverage;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class RecordAverageSpeed extends Command {
+
+ private RollingAverage roll;
+
+ public RecordAverageSpeed() {
+ roll = new RollingAverage(3){
+ @Override
+ public double get(){
+ return Robot.drive.getAverageSpeed();
+ }
+ };
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ roll.reset();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ roll.compute();
+ SmartDashboard.putNumber("Average Speed", roll.getAverage());
+
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/SendSDSpeed.java b/src/org/usfirst/frc/team3735/robot/commands/drive/SendSDSpeed.java
new file mode 100644
index 0000000..5abc43a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/SendSDSpeed.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team3735.robot.commands.drive;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class SendSDSpeed extends Command {
+
+ private static Setting speed;
+ public SendSDSpeed() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.drive);
+ speed = new Setting("Drive Speed Input", 0);
+
+ }
+
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.drive.setLeftRight(speed.getValue(), speed.getValue());
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/TurnTo.java b/src/org/usfirst/frc/team3735/robot/commands/drive/TurnTo.java
new file mode 100644
index 0000000..a0ece53
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/TurnTo.java
@@ -0,0 +1,130 @@
+package org.usfirst.frc.team3735.robot.commands.drive;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import org.usfirst.frc.team3735.robot.subsystems.Navigation;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.util.PIDCtrl;
+import org.usfirst.frc.team3735.robot.util.bases.VortxIterative.Side;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+import org.usfirst.frc.team3735.robot.util.settings.Func;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class TurnTo extends Command{
+
+ private double finishTime = .5;
+ private double timeOnTarget = 0;
+
+ Func getAngle;
+
+ public TurnTo(double angle) {
+ this(new Func(){
+ @Override
+ public double getValue() {
+ return angle;
+ }
+ });
+ }
+
+ /**
+ *
+ * @param angle
+ * @param flag true if a relative angle, false if relative to actual field position (Side independent)
+ */
+ public TurnTo(double angle, boolean flag) {
+ requires(Robot.drive);
+ requires(Robot.navigation);
+
+ if(flag) {
+ getAngle = new Func() {
+ @Override
+ public double getValue() {
+ return VortxMath.navLimit(Robot.navigation.getYaw() + angle);
+ }
+ };
+ }else {
+ getAngle = new Func() {
+ @Override
+ public double getValue() {
+ return Robot.side.equals(Side.Right) ? VortxMath.navLimit(angle + 180) : angle;
+ }
+ };
+ }
+
+ }
+
+ public TurnTo(Pipes p) {
+ this(new Func(){
+ @Override
+ public double getValue() {
+ Robot.vision.setMainHandler(p);
+ return VortxMath.continuousLimit(
+ Robot.navigation.getYaw() + (Robot.vision.getRelativeCX() * Robot.vision.dpp.getValue()),
+ -180, 180);
+ }
+ });
+ requires(Robot.vision);
+ }
+
+ public TurnTo(Location loc) {
+ this(new Func(){
+ @Override
+ public double getValue() {
+ return Robot.navigation.getAngleToLocationCorrected(loc);
+ }
+ });
+
+ }
+
+ public TurnTo(Func fun) {
+ requires(Robot.drive);
+ requires(Robot.navigation);
+ getAngle = fun;
+ }
+
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.navigation.getController().setSetpoint(getAngle.getValue());
+ Robot.navigation.getController().enable();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.navigation.getController().setIZone(Navigation.iZone.getValue());
+ Robot.navigation.getController().updateI(Navigation.actingI.getValue());
+
+ if(Robot.navigation.getController().onTarget()){
+ timeOnTarget += .02;
+ }else{
+ timeOnTarget = 0;
+ }
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return timeOnTarget >= finishTime;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.navigation.getController().disable();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/arcing/DriveMoveInCircleProfile.java b/src/org/usfirst/frc/team3735/robot/commands/drive/arcing/DriveMoveInCircleProfile.java
new file mode 100644
index 0000000..1405e3b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/arcing/DriveMoveInCircleProfile.java
@@ -0,0 +1,94 @@
+package org.usfirst.frc.team3735.robot.commands.drive.arcing;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveMoveDistanceProfile;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+import org.usfirst.frc.team3735.robot.triggers.HasReachedAngle;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.cmds.VortxCommand;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DriveMoveInCircleProfile extends VortxCommand {
+
+ private static final double DRIVE_WIDTH = 28; //in inches
+
+ private static final double MAX_SPEED = 800 * Constants.Drive.InchesPerTick /60.0; //inches per sec
+
+ private HasReachedAngle angler;
+ private DriveMoveDistanceProfile profile;
+ private Double distance;
+ private double radius;
+
+ private double leftMult;
+ private double rightMult;
+
+ /**
+ *
+ * @param radius the radius to turn in. Positive if turning right, negative if turning left,
+ * for both forward and back
+ * @param angle the angle we're trying to turn to
+ * @param isRelative indicates that the angle is relative to the starting angle on initialize
+ * @param v the cruise velocity for the profile
+ * @param a the acceleration of the profile
+ * @param exitV the exit velocity of the profile
+ */
+ public DriveMoveInCircleProfile(double radius, double angle, boolean isRelative, double v, double a, double exitV) {
+ this.radius = radius;
+ angler = new HasReachedAngle(angle, isRelative);
+ distance = new Double(Math.PI * radius * (angle/180.0));
+ if(Math.abs(v) > Math.abs(vMax(radius, DRIVE_WIDTH))){
+ v = Math.signum(v) * Math.abs(vMax(radius, DRIVE_WIDTH));
+ System.out.println("The Cruise Velocity was too great. Reducing to " + v);
+ }
+ profile = new DriveMoveDistanceProfile(distance, v, a, exitV);
+ addT(angler);
+
+ leftMult = 1.0 + (DRIVE_WIDTH/radius);
+ rightMult = 1.0 - (DRIVE_WIDTH/radius);
+
+ }
+
+
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ super.initialize();
+ if(angler.isRelative()){
+ distance = new Double(Math.PI * radius * (angler.degreesToGo()/180.0));
+ }
+ profile.initialize();
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ profile.updateProfile();
+ double s = profile.currentSpeed;
+ Robot.drive.setLeftRight(Drive.speedToPercent(s * leftMult), Drive.speedToPercent(s * rightMult));
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return profile.isFinished() || super.isFinished();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+
+ private double vMax(double r, double w){
+ return (MAX_SPEED * Math.abs(r)) / (Math.abs(r) + w);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveExp.java b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveExp.java
new file mode 100644
index 0000000..d027e43
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveExp.java
@@ -0,0 +1,89 @@
+package org.usfirst.frc.team3735.robot.commands.drive.movedistance;
+
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.cmds.VortxCommand;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class DriveExp extends VortxCommand {
+
+ private double moveSetValue;
+ private double turnSetValue;
+
+ private double moveMotor;
+ private double turnMotor;
+
+ private double moveMotorPrev;
+ private double turnMotorPrev;
+
+ private double moveReactivity = .2;
+ private double turnReactivity = .4;
+
+ public DriveExp(double move, double turn){
+ moveSetValue = move;
+ turnSetValue = turn;
+ requires(Robot.drive);
+ }
+
+ public DriveExp(double move, double turn, double mr, double tr){
+ moveSetValue = move;
+ turnSetValue = turn;
+ moveReactivity = mr;
+ turnReactivity = tr;
+ requires(Robot.drive);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ super.initialize();
+ Robot.drive.setupDriveForSpeedControl();
+ moveMotor = moveMotorPrev = Robot.drive.getCurrentPercent();
+ turnMotor = turnMotorPrev = 0;
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ public void execute() {
+ super.execute();
+
+ moveMotor = (moveSetValue-moveMotorPrev)*moveReactivity + moveMotorPrev;
+ turnMotor = (turnSetValue-turnMotorPrev)*turnReactivity + turnMotorPrev;
+
+ moveMotorPrev = moveMotor;
+ turnMotorPrev = turnMotor;
+
+
+ moveMotor = moveMotor * Math.pow(Math.abs(moveMotor), 1);
+ turnMotor = turnMotor * Math.pow(Math.abs(turnMotor), 1);
+
+ Robot.drive.normalDrive(moveMotor, turnMotor);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ public boolean isFinished() {
+ return super.isFinished();
+ }
+
+ // Called once after isFinished returns true
+ public void end() {
+ super.end();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ public void interrupted() {
+ super.interrupted();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveMoveDistanceProfile.java b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveMoveDistanceProfile.java
new file mode 100644
index 0000000..fa04b0c
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveMoveDistanceProfile.java
@@ -0,0 +1,149 @@
+package org.usfirst.frc.team3735.robot.commands.drive.movedistance;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+import org.usfirst.frc.team3735.robot.util.calc.TrapProfile;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.cmds.VortxCommand;
+
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class DriveMoveDistanceProfile extends VortxCommand {
+
+
+ private static final double FRAMERATE = 50;
+
+
+ private final double cruiseVelocity; //inches per second
+ private final double acceleration; //inches per second^2
+ private final double exitVelocity; //inches per second
+
+ public double currentSpeed;
+ private State state;
+ private double acc;
+
+ public HasMoved distHandler;
+
+
+ private enum State{
+ rampingUp,
+ cruising,
+ rampingDown
+ }
+
+ public DriveMoveDistanceProfile(Double distance, double v, double a, double exitV) {
+ if(Math.abs(exitV) > v){
+ exitV = v;
+ }
+ acceleration = Math.copySign(a, distance);
+ cruiseVelocity = Math.copySign(v, distance);
+ exitVelocity = Math.copySign(exitV, distance);
+
+ requires(Robot.drive);
+ distHandler = new HasMoved(distance);
+ addT(distHandler);
+ acc = acceleration/FRAMERATE;
+
+ }
+
+ // Called just before this Command runs the first time
+ public void initialize() {
+ super.initialize();
+ distHandler.initialize();
+ acc = acceleration/FRAMERATE;
+ currentSpeed = Robot.drive.getAverageSpeed();
+
+ state = State.rampingUp;
+ if(currentSpeed * distHandler.distance() < 0){
+ System.out.println("Profile Error: Robot is moving in the wrong direction");
+ }
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ public void execute() {
+ super.execute();
+ sendErrorReport();
+ updateProfile();
+
+ //Robot.drive.setLeftRightOutputs(speedToPercent(currentSpeed), speedToPercent(currentSpeed));
+ Robot.drive.arcadeDrive(Drive.speedToPercent(currentSpeed), 0);
+ }
+
+ public void updateProfile() {
+ if(needsToRampDown()){
+ System.out.println("Profile: Starting Ramp Down");
+ acc = calcAcceleration()/ FRAMERATE;
+ state = State.rampingDown;
+ }
+
+ switch(state){
+ case rampingUp:
+ currentSpeed += acc;
+ if(Math.abs(currentSpeed) > Math.abs(cruiseVelocity)){
+ currentSpeed = cruiseVelocity;
+ state = State.cruising;
+ }
+ break;
+ case cruising:
+
+ break;
+ case rampingDown:
+ currentSpeed += acc;
+ break;
+ }
+ }
+
+ private void sendErrorReport() {
+ SmartDashboard.putNumber("Profile currentSpeed: ", currentSpeed);
+ SmartDashboard.putNumber("Profile actual Speed", Robot.drive.getAverageSpeed());
+ SmartDashboard.putNumber("Profile Error", currentSpeed - Robot.drive.getAverageSpeed());
+ }
+
+ private double calcAcceleration() {
+ return (Math.pow(exitVelocity, 2) - Math.pow(Robot.drive.getAverageSpeed(), 2)) /
+ (2 * (distHandler.distanceToGo()));
+ }
+
+ private boolean needsToRampDown() {
+ return Math.abs(calcAcceleration()) > Math.abs(acceleration);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ public boolean isFinished() {
+ if(super.isFinished()){
+ System.out.println("Profile canceled by distance");
+ return true;
+ }else if(isProfileFinished()){
+ System.out.println("Profile canceled by itself");
+ return true;
+ }
+ return false;
+ }
+
+ private boolean isProfileFinished() {
+ if(cruiseVelocity > 0){
+ return (state == State.rampingDown) && (Robot.drive.getAverageSpeed() < exitVelocity);
+ }else{
+ return (state == State.rampingDown) && (Robot.drive.getAverageSpeed() > exitVelocity);
+ }
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveRaw.java b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveRaw.java
new file mode 100644
index 0000000..44a8ccb
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/DriveRaw.java
@@ -0,0 +1,48 @@
+package org.usfirst.frc.team3735.robot.commands.drive.movedistance;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DriveRaw extends Command {
+
+ private double move;
+ private double turn;
+
+ public DriveRaw(double move, double turn) {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.drive);
+ this.move = move;
+ this.turn = turn;
+
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.drive.setupDriveForSpeedControl();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.drive.normalDrive(move, turn);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.drive.normalDrive(0, 0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/Move.java b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/Move.java
new file mode 100644
index 0000000..b0af345
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/Move.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team3735.robot.commands.drive.movedistance;
+
+import org.usfirst.frc.team3735.robot.assists.NavxAssist;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * this is a shortcut class for simple moving a distance
+ */
+public class Move extends DriveExp {
+
+ public Move(double dist) {
+ this(dist, .7);
+ }
+
+ public Move(double dist, double spd) {
+ super(Math.signum(dist) * spd,0);
+ addA(new NavxAssist());
+ addT(new HasMoved(dist));
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/MoveSine.java b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/MoveSine.java
new file mode 100644
index 0000000..7017d18
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/MoveSine.java
@@ -0,0 +1,20 @@
+package org.usfirst.frc.team3735.robot.commands.drive.movedistance;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class MoveSine extends CommandGroup {
+ private double maxv = 170;
+ public MoveSine(double dist) {
+ double d = dist/2;
+ double vf;
+ vf = (d/250) * maxv;
+ if(vf > maxv) vf = maxv;
+ addSequential(new SinusoidalProfile(0,vf, d));
+ addSequential(new SinusoidalProfile(vf,0, d));
+
+
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/SinusoidalProfile.java b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/SinusoidalProfile.java
new file mode 100644
index 0000000..fd08d16
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/movedistance/SinusoidalProfile.java
@@ -0,0 +1,74 @@
+package org.usfirst.frc.team3735.robot.commands.drive.movedistance;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+import org.usfirst.frc.team3735.robot.triggers.HasReachedSpeed;
+import org.usfirst.frc.team3735.robot.triggers.TimeOut;
+import org.usfirst.frc.team3735.robot.util.cmds.VortxCommand;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class SinusoidalProfile extends VortxCommand {
+ double initialV;
+ double finalV;
+ double vd;
+ double dist;
+ double acc;
+ double scale;
+ public SinusoidalProfile(double vi, double vf, double d) {
+ requires(Robot.drive);
+ vd = vf-vi;
+ acc = (Math.PI/(4*d)) * (Math.pow(vd+vi, 2) - Math.pow(vi, 2));
+ if(acc/vd < 0) {
+ System.out.println("Sine Profile Error: changing dist");
+ d*= -1;
+ acc = (Math.PI/(4*d)) * (Math.pow(vd+vi, 2) - Math.pow(vi, 2));
+ }
+ initialV = vi;
+ finalV = vf;
+ dist = d;
+ scale = 2 * acc / vd;
+
+ addT(new HasMoved(d));
+ addT(new HasReachedSpeed(vf));
+ addT(new TimeOut(Math.PI/scale, this));
+
+ }
+
+ public double getV() {
+ double t = this.timeSinceInitialized();
+ return (vd/2) * (-Math.cos(t * scale) + 1) + initialV;
+ }
+
+ public double getDuration() {
+ return (Math.PI/2) * (vd/acc);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.drive.normalDrive(Drive.speedToPercent(getV()), 0);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return super.isFinished();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/BeginAtPathY.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/BeginAtPathY.java
new file mode 100644
index 0000000..6f5cf0d
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/BeginAtPathY.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class BeginAtPathY extends InstantCommand {
+ Location[] locs;
+ private double offset;
+
+ public BeginAtPathY(Location[] locs) {
+ this(locs,0);
+ }
+
+ public BeginAtPathY(Location[] locs, double offset) {
+ this.locs = locs;
+ this.offset = offset;
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ Robot.navigation.getPosition().y = locs[0].y + offset;
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/DriveResetPosition.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/DriveResetPosition.java
new file mode 100644
index 0000000..ecfe9e5
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/DriveResetPosition.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class DriveResetPosition extends InstantCommand {
+
+ public DriveResetPosition() {
+ super();
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ Robot.drive.resetEncodersPositions();
+
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPath.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPath.java
new file mode 100644
index 0000000..4654518
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPath.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class FollowPath extends CommandGroup {
+
+ public FollowPath(Location[] locs, boolean rev) {
+ for(Location l : locs) {
+ addSequential(new HitWaypoint(l, rev));
+ }
+ }
+ public FollowPath(Location[] locs) {
+ this(locs, false);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPathPredicted.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPathPredicted.java
new file mode 100644
index 0000000..2a4a064
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPathPredicted.java
@@ -0,0 +1,159 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Dms;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.triggers.HasPassedWaypoint;
+import org.usfirst.frc.team3735.robot.util.calc.DDxLimiter;
+import org.usfirst.frc.team3735.robot.util.calc.Range;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.profiling.Line;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+import org.usfirst.frc.team3735.robot.util.profiling.Position;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class FollowPathPredicted extends Command {
+
+ Location[] locs;
+ boolean rev;
+ double maxP = .5;
+
+ DDxLimiter dxdt; //inches/second
+ DDxLimiter dadt; //radians
+
+ double[] angleChanges;
+ private static Setting coeff = new Setting("Path Prediction coeff", .05);
+ private static Setting turnco = new Setting("Path Prediction turn coeff", 1);
+
+ HasPassedWaypoint cutoffLine;
+
+ double wantedDadx;
+ double wantedDxdt;
+ Line toFollow;
+ int targetIndex;
+ private boolean isDone = false;
+
+ public FollowPathPredicted(Location[] locs) {
+ this(locs, false);
+ }
+
+ public FollowPathPredicted(Location[] locs, boolean rev) {
+ this.locs = locs;
+ this.rev = rev;
+
+ //max speed is 173
+ dxdt = new DDxLimiter(0, new Range(200));
+ dadt = new DDxLimiter(0, new Range(200));
+
+ if(rev) {
+ maxP *= -1;
+ }
+ angleChanges = new double[locs.length];
+
+ //since bezier curves have dadt of 0 at endpoints, assign this to endpoints
+ angleChanges[0] = 0;
+ angleChanges[angleChanges.length-1] = 0;
+
+ //compute the rest
+ for(int i = 1; i < locs.length-1; i++) {
+ Location prev = locs[i-1];
+ Location next = locs[i+1];
+ Location current = locs[i];
+
+ angleChanges[i] = Math.atan2(current.y - prev.y, current.x - prev.x)
+ - Math.atan2(next.y - current.y, next.x - current.x);
+
+ }
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ dxdt.reset(Robot.drive.getAverageSpeed());
+ dadt.reset();
+
+ targetIndex = 0;
+ nextTarget();
+ isDone = false;
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ if(cutoffLine.get() || locs[targetIndex].distanceFrom(Robot.navigation.getPosition()) < 20) {
+ if(!nextTarget()) {
+ isDone = true;
+ return;
+ }
+
+ }
+
+ setControllerAngle();
+
+ double error = coeff.getValue() * toFollow.distanceFrom(Robot.navigation.getPosition());
+
+ error *= Math.signum(Robot.navigation.getController().getError());
+
+ double currDxdt = dxdt.feed(wantedDxdt);
+
+ move(currDxdt, dadt.feed(currDxdt * wantedDadx * turnco.getValue() + error));
+ }
+
+ public boolean nextTarget(){
+ targetIndex++;
+ if(targetIndex >= locs.length) {
+ return false;
+ }
+ wantedDadx = computeTurn(targetIndex);
+ wantedDxdt = Drive.percentToSpeed(maxP / (1 + Math.abs(wantedDadx)*Dms.Bot.DriveBase.HALFWIDTH));
+ cutoffLine = new HasPassedWaypoint(locs[targetIndex], locs[targetIndex-1]);
+ cutoffLine.initialize();
+ toFollow = new Line(locs[targetIndex], locs[targetIndex-1]);
+ return true;
+ }
+
+ //the index of the waypoint we're targeting
+ public double computeTurn(int index) {
+ double da = .5 * (angleChanges[index] + angleChanges[index-1]);
+ double dx = locs[index].distanceFrom(locs[index-1]);
+ return da/dx;
+ }
+
+ public void setControllerAngle() {
+ Position p = Robot.navigation.getPosition();
+ double targetAngle = Math.toDegrees(-Math.atan2(locs[targetIndex].y - p.y, locs[targetIndex].x - p.x));
+ if(rev) {
+ targetAngle = VortxMath.navLimit(targetAngle + 180);
+ }
+ Robot.navigation.getController().setSetpoint(targetAngle);
+ }
+
+ //moves from dx/dt and da/dt
+ public void move(double dx, double da) {
+ double left = dx + (da * Dms.Bot.DriveBase.HALFWIDTH);
+ double right = dx - (da * Dms.Bot.DriveBase.HALFWIDTH);
+ Robot.drive.setLeftRight(Drive.speedToPercent(left), Drive.speedToPercent(right));
+ System.out.println("Left: " + Drive.speedToPercent(left) + "Right: " + Drive.speedToPercent(right));
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return isDone;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPathTight.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPathTight.java
new file mode 100644
index 0000000..959b829
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/FollowPathTight.java
@@ -0,0 +1,27 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import java.awt.geom.Line2D;
+
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class FollowPathTight extends CommandGroup {
+
+ public FollowPathTight(Location[] locs) {
+ this(locs, false);
+ }
+
+ public FollowPathTight(Location[] locs, boolean rev) {
+ addSequential(new HitWaypoint(locs[0], rev));
+ for(int i = 1; i < locs.length; i++) {
+ addSequential(new HitWaypoint(locs[i], locs[i-1], rev));
+ }
+ }
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/GearCycle.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/GearCycle.java
new file mode 100644
index 0000000..98dff8a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/GearCycle.java
@@ -0,0 +1,29 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.commands.drive.ExpDrive;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveExp;
+import org.usfirst.frc.team3735.robot.commands.sequences.DriveAcquireGear;
+import org.usfirst.frc.team3735.robot.commands.sequences.DrivePlaceGear;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class GearCycle extends CommandGroup {
+
+ public GearCycle() {
+ //start a distance away from the airship
+ addSequential(new GoToCenterField());
+ addSequential(new FollowPath(Waypoints.toKeyLeft));
+ addSequential(new DriveAcquireGear());
+ addSequential(new DriveExp(-1,0).addT(new HasMoved(-60)),1);
+ addSequential(new GoToCenterField());
+ addSequential(new FollowPath(Waypoints.toTopGear));
+ addSequential(new DrivePlaceGear());
+ //end where started
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/GoToCenterField.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/GoToCenterField.java
new file mode 100644
index 0000000..091e367
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/GoToCenterField.java
@@ -0,0 +1,26 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+import org.usfirst.frc.team3735.robot.util.settings.Func;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class GoToCenterField extends CommandGroup {
+
+ public GoToCenterField() {
+ addSequential(new TurnTo(new Func() {
+ @Override
+ public double getValue() {
+ Location targetLocation = Robot.navigation.getClosestLocation(Waypoints.centerFieldSquares);
+ return Robot.navigation.getAngleToLocationCorrected(targetLocation);
+ }
+ }));
+ addSequential(new HitCenterField());
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/HitCenterField.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/HitCenterField.java
new file mode 100644
index 0000000..6a843c9
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/HitCenterField.java
@@ -0,0 +1,22 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+public class HitCenterField extends HitWaypoint{
+ public HitCenterField() {
+ super(Waypoints.center, false);
+ }
+
+ @Override
+ protected void initialize() {
+ target = Robot.navigation.getClosestLocation(Waypoints.centerFieldSquares);
+ super.initialize();
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/HitWaypoint.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/HitWaypoint.java
new file mode 100644
index 0000000..92b319c
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/HitWaypoint.java
@@ -0,0 +1,89 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.subsystems.Drive;
+import org.usfirst.frc.team3735.robot.triggers.HasPassedWaypoint;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.cmds.VortxCommand;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+import org.usfirst.frc.team3735.robot.util.profiling.Position;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+public class HitWaypoint extends VortxCommand{
+
+ public Location target;
+ private double speed;
+ private boolean isReversed;
+
+
+ //use 3.8 for navx assist!!!
+ private double maxSpeed = .5;
+ private double minSpeed = .2;
+
+ public HitWaypoint(Location target, boolean rev) {
+ this(target, null, rev);
+
+ }
+
+
+ public HitWaypoint(Location target, Location from, boolean rev) {
+ requires(Robot.drive);
+ requires(Robot.navigation);
+ this.target = target;
+ addT(new HasPassedWaypoint(target, from));
+ isReversed = rev;
+ }
+
+ @Override
+ protected void initialize() {
+ super.initialize();
+
+ }
+
+ private static Setting maxTurn = new Setting("Max Turn Profiling", 40);
+
+ @Override
+ protected void execute() {
+ super.execute();
+
+ setControllerAngle();
+
+ double err = Robot.navigation.getController().getError();
+ speed =((maxSpeed-minSpeed)*Math.exp(-Math.abs(.05*err))) + minSpeed;
+ if(isReversed) {
+ speed *= -1;
+ }
+ Robot.drive.setNavxAssist(VortxMath.limit(err, -1 * maxTurn.getValue(), maxTurn.getValue()));
+ Robot.drive.limitedDrive(speed, 0);
+
+
+ }
+
+ public void setControllerAngle() {
+ Position p = Robot.navigation.getPosition();
+ double targetAngle = Math.toDegrees(-Math.atan2(target.y - p.y, target.x - p.x));
+ if(isReversed) {
+ targetAngle = VortxMath.navLimit(targetAngle + 180);
+ }
+ Robot.navigation.getController().setSetpoint(targetAngle);
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return super.isFinished() || Robot.navigation.getPosition().distanceFrom(target) < 25;
+ }
+
+ @Override
+ protected void end() {
+ super.end();
+ Robot.drive.setNavxAssist(0);
+ }
+
+ @Override
+ protected void interrupted() {
+ super.interrupted();
+ end();
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/ResetPosition.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/ResetPosition.java
new file mode 100644
index 0000000..f0dd5ef
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/ResetPosition.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class ResetPosition extends InstantCommand {
+
+ public ResetPosition() {
+ this.setRunWhenDisabled(true);
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ Robot.navigation.resetPosition();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/TurnFollowPath.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/TurnFollowPath.java
new file mode 100644
index 0000000..7217a6b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/TurnFollowPath.java
@@ -0,0 +1,17 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class TurnFollowPath extends CommandGroup {
+
+ public TurnFollowPath(Location[] locs) {
+ addSequential(new TurnTo(locs[0]),1);
+ addSequential(new FollowPath(locs));
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/positions/ZeroYaw.java b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/ZeroYaw.java
new file mode 100644
index 0000000..6474857
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/positions/ZeroYaw.java
@@ -0,0 +1,21 @@
+package org.usfirst.frc.team3735.robot.commands.drive.positions;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class ZeroYaw extends InstantCommand {
+
+ public ZeroYaw() {
+
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ Robot.navigation.zeroYaw();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveAddSensitiveLeft.java b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveAddSensitiveLeft.java
new file mode 100644
index 0000000..8f4233b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveAddSensitiveLeft.java
@@ -0,0 +1,41 @@
+package org.usfirst.frc.team3735.robot.commands.drive.simple;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DriveAddSensitiveLeft extends Command {
+
+ public DriveAddSensitiveLeft() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.drive.setLeftTurn(Constants.Drive.lowSensitivityLeftTurn);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ Robot.drive.setLeftTurn(0);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveAddSensitiveRight.java b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveAddSensitiveRight.java
new file mode 100644
index 0000000..c5b622e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveAddSensitiveRight.java
@@ -0,0 +1,41 @@
+package org.usfirst.frc.team3735.robot.commands.drive.simple;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DriveAddSensitiveRight extends Command {
+
+ public DriveAddSensitiveRight() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.drive.setLeftTurn(Constants.Drive.lowSensitivityRightTurn);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ Robot.drive.setLeftTurn(0);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveBrake.java b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveBrake.java
new file mode 100644
index 0000000..9c8be81
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveBrake.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc.team3735.robot.commands.drive.simple;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class DriveBrake extends Command {
+ public DriveBrake() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.drive);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.drive.setLeftRight(0, 0);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ //Robot.drive.setEnableBrake(false);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveBrakeUntilStopped.java b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveBrakeUntilStopped.java
new file mode 100644
index 0000000..774d3b4
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveBrakeUntilStopped.java
@@ -0,0 +1,26 @@
+package org.usfirst.frc.team3735.robot.commands.drive.simple;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class DriveBrakeUntilStopped extends CommandGroup {
+
+ protected static final double SPEED_THRESH = .4;
+
+ public DriveBrakeUntilStopped() {
+ addSequential(new DriveBrake(){
+ @Override
+ public boolean isFinished(){
+ return super.isFinished() ||
+ (Math.abs(Robot.drive.getLeftSpeed()) < SPEED_THRESH &&
+ Math.abs(Robot.drive.getRightSpeed()) < SPEED_THRESH);
+
+ }
+ });
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveChangeBrakeMode.java b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveChangeBrakeMode.java
new file mode 100644
index 0000000..c42120b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/drive/simple/DriveChangeBrakeMode.java
@@ -0,0 +1,41 @@
+package org.usfirst.frc.team3735.robot.commands.drive.simple;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DriveChangeBrakeMode extends Command {
+
+ public DriveChangeBrakeMode() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.drive.setEnableBrake(true);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.drive.setEnableBrake(false);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeFeeding.java b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeFeeding.java
new file mode 100644
index 0000000..6d4f87c
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeFeeding.java
@@ -0,0 +1,46 @@
+
+package org.usfirst.frc.team3735.robot.commands.gearintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class GearIntakeFeeding extends Command {
+
+ public GearIntakeFeeding() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.gearIntake);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.gearIntake.liftDown();
+ Robot.gearIntake.setRollerSpeed(Constants.GearIntake.feedingVoltage);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.gearIntake.setRollerSpeed(Constants.GearIntake.feedingVoltage);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ Robot.gearIntake.liftUp();
+ Robot.gearIntake.setRollerSpeed(0);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeLiftDown.java b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeLiftDown.java
new file mode 100644
index 0000000..ba30353
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeLiftDown.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team3735.robot.commands.gearintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class GearIntakeLiftDown extends InstantCommand {
+
+ public GearIntakeLiftDown() {
+ super();
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ //requires(Robot.gearIntake);
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ Robot.gearIntake.liftDown();
+ System.out.println("Lift down init");
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeLiftUp.java b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeLiftUp.java
new file mode 100644
index 0000000..4d74952
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeLiftUp.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.commands.gearintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class GearIntakeLiftUp extends InstantCommand {
+
+ public GearIntakeLiftUp() {
+ super();
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ //requires(Robot.gearIntake);
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ Robot.gearIntake.liftUp();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersIn.java b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersIn.java
new file mode 100644
index 0000000..aa11f4e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersIn.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team3735.robot.commands.gearintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class GearIntakeRollersIn extends Command {
+
+ public GearIntakeRollersIn() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.cubeIntake);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.gearIntake.setRollerSpeed(Constants.GearIntake.rollerInVoltage);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.gearIntake.setRollerSpeed(0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersOff.java b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersOff.java
new file mode 100644
index 0000000..d121758
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersOff.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team3735.robot.commands.gearintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+/**
+ *
+ */
+public class GearIntakeRollersOff extends InstantCommand {
+
+ public GearIntakeRollersOff() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.gearIntake);
+ }
+
+ // Called once when the command executes
+ protected void initialize() {
+ Robot.gearIntake.setRollerSpeed(0);
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersOut.java b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersOut.java
new file mode 100644
index 0000000..d3a96c7
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeRollersOut.java
@@ -0,0 +1,43 @@
+package org.usfirst.frc.team3735.robot.commands.gearintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class GearIntakeRollersOut extends Command {
+
+ public GearIntakeRollersOut() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.gearIntake);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.gearIntake.setRollerSpeed(Constants.GearIntake.rollerOutVoltage);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.gearIntake.setRollerSpeed(0);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeSwitchUpDown.java b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeSwitchUpDown.java
new file mode 100644
index 0000000..3f04a61
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/gearintake/GearIntakeSwitchUpDown.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team3735.robot.commands.gearintake;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class GearIntakeSwitchUpDown extends Command {
+
+ public GearIntakeSwitchUpDown() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.gearIntake.switchLift();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return true;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/recorder/RecordSmartDashboardFile.java b/src/org/usfirst/frc/team3735/robot/commands/recorder/RecordSmartDashboardFile.java
new file mode 100644
index 0000000..8b14435
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/recorder/RecordSmartDashboardFile.java
@@ -0,0 +1,56 @@
+package org.usfirst.frc.team3735.robot.commands.recorder;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.recording.DataRecorder;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+import org.usfirst.frc.team3735.robot.util.settings.StringSetting;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class RecordSmartDashboardFile extends Command {
+
+ StringSetting fileName = new StringSetting("Recording File", "default file");
+
+ public RecordSmartDashboardFile() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+// requires(Robot.drive);
+// requires(Robot.ballIntake);
+// requires(Robot.gearIntake);
+// requires(Robot.navigation);
+// requires(Robot.scaler);
+// requires(Robot.shooter);
+// requires(Robot.ultra);
+// requires(Robot.vision);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ DataRecorder.startRecording(fileName.getValue());
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+
+ DataRecorder.recordData();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ DataRecorder.endRecording();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team3735/robot/commands/recorder/SendSmartDashboardFile.java b/src/org/usfirst/frc/team3735/robot/commands/recorder/SendSmartDashboardFile.java
new file mode 100644
index 0000000..c2a0b3c
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/recorder/SendSmartDashboardFile.java
@@ -0,0 +1,57 @@
+package org.usfirst.frc.team3735.robot.commands.recorder;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.recording.DataRecorder;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+import org.usfirst.frc.team3735.robot.util.settings.StringSetting;
+
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class SendSmartDashboardFile extends Command {
+
+ StringSetting fileName = new StringSetting("Sending File", "default file");
+
+ public SendSmartDashboardFile() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.drive);
+ requires(Robot.gearIntake);
+ requires(Robot.navigation);
+ requires(Robot.scaler);
+ requires(Robot.ultra);
+ requires(Robot.vision);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ DataRecorder.startSending(fileName.getValue());
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ if(!DataRecorder.outOfData()){
+ DataRecorder.sendData();
+ }
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return DataRecorder.outOfData();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ DataRecorder.endSending();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/scaler/ScalerOff.java b/src/org/usfirst/frc/team3735/robot/commands/scaler/ScalerOff.java
new file mode 100644
index 0000000..ceeb533
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/scaler/ScalerOff.java
@@ -0,0 +1,41 @@
+package org.usfirst.frc.team3735.robot.commands.scaler;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ScalerOff extends Command {
+
+ public ScalerOff() {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ requires(Robot.scaler);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.scaler.setPercent(0);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/scaler/ScalerUp.java b/src/org/usfirst/frc/team3735/robot/commands/scaler/ScalerUp.java
new file mode 100644
index 0000000..0992cd0
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/scaler/ScalerUp.java
@@ -0,0 +1,50 @@
+package org.usfirst.frc.team3735.robot.commands.scaler;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class ScalerUp extends Command {
+
+ private double speed;
+
+ public ScalerUp(double speed) {
+ // Use requires() here to declare subsystem dependencies
+ // eg. requires(chassis);
+ this.speed = speed;
+ requires(Robot.scaler);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.scaler.setCurrent(speed);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.scaler.setCurrent(speed);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return Robot.scaler.getOverloaded();
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.scaler.setCurrent(0);
+ System.out.println("This is the Scaler End method");
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ protected void interrupted() {
+ Robot.scaler.setPercent(0);
+ System.out.println("This is the Scaler Interrupted method");
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/commands/sequences/DriveAcquireGear.java b/src/org/usfirst/frc/team3735/robot/commands/sequences/DriveAcquireGear.java
new file mode 100644
index 0000000..31cfaaf
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/sequences/DriveAcquireGear.java
@@ -0,0 +1,44 @@
+package org.usfirst.frc.team3735.robot.commands.sequences;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.assists.NavxVisionAssist;
+import org.usfirst.frc.team3735.robot.commands.drive.ExpDrive;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveExp;
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeLiftDown;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class DriveAcquireGear extends CommandGroup {
+
+ private static final double WIDTH_THRESH = 175;
+
+ public DriveAcquireGear() {
+ addSequential(new GearIntakeLiftDown());
+ addSequential(new DriveExp(.7,0).addA(new NavxVisionAssist(Pipes.Gear)));
+
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return super.isFinished() ||
+ Robot.vision.getWidth() > WIDTH_THRESH ||
+ Robot.oi.isOverriddenByDrive();
+ }
+
+ @Override
+ protected void end() {
+ Robot.gearIntake.liftUp();
+ }
+
+ @Override
+ protected void interrupted() {
+ end();
+ }
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/commands/sequences/DriveGoToPeg.java b/src/org/usfirst/frc/team3735/robot/commands/sequences/DriveGoToPeg.java
new file mode 100644
index 0000000..42e3e4a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/sequences/DriveGoToPeg.java
@@ -0,0 +1,31 @@
+package org.usfirst.frc.team3735.robot.commands.sequences;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.commands.drive.TurnTo;
+
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeLiftDown;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class DriveGoToPeg extends CommandGroup {
+
+
+ public DriveGoToPeg() {
+ addSequential(new TurnTo(Pipes.Peg));
+ // addSequential(new DriveMoveDistanceExpVisionBumped(100, .7, Pipes.Peg));
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return super.isFinished() || Robot.oi.isOverriddenByDrive();
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/commands/sequences/DrivePlaceGear.java b/src/org/usfirst/frc/team3735/robot/commands/sequences/DrivePlaceGear.java
new file mode 100644
index 0000000..9c6dada
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/sequences/DrivePlaceGear.java
@@ -0,0 +1,31 @@
+package org.usfirst.frc.team3735.robot.commands.sequences;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeLiftDown;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class DrivePlaceGear extends CommandGroup {
+
+
+ public DrivePlaceGear() {
+ //addSequential(new DriveMoveDistanceExpVisionBumped(100, .7, Pipes.Peg));
+ addSequential(new GearIntakeDropOff());
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return super.isFinished() ||
+ Robot.oi.isOverriddenByDrive();
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/commands/sequences/GearIntakeDropOff.java b/src/org/usfirst/frc/team3735/robot/commands/sequences/GearIntakeDropOff.java
new file mode 100644
index 0000000..bd42bfd
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/commands/sequences/GearIntakeDropOff.java
@@ -0,0 +1,68 @@
+package org.usfirst.frc.team3735.robot.commands.sequences;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.assists.NavxAssist;
+import org.usfirst.frc.team3735.robot.commands.Wait;
+import org.usfirst.frc.team3735.robot.commands.drive.movedistance.DriveExp;
+
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeLiftDown;
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeLiftUp;
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeRollersOff;
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeRollersOut;
+import org.usfirst.frc.team3735.robot.triggers.HasMoved;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ *
+ */
+public class GearIntakeDropOff extends CommandGroup {
+
+ public GearIntakeDropOff() {
+// addSequential(new ExpDrive(.5,0),.1);
+// addParallel(new GearIntakeRollersOut(),Constants.GearIntake.dropOffTotalTime);
+// addSequential(new Wait(Constants.GearIntake.dropOffRollDelay));
+// addSequential(new GearIntakeLiftDown());
+// addParallel(new DriveMoveDistancePIDBroken(-37), 1);
+// //addSequential(new ExpDrive(Constants.GearIntake.dropOffDrivePercent,0),Constants.GearIntake.dropOffDriveTime);
+// addSequential(new Wait(Constants.GearIntake.dropOffJerkDelay));
+// addSequential(new GearIntakeLiftUp());
+// addSequential(new Wait(GearIntake.dropOffEndDelay));
+// //addSequential(new GearIntakeRollersOff());
+
+ //new code
+ addSequential(new DriveExp(.2,0),.1);
+
+ addParallel(new GearIntakeRollersOut());
+ addSequential(new Wait(.1));
+
+ addSequential(new GearIntakeLiftDown());
+ addSequential(new Wait(.1));
+
+ //addSequential(new DriveMoveDistanceExpNavx(-20, .6, null, .8),2);
+ addSequential(new DriveExp(-.7,0,.4,.4).addA(new NavxAssist()), 1.2);
+ //addSequential(new DriveExp(-.8,0),.7);
+ addSequential(new GearIntakeLiftUp());
+ addSequential(new GearIntakeRollersOff());
+
+
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return super.isFinished() || Robot.oi.isOverriddenByDrive();
+ }
+
+ @Override
+ protected void end() {
+ Robot.gearIntake.liftUp();
+ Robot.gearIntake.setRollerSpeed(0);
+ }
+
+ @Override
+ protected void interrupted() {
+ end();
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/ois/ChineseOI.java b/src/org/usfirst/frc/team3735/robot/ois/ChineseOI.java
new file mode 100644
index 0000000..4840316
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/ois/ChineseOI.java
@@ -0,0 +1,66 @@
+package org.usfirst.frc.team3735.robot.ois;
+
+import org.usfirst.frc.team3735.robot.commands.InterruptOperations;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveLeft;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveRight;
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeFeeding;
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeRollersIn;
+import org.usfirst.frc.team3735.robot.commands.gearintake.GearIntakeRollersOut;
+import org.usfirst.frc.team3735.robot.commands.scaler.ScalerOff;
+import org.usfirst.frc.team3735.robot.commands.scaler.ScalerUp;
+import org.usfirst.frc.team3735.robot.commands.sequences.DriveGoToPeg;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+//import org.usfirst.frc.team3735.robot.util.oi.DriveOI;
+import org.usfirst.frc.team3735.robot.util.oi.ChineseBoard;
+import org.usfirst.frc.team3735.robot.util.oi.DriveOI;
+
+public class ChineseOI implements DriveOI{
+
+ ChineseBoard board;
+
+ public ChineseOI() {
+ board = new ChineseBoard();
+ board.l.whenPressed(new DriveGoToPeg());
+ board.mJoyButton.whenPressed(new GearIntakeDropOff());
+ board.rJoyButton.whileHeld(new GearIntakeFeeding());
+ //main.x.whileHeld(new DriveAddSensitiveLeft());
+ //main.y.whileHeld(new DriveAddSensitiveRight());
+ //Co-Driver
+ board.lWhiteButton.whenPressed(new ScalerUp(1));
+ board.rWhiteButton.whenPressed(new ScalerOff());
+ board.a.whileHeld(new GearIntakeRollersIn());
+ board.b.whileHeld(new GearIntakeRollersOut());
+
+ board.j.whenPressed(new InterruptOperations());
+ }
+
+ public double getDriveMove() {
+ return board.getMiddleY();
+ }
+
+
+ public double getDriveTurn() {
+ return board.getRightX() * .7 + board.getMiddleX() * .3 + board.getRightZ() * .2;
+ }
+
+ @Override
+ public double getFODMag() {
+ return board.getLeftMagnitude();
+ }
+
+ @Override
+ public double getFODAngle() {
+ return board.getLeftAngle();
+ }
+
+ public boolean isOverriddenByDrive(){
+ return Math.abs(getDriveTurn()) > .1 || Math.abs(getDriveMove()) > .1;
+ }
+
+ @Override
+ public void log() {
+ // TODO Auto-generated method stub
+
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/ois/DemoOI.java b/src/org/usfirst/frc/team3735/robot/ois/DemoOI.java
new file mode 100644
index 0000000..882f422
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/ois/DemoOI.java
@@ -0,0 +1,101 @@
+package org.usfirst.frc.team3735.robot.ois;
+
+//import org.usfirst.frc.team3735.robot.commands.DriveTurnToAngleHyperbola;
+import org.usfirst.frc.team3735.robot.commands.*;
+import org.usfirst.frc.team3735.robot.commands.drive.*;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveLeft;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveRight;
+import org.usfirst.frc.team3735.robot.commands.gearintake.*;
+import org.usfirst.frc.team3735.robot.commands.scaler.*;
+import org.usfirst.frc.team3735.robot.commands.sequences.DriveGoToPeg;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+
+
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.util.oi.DriveOI;
+import org.usfirst.frc.team3735.robot.util.oi.JoystickPOVButton;
+import org.usfirst.frc.team3735.robot.util.oi.JoystickTriggerButton;
+import org.usfirst.frc.team3735.robot.util.oi.XboxController;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class DemoOI implements DriveOI{
+
+ public XboxController main;
+ public XboxController co;
+
+ private boolean driveEnabled = true;
+ public DemoOI() {
+
+ main = new XboxController(0);
+ co = new XboxController(1);
+
+ //Baby Driver
+
+ main.lt.whenPressed(new GearIntakeDropOff());
+ main.rt.whileHeld(new GearIntakeFeeding());
+
+ co.start.whenPressed(new InterruptOperations());
+
+ co.a.whileHeld(new GearIntakeRollersIn());
+ co.b.whileHeld(new GearIntakeRollersOut());
+
+
+
+ co.x.whenPressed(new InstantCommand(){
+ @Override
+ public void initialize(){
+ driveEnabled = false;
+ }
+ });
+ co.y.whenPressed(new InstantCommand(){
+ @Override
+ public void initialize(){
+ driveEnabled = true;
+ }
+ });
+
+ }
+
+ @Override
+ public double getDriveMove() {
+ return driveEnabled ? main.getLeftY() : 0;
+ }
+
+ @Override
+ public double getDriveTurn() {
+ return driveEnabled ? main.getRightX() : 0;
+ }
+
+ @Override
+ public double getFODMag() {
+ return 0;
+ }
+
+ @Override
+ public double getFODAngle(){
+ return 0;
+ }
+
+ @Override
+ public boolean isOverriddenByDrive(){
+ return Math.abs(getDriveMove()) > .1 || Math.abs(getDriveTurn()) > .1;
+ }
+
+ @Override
+ public void log() {
+// SmartDashboard.putNumber("right joystick angle", getMainRightAngle());
+// SmartDashboard.putNumber("right joystick magnitude",
+// getMainRightMagnitude());
+
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/ois/GTAOI.java b/src/org/usfirst/frc/team3735/robot/ois/GTAOI.java
new file mode 100644
index 0000000..a30502b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/ois/GTAOI.java
@@ -0,0 +1,100 @@
+package org.usfirst.frc.team3735.robot.ois;
+
+//import org.usfirst.frc.team3735.robot.commands.DriveTurnToAngleHyperbola;
+import org.usfirst.frc.team3735.robot.commands.*;
+import org.usfirst.frc.team3735.robot.commands.drive.ExpDrive;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveLeft;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveRight;
+import org.usfirst.frc.team3735.robot.commands.gearintake.*;
+import org.usfirst.frc.team3735.robot.commands.scaler.*;
+import org.usfirst.frc.team3735.robot.commands.cubeintake.*;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+import org.usfirst.frc.team3735.robot.util.oi.DriveOI;
+import org.usfirst.frc.team3735.robot.util.oi.XboxController;
+
+public class GTAOI implements DriveOI{
+
+ public XboxController main;
+ public XboxController co;
+
+ public GTAOI() {
+
+ main = new XboxController(0);
+
+ main.rb.get();
+ //Baby Driver
+ //main.pov180.whenPressed(new DriveGoToPeg());
+ main.rb.whileHeld(new DriveAddSensitiveRight());
+ main.lb.whileHeld(new DriveAddSensitiveLeft());
+
+
+ main.b.whenPressed(new GearIntakeDropOff());
+ main.a.whileHeld(new GearIntakeFeeding());
+ main.x.whenPressed(new GearIntakeLiftDown());
+ main.y.whenPressed(new GearIntakeLiftUp());
+
+ main.pov0.whileHeld(new CubeIntakePivotUp());
+ main.pov180.whileHeld(new CubeIntakePivotDown());
+ main.pov90.whileHeld(new CubeIntakeRollersIn());
+ main.pov270.whileHeld(new CubeIntakeRollersOut());
+// main.start.whenPressed(new ExpDrive());
+// main.x.whileHeld(new DriveAddSensitiveLeft());
+// main.y.whileHeld(new DriveAddSensitiveRight());
+
+// main.start.whenPressed(new DriveChangeToGearDirection());
+// main.back.whenPressed(new DriveChangeToBallDirection());
+
+ //CoDriver
+ //main.x.whenPressed(new ScalerUp(1));
+ //co..whenPressed(new ScalerOff());
+// co.a.whileHeld(new GearIntakeRollersIn());
+// co.b.whileHeld(new GearIntakeRollersOut());
+//
+//
+//
+// co.start.whenPressed(new InterruptOperations());
+//
+
+
+ }
+
+
+ public double getDriveMove() {
+ //System.out.println(main.getRightTrigger() - main.getLeftTrigger());
+ return (main.getRightTrigger() - main.getLeftTrigger());
+ //return main.getLeftY();
+ }
+
+ public double getDriveTurn() {
+ return main.getLeftX();
+ //return main.getRightX();
+ }
+
+ @Override
+ public double getFODMag() {
+ //return main.getRightMagnitude();
+ return 0;
+ }
+
+ public double getFODAngle(){
+ //return main.getRightAngle();
+ return 0;
+ }
+
+
+ public boolean isOverriddenByDrive(){
+ return Math.abs(getDriveMove()) > .4 || Math.abs(getDriveTurn()) > .4;
+ }
+
+
+ public void log() {
+// SmartDashboard.putNumber("right joystick angle", getMainRightAngle());
+// SmartDashboard.putNumber("right joystick magnitude",
+// getMainRightMagnitude());
+
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/ois/NormieOI.java b/src/org/usfirst/frc/team3735/robot/ois/NormieOI.java
new file mode 100644
index 0000000..e9b870e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/ois/NormieOI.java
@@ -0,0 +1,97 @@
+package org.usfirst.frc.team3735.robot.ois;
+
+//import org.usfirst.frc.team3735.robot.commands.DriveTurnToAngleHyperbola;
+import org.usfirst.frc.team3735.robot.commands.*;
+import org.usfirst.frc.team3735.robot.commands.drive.*;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveLeft;
+import org.usfirst.frc.team3735.robot.commands.drive.simple.DriveAddSensitiveRight;
+import org.usfirst.frc.team3735.robot.commands.gearintake.*;
+import org.usfirst.frc.team3735.robot.commands.scaler.*;
+import org.usfirst.frc.team3735.robot.commands.sequences.DriveGoToPeg;
+import org.usfirst.frc.team3735.robot.commands.sequences.GearIntakeDropOff;
+
+
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.util.oi.DriveOI;
+import org.usfirst.frc.team3735.robot.util.oi.JoystickPOVButton;
+import org.usfirst.frc.team3735.robot.util.oi.JoystickTriggerButton;
+import org.usfirst.frc.team3735.robot.util.oi.XboxController;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class NormieOI implements DriveOI{
+
+ public XboxController main;
+ public XboxController co;
+
+ public NormieOI() {
+
+ main = new XboxController(0);
+ co = new XboxController(1);
+
+ //Baby Driver
+ //main.pov180.whenPressed(new DriveGoToPeg());
+
+
+ main.x.whileHeld(new DriveAddSensitiveLeft());
+ main.y.whileHeld(new DriveAddSensitiveRight());
+
+ main.lt.whenPressed(new GearIntakeDropOff());
+ main.rt.whileHeld(new GearIntakeFeeding());
+
+ //CoDriver
+
+
+ co.start.whenPressed(new InterruptOperations());
+
+ co.y.whenPressed(new ScalerUp(1));
+ co.x.whenPressed(new ScalerOff());
+ co.a.whileHeld(new GearIntakeRollersIn());
+ co.b.whileHeld(new GearIntakeRollersOut());
+
+
+
+ }
+
+ @Override
+ public double getDriveMove() {
+ return main.getLeftY();
+ }
+
+ @Override
+ public double getDriveTurn() {
+ return main.getRightX();
+ }
+
+ @Override
+ public double getFODMag() {
+ return 0;
+ }
+
+ @Override
+ public double getFODAngle(){
+ return 0;
+ }
+
+ @Override
+ public boolean isOverriddenByDrive(){
+ return Math.abs(getDriveMove()) > .1 || Math.abs(getDriveTurn()) > .1;
+ }
+
+ @Override
+ public void log() {
+// SmartDashboard.putNumber("right joystick angle", getMainRightAngle());
+// SmartDashboard.putNumber("right joystick magnitude",
+// getMainRightMagnitude());
+
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/ois/drivee.java b/src/org/usfirst/frc/team3735/robot/ois/drivee.java
new file mode 100644
index 0000000..e9ac441
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/ois/drivee.java
@@ -0,0 +1,78 @@
+package org.usfirst.frc.team3735.robot.ois;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+
+public class drivee
+{
+
+ XboxController main = new XboxController(0);
+ WPI_TalonSRX Piv;
+ WPI_TalonSRX arm;
+ WPI_TalonSRX armm;
+
+ Timer t;
+
+ public drivee()
+ {
+ Piv = new WPI_TalonSRX(8);
+ arm = new WPI_TalonSRX(2);
+ armm = new WPI_TalonSRX(1);
+
+
+ //Pivot();
+ }
+
+ public void Pivot() //Cube intake
+ {
+ if(main.getAButton() == true)
+ {
+ Piv.set(.5);
+ t.delay(.2);
+ Piv.set(0);
+
+ }
+
+
+ if(main.getBButton() == true)
+ {
+ Piv.set(-.5);
+ t.delay(.2);
+ Piv.set(0);
+ }
+
+ }
+
+ public void Arm() //Cube intake
+ {
+ if(main.getYButton())
+ {
+ arm.set(-1);
+ armm.set(.8);
+
+ }
+ else if(main.getXButton())
+ {
+ arm.set(1);
+ armm.set(-.8);
+ }
+ else
+ {
+ arm.set(0);
+ armm.set(0);
+ }
+
+
+ }
+ public void ArmReverse() //shoot
+ {
+
+ }
+
+
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/GearPipeline.java b/src/org/usfirst/frc/team3735/robot/pipelines/GearPipeline.java
new file mode 100644
index 0000000..55799a5
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/GearPipeline.java
@@ -0,0 +1,435 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+
+/**
+* GearPipeline class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class GearPipeline implements VisionPipeline, ContoursOutputPipeline{
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat hsvThresholdOutput = new Mat();
+ private Mat cvErodeOutput = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 640;
+ double resizeImageHeight = 480;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step HSV_Threshold0:
+ Mat hsvThresholdInput = resizeImageOutput;
+ double[] hsvThresholdHue = {20.338983050847457, 29.197860962566843};
+ double[] hsvThresholdSaturation = {138.8700564971751, 255.0};
+ double[] hsvThresholdValue = {163.6440677966102, 255.0};
+ hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
+
+ // Step CV_erode0:
+ Mat cvErodeSrc = hsvThresholdOutput;
+ Mat cvErodeKernel = new Mat();
+ Point cvErodeAnchor = new Point(-1, -1);
+ double cvErodeIterations = 1.0;
+ int cvErodeBordertype = Core.BORDER_CONSTANT;
+ Scalar cvErodeBordervalue = new Scalar(-1);
+ cvErode(cvErodeSrc, cvErodeKernel, cvErodeAnchor, cvErodeIterations, cvErodeBordertype, cvErodeBordervalue, cvErodeOutput);
+
+ // Step Blur0:
+ Mat blurInput = cvErodeOutput;
+ BlurType blurType = BlurType.get("Gaussian Blur");
+ double blurRadius = 5.660377358490567;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 0.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 100000.0;
+ double filterContoursMinHeight = 10.0;
+ double filterContoursMaxHeight = 100000.0;
+ double[] filterContoursSolidity = {0.0, 100.0};
+ double filterContoursMaxVertices = 1000000.0;
+ double filterContoursMinVertices = 16.0;
+ double filterContoursMinRatio = 0.5;
+ double filterContoursMaxRatio = 2.0;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSV_Threshold.
+ * @return Mat output from HSV_Threshold.
+ */
+ public Mat hsvThresholdOutput() {
+ return hsvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErodeOutput() {
+ return cvErodeOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and value ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param val The min and max value
+ * @param output The image in which to store the output.
+ */
+ private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
+ Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
+ new Scalar(hue[1], sat[1], val[1]), out);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+ public double getCenterX(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else{
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ return r.x + (r.width / 2);
+ }
+ }
+ public double getCenterY(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ return r.y + (r.height / 2);
+ }else{
+ Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+ return r.y + (r.height / 2);
+ }
+ }
+ public double getHeight(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).height;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).height;
+ }
+ }
+ public double getWidth(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).width;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).width;
+ }
+ }
+
+ public double getArea(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).area();
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).area();
+ }
+ }
+ public MatOfPoint getLargestAreaMat(ArrayList arr) {
+ MatOfPoint ret = arr.get(0);
+ for(MatOfPoint m : arr){
+ if(Imgproc.boundingRect(m).area() > Imgproc.boundingRect(ret).area()){
+ ret = m;
+ }
+ }
+ return ret;
+ }
+
+
+
+
+}
+
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/PegPipeline.java b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipeline.java
new file mode 100644
index 0000000..ea81eba
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipeline.java
@@ -0,0 +1,520 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+
+/**
+* PegPipeline class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class PegPipeline implements VisionPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat cvExtractchannelOutput = new Mat();
+ private Mat hsvThresholdOutput = new Mat();
+ private Mat cvSubtractOutput = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step CV_extractChannel0:
+ Mat cvExtractchannelSrc = resizeImageOutput;
+ double cvExtractchannelChannel = 2.0;
+ cvExtractchannel(cvExtractchannelSrc, cvExtractchannelChannel, cvExtractchannelOutput);
+
+ // Step HSV_Threshold0:
+ Mat hsvThresholdInput = resizeImageOutput;
+ double[] hsvThresholdHue = {40.67796610169491, 80.53475935828878};
+ double[] hsvThresholdSaturation = {0.0, 61.81818181818181};
+ double[] hsvThresholdValue = {192.090395480226, 255.0};
+ hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
+
+ // Step CV_subtract0:
+ Mat cvSubtractSrc1 = hsvThresholdOutput;
+ Mat cvSubtractSrc2 = cvExtractchannelOutput;
+ cvSubtract(cvSubtractSrc1, cvSubtractSrc2, cvSubtractOutput);
+
+ // Step Blur0:
+ Mat blurInput = cvSubtractOutput;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 2.8301886792452833;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 0.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 10000.0;
+ double filterContoursMinHeight = 18.0;
+ double filterContoursMaxHeight = 10000.0;
+ double[] filterContoursSolidity = {0.0, 100.0};
+ double filterContoursMaxVertices = 20.0;
+ double filterContoursMinVertices = 0.0;
+ double filterContoursMinRatio = 0.4;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_extractChannel.
+ * @return Mat output from CV_extractChannel.
+ */
+ public Mat cvExtractchannelOutput() {
+ return cvExtractchannelOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSV_Threshold.
+ * @return Mat output from HSV_Threshold.
+ */
+ public Mat hsvThresholdOutput() {
+ return hsvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_subtract.
+ * @return Mat output from CV_subtract.
+ */
+ public Mat cvSubtractOutput() {
+ return cvSubtractOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Extracts given channel from an image.
+ * @param src the image to extract.
+ * @param channel zero indexed channel number to extract.
+ * @param dst output image.
+ */
+ private void cvExtractchannel(Mat src, double channel, Mat dst) {
+ Core.extractChannel(src, dst, (int)channel);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and value ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param val The min and max value
+ * @param output The image in which to store the output.
+ */
+ private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
+ Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
+ new Scalar(hue[1], sat[1], val[1]), out);
+ }
+
+ /**
+ * Subtracts the second Mat from the first.
+ * @param src1 the first Mat
+ * @param src2 the second Mat
+ * @param out the Mat that is the subtraction of the two Mats
+ */
+ private void cvSubtract(Mat src1, Mat src2, Mat out) {
+ Core.subtract(src1, src2, out);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+
+
+ public double getCenterX(){
+ for(MatOfPoint m : filterContoursOutput() ){
+ Rect w = Imgproc.boundingRect(m);
+ System.out.print(w.x + " ");
+ }
+ System.out.println();
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }else{
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }
+// }else if(filterContoursOutput().size() == 2){
+//// Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+//// Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+// Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+// Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+// Rect r = averageBoxes(r1,r2);
+// //System.out.println(r.x);
+// return r.x + (r.width/2);
+//// return .5 * ((r.x + (r.width / 2)) + (r2.x + (r2.width / 2)));
+// }else{
+// Rect r = getCentralRect(filterContoursOutput());
+// if(r != null){
+// //System.out.println(r.x);
+// return r.x + (r.width);
+// }else{
+// return -1;
+// }
+
+
+ }
+ public double getCenterY(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ return r.y + (r.height / 2);
+ }else{
+ Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+ Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+
+ return .5 * ((r.y + (r.height / 2)) + (r2.y + (r2.height / 2)));
+ }
+ }
+ public double getHeight(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).height;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).height;
+ }
+ }
+ public double getWidth(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).width;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).width;
+ }
+ }
+
+ public double getArea(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).area();
+ }else if(filterContoursOutput().size() == 2){
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ return r.area();
+ }else{
+ Rect r = getCentralRect(filterContoursOutput());
+ if(r != null){
+ return r.area();
+ }else{
+ return -1;
+ }
+ }
+ }
+ public MatOfPoint getLargestAreaMat(ArrayList arr) {
+ MatOfPoint ret = arr.get(0);
+ for(MatOfPoint m : arr){
+ if(Imgproc.boundingRect(m).area() > Imgproc.boundingRect(ret).area()){
+ ret = m;
+ }
+ }
+ return ret;
+ }
+ public MatOfPoint getSecondLargestAreaMat(ArrayList arr){
+ ArrayList loc = (ArrayList) arr.clone();
+ loc.remove(getLargestAreaMat(loc));
+ return getLargestAreaMat(loc);
+ }
+
+ public Rect getCentralRect(ArrayList arr){
+ for(MatOfPoint m : arr){
+ Rect r = Imgproc.boundingRect(m);
+ for(MatOfPoint n: arr){
+ Rect r2 = Imgproc.boundingRect(n);
+ if(VortxMath.isWithinThreshold(r.y, r2.y, 15)){
+ //VortxMath.isWithinThreshold(r.area(), r2.area(), 100))
+
+ return(averageBoxes(r,r2));
+ }
+ }
+ }
+ return null;
+ }
+
+ public Rect averageBoxes(Rect r, Rect r2){
+ Rect left;
+ Rect right;
+ if(r.x < r2.x){
+ left = r;
+ right = r2;
+ }else{
+ left = r2;
+ right = r;
+ }
+
+ return new Rect(left.x,
+ left.y,
+ (right.x + right.width) - left.x,
+ (left.height + right.height)/2);
+ }
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSN.java b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSN.java
new file mode 100644
index 0000000..d85c98a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSN.java
@@ -0,0 +1,532 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+
+/**
+* PegPipelineLSN class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class PegPipelineLSN implements VisionPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat cvExtractchannel0Output = new Mat();
+ private Mat hsvThresholdOutput = new Mat();
+ private Mat cvExtractchannel1Output = new Mat();
+ private Mat cvSubtractOutput = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step CV_extractChannel0:
+ Mat cvExtractchannel0Src = resizeImageOutput;
+ double cvExtractchannel0Channel = 0.0;
+ cvExtractchannel(cvExtractchannel0Src, cvExtractchannel0Channel, cvExtractchannel0Output);
+
+ // Step HSV_Threshold0:
+ Mat hsvThresholdInput = resizeImageOutput;
+ double[] hsvThresholdHue = {3.116207712087165, 109.17794652494501};
+ double[] hsvThresholdSaturation = {0.0, 14.090908684513797};
+ double[] hsvThresholdValue = {230.50847457627117, 255.0};
+ hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
+
+ // Step CV_extractChannel1:
+ Mat cvExtractchannel1Src = resizeImageOutput;
+ double cvExtractchannel1Channel = 0.0;
+ cvExtractchannel(cvExtractchannel1Src, cvExtractchannel1Channel, cvExtractchannel1Output);
+
+ // Step CV_subtract0:
+ Mat cvSubtractSrc1 = hsvThresholdOutput;
+ Mat cvSubtractSrc2 = cvExtractchannel1Output;
+ cvSubtract(cvSubtractSrc1, cvSubtractSrc2, cvSubtractOutput);
+
+ // Step Blur0:
+ Mat blurInput = cvSubtractOutput;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 2.8301886792452833;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 0.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 10000.0;
+ double filterContoursMinHeight = 18.0;
+ double filterContoursMaxHeight = 10000.0;
+ double[] filterContoursSolidity = {50.847457627118644, 100.0};
+ double filterContoursMaxVertices = 16.0;
+ double filterContoursMinVertices = 6.0;
+ double filterContoursMinRatio = 0.4;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_extractChannel.
+ * @return Mat output from CV_extractChannel.
+ */
+ public Mat cvExtractchannel0Output() {
+ return cvExtractchannel0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSV_Threshold.
+ * @return Mat output from HSV_Threshold.
+ */
+ public Mat hsvThresholdOutput() {
+ return hsvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_extractChannel.
+ * @return Mat output from CV_extractChannel.
+ */
+ public Mat cvExtractchannel1Output() {
+ return cvExtractchannel1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_subtract.
+ * @return Mat output from CV_subtract.
+ */
+ public Mat cvSubtractOutput() {
+ return cvSubtractOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and value ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param val The min and max value
+ * @param output The image in which to store the output.
+ */
+ private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
+ Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
+ new Scalar(hue[1], sat[1], val[1]), out);
+ }
+
+ /**
+ * Extracts given channel from an image.
+ * @param src the image to extract.
+ * @param channel zero indexed channel number to extract.
+ * @param dst output image.
+ */
+ private void cvExtractchannel(Mat src, double channel, Mat dst) {
+ Core.extractChannel(src, dst, (int)channel);
+ }
+
+ /**
+ * Subtracts the second Mat from the first.
+ * @param src1 the first Mat
+ * @param src2 the second Mat
+ * @param out the Mat that is the subtraction of the two Mats
+ */
+ private void cvSubtract(Mat src1, Mat src2, Mat out) {
+ Core.subtract(src1, src2, out);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+ public double getCenterX(){
+ for(MatOfPoint m : filterContoursOutput() ){
+ Rect w = Imgproc.boundingRect(m);
+ System.out.print(w.x + " ");
+ }
+ System.out.println();
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }else{
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }
+// }else if(filterContoursOutput().size() == 2){
+//// Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+//// Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+// Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+// Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+// Rect r = averageBoxes(r1,r2);
+// //System.out.println(r.x);
+// return r.x + (r.width/2);
+//// return .5 * ((r.x + (r.width / 2)) + (r2.x + (r2.width / 2)));
+// }else{
+// Rect r = getCentralRect(filterContoursOutput());
+// if(r != null){
+// //System.out.println(r.x);
+// return r.x + (r.width);
+// }else{
+// return -1;
+// }
+
+
+ }
+ public double getCenterY(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ return r.y + (r.height / 2);
+ }else{
+ Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+ Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+
+ return .5 * ((r.y + (r.height / 2)) + (r2.y + (r2.height / 2)));
+ }
+ }
+ public double getHeight(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).height;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).height;
+ }
+ }
+ public double getWidth(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).width;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).width;
+ }
+ }
+
+ public double getArea(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).area();
+ }else if(filterContoursOutput().size() == 2){
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ return r.area();
+ }else{
+ Rect r = getCentralRect(filterContoursOutput());
+ if(r != null){
+ return r.area();
+ }else{
+ return -1;
+ }
+ }
+ }
+ public MatOfPoint getLargestAreaMat(ArrayList arr) {
+ MatOfPoint ret = arr.get(0);
+ for(MatOfPoint m : arr){
+ if(Imgproc.boundingRect(m).area() > Imgproc.boundingRect(ret).area()){
+ ret = m;
+ }
+ }
+ return ret;
+ }
+ public MatOfPoint getSecondLargestAreaMat(ArrayList arr){
+ ArrayList loc = (ArrayList) arr.clone();
+ loc.remove(getLargestAreaMat(loc));
+ return getLargestAreaMat(loc);
+ }
+
+ public Rect getCentralRect(ArrayList arr){
+ for(MatOfPoint m : arr){
+ Rect r = Imgproc.boundingRect(m);
+ for(MatOfPoint n: arr){
+ Rect r2 = Imgproc.boundingRect(n);
+ if(VortxMath.isWithinThreshold(r.y, r2.y, 15)){
+ //VortxMath.isWithinThreshold(r.area(), r2.area(), 100))
+
+ return(averageBoxes(r,r2));
+ }
+ }
+ }
+ return null;
+ }
+
+ public Rect averageBoxes(Rect r, Rect r2){
+ Rect left;
+ Rect right;
+ if(r.x < r2.x){
+ left = r;
+ right = r2;
+ }else{
+ left = r2;
+ right = r;
+ }
+
+ return new Rect(left.x,
+ left.y,
+ (right.x + right.width) - left.x,
+ (left.height + right.height)/2);
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSN2Test2.java b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSN2Test2.java
new file mode 100644
index 0000000..b7ec9c0
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSN2Test2.java
@@ -0,0 +1,573 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+
+/**
+* PegPiplineLSNTest class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class PegPipelineLSN2Test2 implements VisionPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat cvErode0Output = new Mat();
+ private Mat cvDilateOutput = new Mat();
+ private Mat hsvThresholdOutput = new Mat();
+ private Mat cvErode1Output = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step CV_erode0:
+ Mat cvErode0Src = resizeImageOutput;
+ Mat cvErode0Kernel = new Mat();
+ Point cvErode0Anchor = new Point(-1, -1);
+ double cvErode0Iterations = 2.0;
+ int cvErode0Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode0Bordervalue = new Scalar(-1);
+ cvErode(cvErode0Src, cvErode0Kernel, cvErode0Anchor, cvErode0Iterations, cvErode0Bordertype, cvErode0Bordervalue, cvErode0Output);
+
+ // Step CV_dilate0:
+ Mat cvDilateSrc = cvErode0Output;
+ Mat cvDilateKernel = new Mat();
+ Point cvDilateAnchor = new Point(-1, -1);
+ double cvDilateIterations = 4.0;
+ int cvDilateBordertype = Core.BORDER_CONSTANT;
+ Scalar cvDilateBordervalue = new Scalar(-1);
+ cvDilate(cvDilateSrc, cvDilateKernel, cvDilateAnchor, cvDilateIterations, cvDilateBordertype, cvDilateBordervalue, cvDilateOutput);
+
+ // Step HSV_Threshold0:
+ Mat hsvThresholdInput = cvDilateOutput;
+ double[] hsvThresholdHue = {42.086330935251794, 93.99317406143345};
+ double[] hsvThresholdSaturation = {0.0, 17.430189264660275};
+ double[] hsvThresholdValue = {245.36870503597123, 255.0};
+ hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
+
+ // Step CV_erode1:
+ Mat cvErode1Src = hsvThresholdOutput;
+ Mat cvErode1Kernel = new Mat();
+ Point cvErode1Anchor = new Point(-1, -1);
+ double cvErode1Iterations = 1.0;
+ int cvErode1Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode1Bordervalue = new Scalar(-1);
+ cvErode(cvErode1Src, cvErode1Kernel, cvErode1Anchor, cvErode1Iterations, cvErode1Bordertype, cvErode1Bordervalue, cvErode1Output);
+
+ // Step Blur0:
+ Mat blurInput = cvErode1Output;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 8.455298889084817;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 0.0;
+ double filterContoursMaxWidth = 1000.0;
+ double filterContoursMinHeight = 0.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0, 100};
+ double filterContoursMaxVertices = 16.0;
+ double filterContoursMinVertices = 4.0;
+ double filterContoursMinRatio = 0.4;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode0Output() {
+ return cvErode0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_dilate.
+ * @return Mat output from CV_dilate.
+ */
+ public Mat cvDilateOutput() {
+ return cvDilateOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSV_Threshold.
+ * @return Mat output from HSV_Threshold.
+ */
+ public Mat hsvThresholdOutput() {
+ return hsvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode1Output() {
+ return cvErode1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Expands area of higher value in an image.
+ * @param src the Image to dilate.
+ * @param kernel the kernel for dilation.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the dilation.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvDilate(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null){
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.dilate(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and value ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param val The min and max value
+ * @param output The image in which to store the output.
+ */
+ private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
+ Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
+ new Scalar(hue[1], sat[1], val[1]), out);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+ public double getCenterX(){
+ for(MatOfPoint m : filterContoursOutput() ){
+ Rect w = Imgproc.boundingRect(m);
+ System.out.print(w.x + " ");
+ }
+ System.out.println();
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }else{
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }
+// }else if(filterContoursOutput().size() == 2){
+//// Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+//// Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+// Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+// Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+// Rect r = averageBoxes(r1,r2);
+// //System.out.println(r.x);
+// return r.x + (r.width/2);
+//// return .5 * ((r.x + (r.width / 2)) + (r2.x + (r2.width / 2)));
+// }else{
+// Rect r = getCentralRect(filterContoursOutput());
+// if(r != null){
+// //System.out.println(r.x);
+// return r.x + (r.width);
+// }else{
+// return -1;
+// }
+
+
+ }
+ public double getCenterY(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ return r.y + (r.height / 2);
+ }else{
+ Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+ Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+
+ return .5 * ((r.y + (r.height / 2)) + (r2.y + (r2.height / 2)));
+ }
+ }
+ public double getHeight(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).height;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).height;
+ }
+ }
+ public double getWidth(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).width;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).width;
+ }
+ }
+
+ public double getArea(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).area();
+ }else if(filterContoursOutput().size() == 2){
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ return r.area();
+ }else{
+ Rect r = getCentralRect(filterContoursOutput());
+ if(r != null){
+ return r.area();
+ }else{
+ return -1;
+ }
+ }
+ }
+ public MatOfPoint getLargestAreaMat(ArrayList arr) {
+ MatOfPoint ret = arr.get(0);
+ for(MatOfPoint m : arr){
+ if(Imgproc.boundingRect(m).area() > Imgproc.boundingRect(ret).area()){
+ ret = m;
+ }
+ }
+ return ret;
+ }
+ public MatOfPoint getSecondLargestAreaMat(ArrayList arr){
+ ArrayList loc = (ArrayList) arr.clone();
+ loc.remove(getLargestAreaMat(loc));
+ return getLargestAreaMat(loc);
+ }
+
+ public Rect getCentralRect(ArrayList arr){
+ for(MatOfPoint m : arr){
+ Rect r = Imgproc.boundingRect(m);
+ for(MatOfPoint n: arr){
+ Rect r2 = Imgproc.boundingRect(n);
+ if(VortxMath.isWithinThreshold(r.y, r2.y, 15)){
+ //VortxMath.isWithinThreshold(r.area(), r2.area(), 100))
+
+ return(averageBoxes(r,r2));
+ }
+ }
+ }
+ return null;
+ }
+
+ public Rect averageBoxes(Rect r, Rect r2){
+ Rect left;
+ Rect right;
+ if(r.x < r2.x){
+ left = r;
+ right = r2;
+ }else{
+ left = r2;
+ right = r;
+ }
+
+ return new Rect(left.x,
+ left.y,
+ (right.x + right.width) - left.x,
+ (left.height + right.height)/2);
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSNTest4.java b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSNTest4.java
new file mode 100644
index 0000000..94f998b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSNTest4.java
@@ -0,0 +1,643 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+
+/**
+* PegPipelineLSNTest class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class PegPipelineLSNTest4 implements VisionPipeline, ContoursOutputPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat cvDilateOutput = new Mat();
+ private Mat cvErode0Output = new Mat();
+ private Mat cvBitwiseAnd0Output = new Mat();
+ private Mat hsvThresholdOutput = new Mat();
+ private Mat hslThresholdOutput = new Mat();
+ private Mat cvBitwiseAnd1Output = new Mat();
+ private Mat cvErode1Output = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step CV_dilate0:
+ Mat cvDilateSrc = resizeImageOutput;
+ Mat cvDilateKernel = new Mat();
+ Point cvDilateAnchor = new Point(-1, -1);
+ double cvDilateIterations = 3.0;
+ int cvDilateBordertype = Core.BORDER_CONSTANT;
+ Scalar cvDilateBordervalue = new Scalar(-1);
+ cvDilate(cvDilateSrc, cvDilateKernel, cvDilateAnchor, cvDilateIterations, cvDilateBordertype, cvDilateBordervalue, cvDilateOutput);
+
+ // Step CV_erode0:
+ Mat cvErode0Src = cvDilateOutput;
+ Mat cvErode0Kernel = new Mat();
+ Point cvErode0Anchor = new Point(-1, -1);
+ double cvErode0Iterations = 3.0;
+ int cvErode0Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode0Bordervalue = new Scalar(-1);
+ cvErode(cvErode0Src, cvErode0Kernel, cvErode0Anchor, cvErode0Iterations, cvErode0Bordertype, cvErode0Bordervalue, cvErode0Output);
+
+ // Step CV_bitwise_and0:
+ Mat cvBitwiseAnd0Src1 = cvDilateOutput;
+ Mat cvBitwiseAnd0Src2 = cvErode0Output;
+ cvBitwiseAnd(cvBitwiseAnd0Src1, cvBitwiseAnd0Src2, cvBitwiseAnd0Output);
+
+ // Step HSV_Threshold0:
+ Mat hsvThresholdInput = cvBitwiseAnd0Output;
+ double[] hsvThresholdHue = {26.832093647116203, 90.78461791170082};
+ double[] hsvThresholdSaturation = {0.0, 46.97564381011482};
+ double[] hsvThresholdValue = {238.16531520546278, 255.0};
+ hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
+
+ // Step HSL_Threshold0:
+ Mat hslThresholdInput = cvBitwiseAnd0Output;
+ double[] hslThresholdHue = {0.0, 180.0};
+ double[] hslThresholdSaturation = {0.0, 255.0};
+ double[] hslThresholdLuminance = {247.31638418079098, 255.0};
+ hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput);
+
+ // Step CV_bitwise_and1:
+ Mat cvBitwiseAnd1Src1 = hsvThresholdOutput;
+ Mat cvBitwiseAnd1Src2 = hslThresholdOutput;
+ cvBitwiseAnd(cvBitwiseAnd1Src1, cvBitwiseAnd1Src2, cvBitwiseAnd1Output);
+
+ // Step CV_erode1:
+ Mat cvErode1Src = cvBitwiseAnd1Output;
+ Mat cvErode1Kernel = new Mat();
+ Point cvErode1Anchor = new Point(-1, -1);
+ double cvErode1Iterations = 1.0;
+ int cvErode1Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode1Bordervalue = new Scalar(-1);
+ cvErode(cvErode1Src, cvErode1Kernel, cvErode1Anchor, cvErode1Iterations, cvErode1Bordertype, cvErode1Bordervalue, cvErode1Output);
+
+ // Step Blur0:
+ Mat blurInput = cvErode1Output;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 4.681713983424438;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 0.0;
+ double filterContoursMaxWidth = 1000.0;
+ double filterContoursMinHeight = 0.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0, 100};
+ double filterContoursMaxVertices = 16.0;
+ double filterContoursMinVertices = 4.0;
+ double filterContoursMinRatio = 0.4;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_dilate.
+ * @return Mat output from CV_dilate.
+ */
+ public Mat cvDilateOutput() {
+ return cvDilateOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode0Output() {
+ return cvErode0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAnd0Output() {
+ return cvBitwiseAnd0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSV_Threshold.
+ * @return Mat output from HSV_Threshold.
+ */
+ public Mat hsvThresholdOutput() {
+ return hsvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSL_Threshold.
+ * @return Mat output from HSL_Threshold.
+ */
+ public Mat hslThresholdOutput() {
+ return hslThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAnd1Output() {
+ return cvBitwiseAnd1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode1Output() {
+ return cvErode1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Expands area of higher value in an image.
+ * @param src the Image to dilate.
+ * @param kernel the kernel for dilation.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the dilation.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvDilate(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null){
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.dilate(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and value ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param val The min and max value
+ * @param output The image in which to store the output.
+ */
+ private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
+ Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
+ new Scalar(hue[1], sat[1], val[1]), out);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and luminance ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param lum The min and max luminance
+ * @param output The image in which to store the output.
+ */
+ private void hslThreshold(Mat input, double[] hue, double[] sat, double[] lum,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HLS);
+ Core.inRange(out, new Scalar(hue[0], lum[0], sat[0]),
+ new Scalar(hue[1], lum[1], sat[1]), out);
+ }
+
+ /**
+ * Computes the per channel and of two images.
+ * @param src1 The first image to use.
+ * @param src2 The second image to use.
+ * @param dst the result image when the and is performed.
+ */
+ private void cvBitwiseAnd(Mat src1, Mat src2, Mat dst) {
+ Core.bitwise_and(src1, src2, dst);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+ public double getCenterX(){
+ for(MatOfPoint m : filterContoursOutput() ){
+ Rect w = Imgproc.boundingRect(m);
+ System.out.print(w.x + " ");
+ }
+ System.out.println();
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }else{
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }
+// }else if(filterContoursOutput().size() == 2){
+//// Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+//// Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+// Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+// Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+// Rect r = averageBoxes(r1,r2);
+// //System.out.println(r.x);
+// return r.x + (r.width/2);
+//// return .5 * ((r.x + (r.width / 2)) + (r2.x + (r2.width / 2)));
+// }else{
+// Rect r = getCentralRect(filterContoursOutput());
+// if(r != null){
+// //System.out.println(r.x);
+// return r.x + (r.width);
+// }else{
+// return -1;
+// }
+
+
+ }
+ public double getCenterY(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ return r.y + (r.height / 2);
+ }else{
+ Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+ Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+
+ return .5 * ((r.y + (r.height / 2)) + (r2.y + (r2.height / 2)));
+ }
+ }
+ public double getHeight(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).height;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).height;
+ }
+ }
+ public double getWidth(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).width;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).width;
+ }
+ }
+
+ public double getArea(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).area();
+ }else if(filterContoursOutput().size() == 2){
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ return r.area();
+ }else{
+ Rect r = getCentralRect(filterContoursOutput());
+ if(r != null){
+ return r.area();
+ }else{
+ return -1;
+ }
+ }
+ }
+ public MatOfPoint getLargestAreaMat(ArrayList arr) {
+ MatOfPoint ret = arr.get(0);
+ for(MatOfPoint m : arr){
+ if(Imgproc.boundingRect(m).area() > Imgproc.boundingRect(ret).area()){
+ ret = m;
+ }
+ }
+ return ret;
+ }
+ public MatOfPoint getSecondLargestAreaMat(ArrayList arr){
+ ArrayList loc = (ArrayList) arr.clone();
+ loc.remove(getLargestAreaMat(loc));
+ return getLargestAreaMat(loc);
+ }
+
+ public Rect getCentralRect(ArrayList arr){
+ for(MatOfPoint m : arr){
+ Rect r = Imgproc.boundingRect(m);
+ for(MatOfPoint n: arr){
+ Rect r2 = Imgproc.boundingRect(n);
+ if(VortxMath.isWithinThreshold(r.y, r2.y, 15)){
+ //VortxMath.isWithinThreshold(r.area(), r2.area(), 100))
+
+ return(averageBoxes(r,r2));
+ }
+ }
+ }
+ return null;
+ }
+
+ public Rect averageBoxes(Rect r, Rect r2){
+ Rect left;
+ Rect right;
+ if(r.x < r2.x){
+ left = r;
+ right = r2;
+ }else{
+ left = r2;
+ right = r;
+ }
+
+ return new Rect(left.x,
+ left.y,
+ (right.x + right.width) - left.x,
+ (left.height + right.height)/2);
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSNTest5.java b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSNTest5.java
new file mode 100644
index 0000000..3705cda
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/PegPipelineLSNTest5.java
@@ -0,0 +1,644 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+
+/**
+* PegPipelineTest class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class PegPipelineLSNTest5 implements VisionPipeline, ContoursOutputPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat cvDilateOutput = new Mat();
+ private Mat cvErode0Output = new Mat();
+ private Mat cvBitwiseAnd0Output = new Mat();
+ private Mat hsvThresholdOutput = new Mat();
+ private Mat hslThresholdOutput = new Mat();
+ private Mat cvBitwiseAnd1Output = new Mat();
+ private Mat cvErode1Output = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step CV_dilate0:
+ Mat cvDilateSrc = resizeImageOutput;
+ Mat cvDilateKernel = new Mat();
+ Point cvDilateAnchor = new Point(-1, -1);
+ double cvDilateIterations = 3.0;
+ int cvDilateBordertype = Core.BORDER_CONSTANT;
+ Scalar cvDilateBordervalue = new Scalar(-1);
+ cvDilate(cvDilateSrc, cvDilateKernel, cvDilateAnchor, cvDilateIterations, cvDilateBordertype, cvDilateBordervalue, cvDilateOutput);
+
+ // Step CV_erode0:
+ Mat cvErode0Src = cvDilateOutput;
+ Mat cvErode0Kernel = new Mat();
+ Point cvErode0Anchor = new Point(-1, -1);
+ double cvErode0Iterations = 3.0;
+ int cvErode0Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode0Bordervalue = new Scalar(-1);
+ cvErode(cvErode0Src, cvErode0Kernel, cvErode0Anchor, cvErode0Iterations, cvErode0Bordertype, cvErode0Bordervalue, cvErode0Output);
+
+ // Step CV_bitwise_and0:
+ Mat cvBitwiseAnd0Src1 = cvDilateOutput;
+ Mat cvBitwiseAnd0Src2 = cvErode0Output;
+ cvBitwiseAnd(cvBitwiseAnd0Src1, cvBitwiseAnd0Src2, cvBitwiseAnd0Output);
+
+ // Step HSV_Threshold0:
+ Mat hsvThresholdInput = cvBitwiseAnd0Output;
+ double[] hsvThresholdHue = {26.832093647116203, 90.78461791170082};
+ double[] hsvThresholdSaturation = {0.0, 46.97564381011482};
+ double[] hsvThresholdValue = {238.16531520546278, 255.0};
+ hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
+
+ // Step HSL_Threshold0:
+ Mat hslThresholdInput = cvBitwiseAnd0Output;
+ double[] hslThresholdHue = {0.0, 180.0};
+ double[] hslThresholdSaturation = {0.0, 255.0};
+ double[] hslThresholdLuminance = {247.31638418079098, 255.0};
+ hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput);
+
+ // Step CV_bitwise_and1:
+ Mat cvBitwiseAnd1Src1 = hsvThresholdOutput;
+ Mat cvBitwiseAnd1Src2 = hslThresholdOutput;
+ cvBitwiseAnd(cvBitwiseAnd1Src1, cvBitwiseAnd1Src2, cvBitwiseAnd1Output);
+
+ // Step CV_erode1:
+ Mat cvErode1Src = cvBitwiseAnd1Output;
+ Mat cvErode1Kernel = new Mat();
+ Point cvErode1Anchor = new Point(-1, -1);
+ double cvErode1Iterations = 1.0;
+ int cvErode1Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode1Bordervalue = new Scalar(-1);
+ cvErode(cvErode1Src, cvErode1Kernel, cvErode1Anchor, cvErode1Iterations, cvErode1Bordertype, cvErode1Bordervalue, cvErode1Output);
+
+ // Step Blur0:
+ Mat blurInput = cvErode1Output;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 4.681713983424438;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 1000.0;
+ double filterContoursMinHeight = 18.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0.0, 100};
+ double filterContoursMaxVertices = 22.0;
+ double filterContoursMinVertices = 2.0;
+ double filterContoursMinRatio = 0.3;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_dilate.
+ * @return Mat output from CV_dilate.
+ */
+ public Mat cvDilateOutput() {
+ return cvDilateOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode0Output() {
+ return cvErode0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAnd0Output() {
+ return cvBitwiseAnd0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSV_Threshold.
+ * @return Mat output from HSV_Threshold.
+ */
+ public Mat hsvThresholdOutput() {
+ return hsvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSL_Threshold.
+ * @return Mat output from HSL_Threshold.
+ */
+ public Mat hslThresholdOutput() {
+ return hslThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAnd1Output() {
+ return cvBitwiseAnd1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode1Output() {
+ return cvErode1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Expands area of higher value in an image.
+ * @param src the Image to dilate.
+ * @param kernel the kernel for dilation.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the dilation.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvDilate(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null){
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.dilate(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and value ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param val The min and max value
+ * @param output The image in which to store the output.
+ */
+ private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
+ Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
+ new Scalar(hue[1], sat[1], val[1]), out);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and luminance ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param lum The min and max luminance
+ * @param output The image in which to store the output.
+ */
+ private void hslThreshold(Mat input, double[] hue, double[] sat, double[] lum,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HLS);
+ Core.inRange(out, new Scalar(hue[0], lum[0], sat[0]),
+ new Scalar(hue[1], lum[1], sat[1]), out);
+ }
+
+ /**
+ * Computes the per channel and of two images.
+ * @param src1 The first image to use.
+ * @param src2 The second image to use.
+ * @param dst the result image when the and is performed.
+ */
+ private void cvBitwiseAnd(Mat src1, Mat src2, Mat dst) {
+ Core.bitwise_and(src1, src2, dst);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+ public double getCenterX(){
+ for(MatOfPoint m : filterContoursOutput() ){
+ Rect w = Imgproc.boundingRect(m);
+ System.out.print(w.x + " ");
+ }
+ System.out.println();
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }else{
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ //System.out.println(r.x);
+ return r.x + (r.width/2);
+ }
+// }else if(filterContoursOutput().size() == 2){
+//// Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+//// Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+// Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+// Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+// Rect r = averageBoxes(r1,r2);
+// //System.out.println(r.x);
+// return r.x + (r.width/2);
+//// return .5 * ((r.x + (r.width / 2)) + (r2.x + (r2.width / 2)));
+// }else{
+// Rect r = getCentralRect(filterContoursOutput());
+// if(r != null){
+// //System.out.println(r.x);
+// return r.x + (r.width);
+// }else{
+// return -1;
+// }
+
+
+ }
+ public double getCenterY(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ Rect r= Imgproc.boundingRect(filterContoursOutput().get(0));
+ return r.y + (r.height / 2);
+ }else{
+ Rect r= Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput()));
+ Rect r2= Imgproc.boundingRect(getSecondLargestAreaMat(filterContoursOutput()));
+
+ return .5 * ((r.y + (r.height / 2)) + (r2.y + (r2.height / 2)));
+ }
+ }
+ public double getHeight(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).height;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).height;
+ }
+ }
+ public double getWidth(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).width;
+ }else{
+ return Imgproc.boundingRect(getLargestAreaMat(filterContoursOutput())).width;
+ }
+ }
+
+ public double getArea(){
+ if(filterContoursOutput().isEmpty()){
+ return -1;
+ }else if(filterContoursOutput().size() == 1){
+ return Imgproc.boundingRect(filterContoursOutput().get(0)).area();
+ }else if(filterContoursOutput().size() == 2){
+ Rect r1= Imgproc.boundingRect(filterContoursOutput().get(0));
+ Rect r2= Imgproc.boundingRect(filterContoursOutput().get(1));
+ Rect r = averageBoxes(r1,r2);
+ return r.area();
+ }else{
+ Rect r = getCentralRect(filterContoursOutput());
+ if(r != null){
+ return r.area();
+ }else{
+ return -1;
+ }
+ }
+ }
+ public MatOfPoint getLargestAreaMat(ArrayList arr) {
+ MatOfPoint ret = arr.get(0);
+ for(MatOfPoint m : arr){
+ if(Imgproc.boundingRect(m).area() > Imgproc.boundingRect(ret).area()){
+ ret = m;
+ }
+ }
+ return ret;
+ }
+ public MatOfPoint getSecondLargestAreaMat(ArrayList arr){
+ ArrayList loc = (ArrayList) arr.clone();
+ loc.remove(getLargestAreaMat(loc));
+ return getLargestAreaMat(loc);
+ }
+
+ public Rect getCentralRect(ArrayList arr){
+ for(MatOfPoint m : arr){
+ Rect r = Imgproc.boundingRect(m);
+ for(MatOfPoint n: arr){
+ Rect r2 = Imgproc.boundingRect(n);
+ if(VortxMath.isWithinThreshold(r.y, r2.y, 15)){
+ //VortxMath.isWithinThreshold(r.area(), r2.area(), 100))
+
+ return(averageBoxes(r,r2));
+ }
+ }
+ }
+ return null;
+ }
+
+ public Rect averageBoxes(Rect r, Rect r2){
+ Rect left;
+ Rect right;
+ if(r.x < r2.x){
+ left = r;
+ right = r2;
+ }else{
+ left = r2;
+ right = r;
+ }
+
+ return new Rect(left.x,
+ left.y,
+ (right.x + right.width) - left.x,
+ (left.height + right.height)/2);
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/StickyNotePipeline.java b/src/org/usfirst/frc/team3735/robot/pipelines/StickyNotePipeline.java
new file mode 100644
index 0000000..0b1fcf6
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/StickyNotePipeline.java
@@ -0,0 +1,374 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+
+/**
+* StickyNotePipeline class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class StickyNotePipeline implements VisionPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat hsvThresholdOutput = new Mat();
+ private Mat cvErodeOutput = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_LINEAR;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step HSV_Threshold0:
+ Mat hsvThresholdInput = resizeImageOutput;
+ double[] hsvThresholdHue = {10.0, 40.0};
+ double[] hsvThresholdSaturation = {214.80225988700565, 255.0};
+ double[] hsvThresholdValue = {110.0, 255.0};
+ hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
+
+ // Step CV_erode0:
+ Mat cvErodeSrc = hsvThresholdOutput;
+ Mat cvErodeKernel = new Mat();
+ Point cvErodeAnchor = new Point(-1, -1);
+ double cvErodeIterations = 1.0;
+ int cvErodeBordertype = Core.BORDER_CONSTANT;
+ Scalar cvErodeBordervalue = new Scalar(-1);
+ cvErode(cvErodeSrc, cvErodeKernel, cvErodeAnchor, cvErodeIterations, cvErodeBordertype, cvErodeBordervalue, cvErodeOutput);
+
+ // Step Blur0:
+ Mat blurInput = cvErodeOutput;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 1.8867924528301896;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 500.0;
+ double filterContoursMinHeight = 10.0;
+ double filterContoursMaxHeight = 500.0;
+ double[] filterContoursSolidity = {54.61393596986818, 100.0};
+ double filterContoursMaxVertices = 1000000.0;
+ double filterContoursMinVertices = 0.0;
+ double filterContoursMinRatio = 0.0;
+ double filterContoursMaxRatio = 8.0;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSV_Threshold.
+ * @return Mat output from HSV_Threshold.
+ */
+ public Mat hsvThresholdOutput() {
+ return hsvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErodeOutput() {
+ return cvErodeOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and value ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param val The min and max value
+ * @param output The image in which to store the output.
+ */
+ private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
+ Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
+ new Scalar(hue[1], sat[1], val[1]), out);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/Test4Modded.java b/src/org/usfirst/frc/team3735/robot/pipelines/Test4Modded.java
new file mode 100644
index 0000000..773bc3b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/Test4Modded.java
@@ -0,0 +1,459 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+
+/**
+* TestModded class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class Test4Modded implements VisionPipeline, ContoursOutputPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat cvDilateOutput = new Mat();
+ private Mat cvErode0Output = new Mat();
+ private Mat cvBitwiseAndOutput = new Mat();
+ private Mat hslThresholdOutput = new Mat();
+ private Mat cvErode1Output = new Mat();
+ private Mat blurOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step CV_dilate0:
+ Mat cvDilateSrc = resizeImageOutput;
+ Mat cvDilateKernel = new Mat();
+ Point cvDilateAnchor = new Point(-1, -1);
+ double cvDilateIterations = 3.0;
+ int cvDilateBordertype = Core.BORDER_CONSTANT;
+ Scalar cvDilateBordervalue = new Scalar(-1);
+ cvDilate(cvDilateSrc, cvDilateKernel, cvDilateAnchor, cvDilateIterations, cvDilateBordertype, cvDilateBordervalue, cvDilateOutput);
+
+ // Step CV_erode0:
+ Mat cvErode0Src = cvDilateOutput;
+ Mat cvErode0Kernel = new Mat();
+ Point cvErode0Anchor = new Point(-1, -1);
+ double cvErode0Iterations = 3.0;
+ int cvErode0Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode0Bordervalue = new Scalar(-1);
+ cvErode(cvErode0Src, cvErode0Kernel, cvErode0Anchor, cvErode0Iterations, cvErode0Bordertype, cvErode0Bordervalue, cvErode0Output);
+
+ // Step CV_bitwise_and0:
+ Mat cvBitwiseAndSrc1 = cvDilateOutput;
+ Mat cvBitwiseAndSrc2 = cvErode0Output;
+ cvBitwiseAnd(cvBitwiseAndSrc1, cvBitwiseAndSrc2, cvBitwiseAndOutput);
+
+ // Step HSL_Threshold0:
+ Mat hslThresholdInput = cvBitwiseAndOutput;
+ double[] hslThresholdHue = {0.0, 180.0};
+ double[] hslThresholdSaturation = {0.0, 255.0};
+ double[] hslThresholdLuminance = {247.31638418079098, 255.0};
+ hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput);
+
+ // Step CV_erode1:
+ Mat cvErode1Src = hslThresholdOutput;
+ Mat cvErode1Kernel = new Mat();
+ Point cvErode1Anchor = new Point(-1, -1);
+ double cvErode1Iterations = 1.0;
+ int cvErode1Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode1Bordervalue = new Scalar(-1);
+ cvErode(cvErode1Src, cvErode1Kernel, cvErode1Anchor, cvErode1Iterations, cvErode1Bordertype, cvErode1Bordervalue, cvErode1Output);
+
+ // Step Blur0:
+ Mat blurInput = cvErode1Output;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 4.681713983424438;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = blurOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 1000.0;
+ double filterContoursMinHeight = 18.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0.0, 100};
+ double filterContoursMaxVertices = 22.0;
+ double filterContoursMinVertices = 2.0;
+ double filterContoursMinRatio = 0.3;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_dilate.
+ * @return Mat output from CV_dilate.
+ */
+ public Mat cvDilateOutput() {
+ return cvDilateOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode0Output() {
+ return cvErode0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAndOutput() {
+ return cvBitwiseAndOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSL_Threshold.
+ * @return Mat output from HSL_Threshold.
+ */
+ public Mat hslThresholdOutput() {
+ return hslThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode1Output() {
+ return cvErode1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Expands area of higher value in an image.
+ * @param src the Image to dilate.
+ * @param kernel the kernel for dilation.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the dilation.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvDilate(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null){
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.dilate(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * Computes the per channel and of two images.
+ * @param src1 The first image to use.
+ * @param src2 The second image to use.
+ * @param dst the result image when the and is performed.
+ */
+ private void cvBitwiseAnd(Mat src1, Mat src2, Mat dst) {
+ Core.bitwise_and(src1, src2, dst);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and luminance ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param lum The min and max luminance
+ * @param output The image in which to store the output.
+ */
+ private void hslThreshold(Mat input, double[] hue, double[] sat, double[] lum,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HLS);
+ Core.inRange(out, new Scalar(hue[0], lum[0], sat[0]),
+ new Scalar(hue[1], lum[1], sat[1]), out);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/Test4ModdedMore.java b/src/org/usfirst/frc/team3735/robot/pipelines/Test4ModdedMore.java
new file mode 100644
index 0000000..cda28ad
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/Test4ModdedMore.java
@@ -0,0 +1,490 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+
+/**
+* TestModdedMore class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class Test4ModdedMore implements VisionPipeline, ContoursOutputPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat cvDilateOutput = new Mat();
+ private Mat cvErode0Output = new Mat();
+ private Mat cvBitwiseAndOutput = new Mat();
+ private Mat hslThresholdOutput = new Mat();
+ private Mat cvErode1Output = new Mat();
+ private Mat blurOutput = new Mat();
+ private Mat cvThresholdOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step Resize_Image0:
+ Mat resizeImageInput = source0;
+ double resizeImageWidth = 320.0;
+ double resizeImageHeight = 240.0;
+ int resizeImageInterpolation = Imgproc.INTER_CUBIC;
+ resizeImage(resizeImageInput, resizeImageWidth, resizeImageHeight, resizeImageInterpolation, resizeImageOutput);
+
+ // Step CV_dilate0:
+ Mat cvDilateSrc = resizeImageOutput;
+ Mat cvDilateKernel = new Mat();
+ Point cvDilateAnchor = new Point(-1, -1);
+ double cvDilateIterations = 3.0;
+ int cvDilateBordertype = Core.BORDER_CONSTANT;
+ Scalar cvDilateBordervalue = new Scalar(-1);
+ cvDilate(cvDilateSrc, cvDilateKernel, cvDilateAnchor, cvDilateIterations, cvDilateBordertype, cvDilateBordervalue, cvDilateOutput);
+
+ // Step CV_erode0:
+ Mat cvErode0Src = cvDilateOutput;
+ Mat cvErode0Kernel = new Mat();
+ Point cvErode0Anchor = new Point(-1, -1);
+ double cvErode0Iterations = 3.0;
+ int cvErode0Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode0Bordervalue = new Scalar(-1);
+ cvErode(cvErode0Src, cvErode0Kernel, cvErode0Anchor, cvErode0Iterations, cvErode0Bordertype, cvErode0Bordervalue, cvErode0Output);
+
+ // Step CV_bitwise_and0:
+ Mat cvBitwiseAndSrc1 = cvDilateOutput;
+ Mat cvBitwiseAndSrc2 = cvErode0Output;
+ cvBitwiseAnd(cvBitwiseAndSrc1, cvBitwiseAndSrc2, cvBitwiseAndOutput);
+
+ // Step HSL_Threshold0:
+ Mat hslThresholdInput = cvBitwiseAndOutput;
+ double[] hslThresholdHue = {0.0, 180.0};
+ double[] hslThresholdSaturation = {0.0, 255.0};
+ double[] hslThresholdLuminance = {247.31638418079098, 255.0};
+ hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput);
+
+ // Step CV_erode1:
+ Mat cvErode1Src = hslThresholdOutput;
+ Mat cvErode1Kernel = new Mat();
+ Point cvErode1Anchor = new Point(-1, -1);
+ double cvErode1Iterations = 2.0;
+ int cvErode1Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode1Bordervalue = new Scalar(-1);
+ cvErode(cvErode1Src, cvErode1Kernel, cvErode1Anchor, cvErode1Iterations, cvErode1Bordertype, cvErode1Bordervalue, cvErode1Output);
+
+ // Step Blur0:
+ Mat blurInput = cvErode1Output;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 4.681713983424442;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step CV_Threshold0:
+ Mat cvThresholdSrc = blurOutput;
+ double cvThresholdThresh = 50.0;
+ double cvThresholdMaxval = 1000.0;
+ int cvThresholdType = Imgproc.THRESH_BINARY;
+ cvThreshold(cvThresholdSrc, cvThresholdThresh, cvThresholdMaxval, cvThresholdType, cvThresholdOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = cvThresholdOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 50.0;
+ double filterContoursMinHeight = 18.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0.0, 100};
+ double filterContoursMaxVertices = 22.0;
+ double filterContoursMinVertices = 2.0;
+ double filterContoursMinRatio = 0.3;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a Resize_Image.
+ * @return Mat output from Resize_Image.
+ */
+ public Mat resizeImageOutput() {
+ return resizeImageOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_dilate.
+ * @return Mat output from CV_dilate.
+ */
+ public Mat cvDilateOutput() {
+ return cvDilateOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode0Output() {
+ return cvErode0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAndOutput() {
+ return cvBitwiseAndOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSL_Threshold.
+ * @return Mat output from HSL_Threshold.
+ */
+ public Mat hslThresholdOutput() {
+ return hslThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode1Output() {
+ return cvErode1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_Threshold.
+ * @return Mat output from CV_Threshold.
+ */
+ public Mat cvThresholdOutput() {
+ return cvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Scales and image to an exact size.
+ * @param input The image on which to perform the Resize.
+ * @param width The width of the output in pixels.
+ * @param height The height of the output in pixels.
+ * @param interpolation The type of interpolation.
+ * @param output The image in which to store the output.
+ */
+ private void resizeImage(Mat input, double width, double height,
+ int interpolation, Mat output) {
+ Imgproc.resize(input, output, new Size(width, height), 0.0, 0.0, interpolation);
+ }
+
+ /**
+ * Expands area of higher value in an image.
+ * @param src the Image to dilate.
+ * @param kernel the kernel for dilation.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the dilation.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvDilate(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null){
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.dilate(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * Computes the per channel and of two images.
+ * @param src1 The first image to use.
+ * @param src2 The second image to use.
+ * @param dst the result image when the and is performed.
+ */
+ private void cvBitwiseAnd(Mat src1, Mat src2, Mat dst) {
+ Core.bitwise_and(src1, src2, dst);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and luminance ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param lum The min and max luminance
+ * @param output The image in which to store the output.
+ */
+ private void hslThreshold(Mat input, double[] hue, double[] sat, double[] lum,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HLS);
+ Core.inRange(out, new Scalar(hue[0], lum[0], sat[0]),
+ new Scalar(hue[1], lum[1], sat[1]), out);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Apply a fixed-level threshold to each array element in an image.
+ * @param src Image to threshold.
+ * @param threshold threshold value.
+ * @param maxVal Maximum value for THRES_BINARY and THRES_BINARY_INV
+ * @param type Type of threshold to appy.
+ * @param dst output Image.
+ */
+ private void cvThreshold(Mat src, double threshold, double maxVal, int type,
+ Mat dst) {
+ Imgproc.threshold(src, dst, threshold, maxVal, type);
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/Test4ModdedMorem.java b/src/org/usfirst/frc/team3735/robot/pipelines/Test4ModdedMorem.java
new file mode 100644
index 0000000..c65b6c7
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/Test4ModdedMorem.java
@@ -0,0 +1,459 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+
+/**
+* TestModdedMorem class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class Test4ModdedMorem implements VisionPipeline, ContoursOutputPipeline {
+
+ //Outputs
+ private Mat cvDilateOutput = new Mat();
+ private Mat cvErode0Output = new Mat();
+ private Mat cvBitwiseAndOutput = new Mat();
+ private Mat hslThresholdOutput = new Mat();
+ private Mat cvErode1Output = new Mat();
+ private Mat blurOutput = new Mat();
+ private Mat cvThresholdOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step CV_dilate0:
+ Mat cvDilateSrc = source0;
+ Mat cvDilateKernel = new Mat();
+ Point cvDilateAnchor = new Point(-1, -1);
+ double cvDilateIterations = 3.0;
+ int cvDilateBordertype = Core.BORDER_CONSTANT;
+ Scalar cvDilateBordervalue = new Scalar(-1);
+ cvDilate(cvDilateSrc, cvDilateKernel, cvDilateAnchor, cvDilateIterations, cvDilateBordertype, cvDilateBordervalue, cvDilateOutput);
+
+ // Step CV_erode0:
+ Mat cvErode0Src = cvDilateOutput;
+ Mat cvErode0Kernel = new Mat();
+ Point cvErode0Anchor = new Point(-1, -1);
+ double cvErode0Iterations = 3.0;
+ int cvErode0Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode0Bordervalue = new Scalar(-1);
+ cvErode(cvErode0Src, cvErode0Kernel, cvErode0Anchor, cvErode0Iterations, cvErode0Bordertype, cvErode0Bordervalue, cvErode0Output);
+
+ // Step CV_bitwise_and0:
+ Mat cvBitwiseAndSrc1 = cvDilateOutput;
+ Mat cvBitwiseAndSrc2 = cvErode0Output;
+ cvBitwiseAnd(cvBitwiseAndSrc1, cvBitwiseAndSrc2, cvBitwiseAndOutput);
+
+ // Step HSL_Threshold0:
+ Mat hslThresholdInput = cvBitwiseAndOutput;
+ double[] hslThresholdHue = {0.0, 180.0};
+ double[] hslThresholdSaturation = {0.0, 255.0};
+ double[] hslThresholdLuminance = {247.31638418079098, 255.0};
+ hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput);
+
+ // Step CV_erode1:
+ Mat cvErode1Src = hslThresholdOutput;
+ Mat cvErode1Kernel = new Mat();
+ Point cvErode1Anchor = new Point(-1, -1);
+ double cvErode1Iterations = 2.0;
+ int cvErode1Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode1Bordervalue = new Scalar(-1);
+ cvErode(cvErode1Src, cvErode1Kernel, cvErode1Anchor, cvErode1Iterations, cvErode1Bordertype, cvErode1Bordervalue, cvErode1Output);
+
+ // Step Blur0:
+ Mat blurInput = cvErode1Output;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 4.681713983424442;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step CV_Threshold0:
+ Mat cvThresholdSrc = blurOutput;
+ double cvThresholdThresh = 50.0;
+ double cvThresholdMaxval = 1000.0;
+ int cvThresholdType = Imgproc.THRESH_BINARY;
+ cvThreshold(cvThresholdSrc, cvThresholdThresh, cvThresholdMaxval, cvThresholdType, cvThresholdOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = cvThresholdOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 50.0;
+ double filterContoursMinHeight = 18.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0.0, 100};
+ double filterContoursMaxVertices = 22.0;
+ double filterContoursMinVertices = 2.0;
+ double filterContoursMinRatio = 0.3;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_dilate.
+ * @return Mat output from CV_dilate.
+ */
+ public Mat cvDilateOutput() {
+ return cvDilateOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode0Output() {
+ return cvErode0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAndOutput() {
+ return cvBitwiseAndOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSL_Threshold.
+ * @return Mat output from HSL_Threshold.
+ */
+ public Mat hslThresholdOutput() {
+ return hslThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode1Output() {
+ return cvErode1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_Threshold.
+ * @return Mat output from CV_Threshold.
+ */
+ public Mat cvThresholdOutput() {
+ return cvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Expands area of higher value in an image.
+ * @param src the Image to dilate.
+ * @param kernel the kernel for dilation.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the dilation.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvDilate(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null){
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.dilate(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * Computes the per channel and of two images.
+ * @param src1 The first image to use.
+ * @param src2 The second image to use.
+ * @param dst the result image when the and is performed.
+ */
+ private void cvBitwiseAnd(Mat src1, Mat src2, Mat dst) {
+ Core.bitwise_and(src1, src2, dst);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and luminance ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param lum The min and max luminance
+ * @param output The image in which to store the output.
+ */
+ private void hslThreshold(Mat input, double[] hue, double[] sat, double[] lum,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HLS);
+ Core.inRange(out, new Scalar(hue[0], lum[0], sat[0]),
+ new Scalar(hue[1], lum[1], sat[1]), out);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Apply a fixed-level threshold to each array element in an image.
+ * @param src Image to threshold.
+ * @param threshold threshold value.
+ * @param maxVal Maximum value for THRES_BINARY and THRES_BINARY_INV
+ * @param type Type of threshold to appy.
+ * @param dst output Image.
+ */
+ private void cvThreshold(Mat src, double threshold, double maxVal, int type,
+ Mat dst) {
+ Imgproc.threshold(src, dst, threshold, maxVal, type);
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/pipelines/TestModdedMoreTRI.java b/src/org/usfirst/frc/team3735/robot/pipelines/TestModdedMoreTRI.java
new file mode 100644
index 0000000..accabf5
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/pipelines/TestModdedMoreTRI.java
@@ -0,0 +1,466 @@
+package org.usfirst.frc.team3735.robot.pipelines;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.stream.Collectors;
+import java.util.HashMap;
+
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+
+import org.opencv.core.*;
+import org.opencv.core.Core.*;
+import org.opencv.features2d.FeatureDetector;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.*;
+import org.opencv.objdetect.*;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+
+/**
+* GearPipeline class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+/**
+* TestModdedMoreTRI class.
+*
+*
An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class TestModdedMoreTRI implements VisionPipeline, ContoursOutputPipeline {
+
+ //Outputs
+ private Mat cvDilateOutput = new Mat();
+ private Mat cvErode0Output = new Mat();
+ private Mat cvBitwiseAndOutput = new Mat();
+ private Mat hslThresholdOutput = new Mat();
+ private Mat cvErode1Output = new Mat();
+ private Mat blurOutput = new Mat();
+ private Mat cvThresholdOutput = new Mat();
+ private ArrayList findContoursOutput = new ArrayList();
+ private ArrayList convexHullsOutput = new ArrayList();
+ private ArrayList filterContoursOutput = new ArrayList();
+
+ static {
+ System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
+ }
+
+ /**
+ * This is the primary method that runs the entire pipeline and updates the outputs.
+ */
+ @Override public void process(Mat source0) {
+ // Step CV_dilate0:
+ Mat cvDilateSrc = source0;
+ Mat cvDilateKernel = new Mat();
+ Point cvDilateAnchor = new Point(-1, -1);
+ double cvDilateIterations = 3.0;
+ int cvDilateBordertype = Core.BORDER_CONSTANT;
+ Scalar cvDilateBordervalue = new Scalar(-1);
+ cvDilate(cvDilateSrc, cvDilateKernel, cvDilateAnchor, cvDilateIterations, cvDilateBordertype, cvDilateBordervalue, cvDilateOutput);
+
+ // Step CV_erode0:
+ Mat cvErode0Src = cvDilateOutput;
+ Mat cvErode0Kernel = new Mat();
+ Point cvErode0Anchor = new Point(-1, -1);
+ double cvErode0Iterations = 3.0;
+ int cvErode0Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode0Bordervalue = new Scalar(-1);
+ cvErode(cvErode0Src, cvErode0Kernel, cvErode0Anchor, cvErode0Iterations, cvErode0Bordertype, cvErode0Bordervalue, cvErode0Output);
+
+ // Step CV_bitwise_and0:
+ Mat cvBitwiseAndSrc1 = cvDilateOutput;
+ Mat cvBitwiseAndSrc2 = cvErode0Output;
+ cvBitwiseAnd(cvBitwiseAndSrc1, cvBitwiseAndSrc2, cvBitwiseAndOutput);
+
+ // Step HSL_Threshold0:
+ Mat hslThresholdInput = cvBitwiseAndOutput;
+ double[] hslThresholdHue = {0.0, 131.8716577540107};
+ double[] hslThresholdSaturation = {0.0, 255.0};
+ double[] hslThresholdLuminance = {252.1186440677966, 255.0};
+ hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput);
+
+ // Step CV_erode1:
+ Mat cvErode1Src = hslThresholdOutput;
+ Mat cvErode1Kernel = new Mat();
+ Point cvErode1Anchor = new Point(-1, -1);
+ double cvErode1Iterations = 2.0;
+ int cvErode1Bordertype = Core.BORDER_CONSTANT;
+ Scalar cvErode1Bordervalue = new Scalar(-1);
+ cvErode(cvErode1Src, cvErode1Kernel, cvErode1Anchor, cvErode1Iterations, cvErode1Bordertype, cvErode1Bordervalue, cvErode1Output);
+
+ // Step Blur0:
+ Mat blurInput = cvErode1Output;
+ BlurType blurType = BlurType.get("Box Blur");
+ double blurRadius = 4.681713983424442;
+ blur(blurInput, blurType, blurRadius, blurOutput);
+
+ // Step CV_Threshold0:
+ Mat cvThresholdSrc = blurOutput;
+ double cvThresholdThresh = 50.0;
+ double cvThresholdMaxval = 1000.0;
+ int cvThresholdType = Imgproc.THRESH_BINARY;
+ cvThreshold(cvThresholdSrc, cvThresholdThresh, cvThresholdMaxval, cvThresholdType, cvThresholdOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = cvThresholdOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Convex_Hulls0:
+ ArrayList convexHullsContours = findContoursOutput;
+ convexHulls(convexHullsContours, convexHullsOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = convexHullsOutput;
+ double filterContoursMinArea = 100.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 10.0;
+ double filterContoursMaxWidth = 50.0;
+ double filterContoursMinHeight = 18.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0.0, 100};
+ double filterContoursMaxVertices = 22.0;
+ double filterContoursMinVertices = 2.0;
+ double filterContoursMinRatio = 0.3;
+ double filterContoursMaxRatio = 0.7;
+ filterContours(filterContoursContours, filterContoursMinArea, filterContoursMinPerimeter, filterContoursMinWidth, filterContoursMaxWidth, filterContoursMinHeight, filterContoursMaxHeight, filterContoursSolidity, filterContoursMaxVertices, filterContoursMinVertices, filterContoursMinRatio, filterContoursMaxRatio, filterContoursOutput);
+
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_dilate.
+ * @return Mat output from CV_dilate.
+ */
+ public Mat cvDilateOutput() {
+ return cvDilateOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode0Output() {
+ return cvErode0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_bitwise_and.
+ * @return Mat output from CV_bitwise_and.
+ */
+ public Mat cvBitwiseAndOutput() {
+ return cvBitwiseAndOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a HSL_Threshold.
+ * @return Mat output from HSL_Threshold.
+ */
+ public Mat hslThresholdOutput() {
+ return hslThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_erode.
+ * @return Mat output from CV_erode.
+ */
+ public Mat cvErode1Output() {
+ return cvErode1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Blur.
+ * @return Mat output from Blur.
+ */
+ public Mat blurOutput() {
+ return blurOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a CV_Threshold.
+ * @return Mat output from CV_Threshold.
+ */
+ public Mat cvThresholdOutput() {
+ return cvThresholdOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContoursOutput() {
+ return findContoursOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Convex_Hulls.
+ * @return ArrayList output from Convex_Hulls.
+ */
+ public ArrayList convexHullsOutput() {
+ return convexHullsOutput;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Expands area of higher value in an image.
+ * @param src the Image to dilate.
+ * @param kernel the kernel for dilation.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the dilation.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvDilate(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null){
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.dilate(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * Computes the per channel and of two images.
+ * @param src1 The first image to use.
+ * @param src2 The second image to use.
+ * @param dst the result image when the and is performed.
+ */
+ private void cvBitwiseAnd(Mat src1, Mat src2, Mat dst) {
+ Core.bitwise_and(src1, src2, dst);
+ }
+
+ /**
+ * Segment an image based on hue, saturation, and luminance ranges.
+ *
+ * @param input The image on which to perform the HSL threshold.
+ * @param hue The min and max hue
+ * @param sat The min and max saturation
+ * @param lum The min and max luminance
+ * @param output The image in which to store the output.
+ */
+ private void hslThreshold(Mat input, double[] hue, double[] sat, double[] lum,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HLS);
+ Core.inRange(out, new Scalar(hue[0], lum[0], sat[0]),
+ new Scalar(hue[1], lum[1], sat[1]), out);
+ }
+
+ /**
+ * Expands area of lower value in an image.
+ * @param src the Image to erode.
+ * @param kernel the kernel for erosion.
+ * @param anchor the center of the kernel.
+ * @param iterations the number of times to perform the erosion.
+ * @param borderType pixel extrapolation method.
+ * @param borderValue value to be used for a constant border.
+ * @param dst Output Image.
+ */
+ private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
+ int borderType, Scalar borderValue, Mat dst) {
+ if (kernel == null) {
+ kernel = new Mat();
+ }
+ if (anchor == null) {
+ anchor = new Point(-1,-1);
+ }
+ if (borderValue == null) {
+ borderValue = new Scalar(-1);
+ }
+ Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
+ }
+
+ /**
+ * An indication of which type of filter to use for a blur.
+ * Choices are BOX, GAUSSIAN, MEDIAN, and BILATERAL
+ */
+ enum BlurType{
+ BOX("Box Blur"), GAUSSIAN("Gaussian Blur"), MEDIAN("Median Filter"),
+ BILATERAL("Bilateral Filter");
+
+ private final String label;
+
+ BlurType(String label) {
+ this.label = label;
+ }
+
+ public static BlurType get(String type) {
+ if (BILATERAL.label.equals(type)) {
+ return BILATERAL;
+ }
+ else if (GAUSSIAN.label.equals(type)) {
+ return GAUSSIAN;
+ }
+ else if (MEDIAN.label.equals(type)) {
+ return MEDIAN;
+ }
+ else {
+ return BOX;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return this.label;
+ }
+ }
+
+ /**
+ * Softens an image using one of several filters.
+ * @param input The image on which to perform the blur.
+ * @param type The blurType to perform.
+ * @param doubleRadius The radius for the blur.
+ * @param output The image in which to store the output.
+ */
+ private void blur(Mat input, BlurType type, double doubleRadius,
+ Mat output) {
+ int radius = (int)(doubleRadius + 0.5);
+ int kernelSize;
+ switch(type){
+ case BOX:
+ kernelSize = 2 * radius + 1;
+ Imgproc.blur(input, output, new Size(kernelSize, kernelSize));
+ break;
+ case GAUSSIAN:
+ kernelSize = 6 * radius + 1;
+ Imgproc.GaussianBlur(input,output, new Size(kernelSize, kernelSize), radius);
+ break;
+ case MEDIAN:
+ kernelSize = 2 * radius + 1;
+ Imgproc.medianBlur(input, output, kernelSize);
+ break;
+ case BILATERAL:
+ Imgproc.bilateralFilter(input, output, -1, radius, radius);
+ break;
+ }
+ }
+
+ /**
+ * Apply a fixed-level threshold to each array element in an image.
+ * @param src Image to threshold.
+ * @param threshold threshold value.
+ * @param maxVal Maximum value for THRES_BINARY and THRES_BINARY_INV
+ * @param type Type of threshold to appy.
+ * @param dst output Image.
+ */
+ private void cvThreshold(Mat src, double threshold, double maxVal, int type,
+ Mat dst) {
+ Imgproc.threshold(src, dst, threshold, maxVal, type);
+ }
+
+ /**
+ * Sets the values of pixels in a binary image to their distance to the nearest black pixel.
+ * @param input The image on which to perform the Distance Transform.
+ * @param type The Transform.
+ * @param maskSize the size of the mask.
+ * @param output The image in which to store the output.
+ */
+ private void findContours(Mat input, boolean externalOnly,
+ List contours) {
+ Mat hierarchy = new Mat();
+ contours.clear();
+ int mode;
+ if (externalOnly) {
+ mode = Imgproc.RETR_EXTERNAL;
+ }
+ else {
+ mode = Imgproc.RETR_LIST;
+ }
+ int method = Imgproc.CHAIN_APPROX_SIMPLE;
+ Imgproc.findContours(input, contours, hierarchy, mode, method);
+ }
+
+ /**
+ * Compute the convex hulls of contours.
+ * @param inputContours The contours on which to perform the operation.
+ * @param outputContours The contours where the output will be stored.
+ */
+ private void convexHulls(List inputContours,
+ ArrayList outputContours) {
+ final MatOfInt hull = new MatOfInt();
+ outputContours.clear();
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final MatOfPoint mopHull = new MatOfPoint();
+ Imgproc.convexHull(contour, hull);
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int) hull.get(j, 0)[0];
+ double[] point = new double[] {contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ outputContours.add(mopHull);
+ }
+ }
+
+
+ /**
+ * Filters out contours that do not meet certain criteria.
+ * @param inputContours is the input list of contours
+ * @param output is the the output list of contours
+ * @param minArea is the minimum area of a contour that will be kept
+ * @param minPerimeter is the minimum perimeter of a contour that will be kept
+ * @param minWidth minimum width of a contour
+ * @param maxWidth maximum width
+ * @param minHeight minimum height
+ * @param maxHeight maximimum height
+ * @param Solidity the minimum and maximum solidity of a contour
+ * @param minVertexCount minimum vertex Count of the contours
+ * @param maxVertexCount maximum vertex Count
+ * @param minRatio minimum ratio of width to height
+ * @param maxRatio maximum ratio of width to height
+ */
+ private void filterContours(List inputContours, double minArea,
+ double minPerimeter, double minWidth, double maxWidth, double minHeight, double
+ maxHeight, double[] solidity, double maxVertexCount, double minVertexCount, double
+ minRatio, double maxRatio, List output) {
+ final MatOfInt hull = new MatOfInt();
+ output.clear();
+ //operation
+ for (int i = 0; i < inputContours.size(); i++) {
+ final MatOfPoint contour = inputContours.get(i);
+ final Rect bb = Imgproc.boundingRect(contour);
+ if (bb.width < minWidth || bb.width > maxWidth) continue;
+ if (bb.height < minHeight || bb.height > maxHeight) continue;
+ final double area = Imgproc.contourArea(contour);
+ if (area < minArea) continue;
+ if (Imgproc.arcLength(new MatOfPoint2f(contour.toArray()), true) < minPerimeter) continue;
+ Imgproc.convexHull(contour, hull);
+ MatOfPoint mopHull = new MatOfPoint();
+ mopHull.create((int) hull.size().height, 1, CvType.CV_32SC2);
+ for (int j = 0; j < hull.size().height; j++) {
+ int index = (int)hull.get(j, 0)[0];
+ double[] point = new double[] { contour.get(index, 0)[0], contour.get(index, 0)[1]};
+ mopHull.put(j, 0, point);
+ }
+ final double solid = 100 * area / Imgproc.contourArea(mopHull);
+ if (solid < solidity[0] || solid > solidity[1]) continue;
+ if (contour.rows() < minVertexCount || contour.rows() > maxVertexCount) continue;
+ final double ratio = bb.width / (double)bb.height;
+ if (ratio < minRatio || ratio > maxRatio) continue;
+ output.add(contour);
+ }
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/settings/Constants.java b/src/org/usfirst/frc/team3735/robot/settings/Constants.java
new file mode 100644
index 0000000..da5d143
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/settings/Constants.java
@@ -0,0 +1,53 @@
+package org.usfirst.frc.team3735.robot.settings;
+
+public class Constants {
+
+ public class Drive{
+
+ public static final double InchesPerTick = .00309196; //in inches
+
+ //exp drive
+ public static final double moveReactivity = 1; //(0,1] (least reactive, most reactive]
+ public static final double turnReactivity = 1; //(0,1] (least reactive, most reactive]
+ public static final double scaledMaxMove = .8;
+ public static final double scaledMaxTurn = .5; //(0,1] directly to the arcadedrive turn value
+ //these retain the range but shift more of the action towards lower values as the exponent is raised higher
+ //graph y = x * x^(p-1) {-1 < x < 1} for visualization
+ public static final double moveExponent = 3; //[1,inf) 1 is linear, 2 is squared (normal), etc.
+ public static final double turnExponent = 3; //[1,inf)
+
+ //for turning slowly with lb and rb
+ public static final double lowSensitivityLeftTurn = -.2;
+ public static final double lowSensitivityRightTurn = .2;
+
+ public static final boolean isUsingLeftEncoders = true;
+ public static final boolean isUsingRightEncoders = true;
+
+ }
+
+ public class GearIntake{
+ //these arent voltage anymore
+ public final static double rollerOutVoltage = .25;
+ public final static double rollerInVoltage = -.85;
+ public final static double feedingVoltage = -.5;
+ }
+
+ public class Scaler{
+ public final static double upCurrent = 1.0;
+ public final static double powerMax = 20000; //350
+ public static final double rampRate = .02;
+ }
+
+ public class BallIntake{
+ public final static double rollerInSpeed = .7;
+ }
+
+ public class Shooter{
+ public final static double shootSpeed = 32000; //in rpms
+
+ public static final double lowAgitatorVoltage = 3;
+ public static final double agitatorVoltage = 7;
+ public static final double highAgitatorVoltage = 12;
+ }
+
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team3735/robot/settings/Dms.java b/src/org/usfirst/frc/team3735/robot/settings/Dms.java
new file mode 100644
index 0000000..af883df
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/settings/Dms.java
@@ -0,0 +1,52 @@
+package org.usfirst.frc.team3735.robot.settings;
+
+//it stands for Dimensions if you haven't figured that out
+public class Dms {
+ //all in inches
+
+ public class Bot{
+ public class DriveBase{
+ //???
+ public static final double WIDTH = 28;
+ public static final double HALFWIDTH = WIDTH/2.0;
+ public static final double LENGTH = 31;
+ public static final double HALFLENGTH = LENGTH/2.0;
+ }
+ public static final double WIDTH = 37;
+ public static final double HALFWIDTH = WIDTH/2.0;
+ public static final double LENGTH = 31;
+ public static final double HALFLENGTH = LENGTH/2.0;
+
+ }
+
+ public class Field{
+ //just guesses rn
+ public static final double LENGTH = 653;
+ public static final double HALFLENGTH = LENGTH/2.0;
+ public static final double WIDTH = 324;
+ public static final double HALFWIDTH = WIDTH/2.0;
+ public static final double BASELINE = 93.3;
+
+ public class AirShip{
+ public static final double SIDELENGTH = 0;
+ public static final double HALFSL = SIDELENGTH/2.0;
+ public static final double DISTANCEFROMWALL = 114;
+ public static final double MOUNTDISTANCEFROMWALL = 111;
+
+ }
+ }
+
+
+
+
+// if (rotateValue * moveValue > 0.0) {
+// leftMotorSpeed = Math.signum(moveValue) * (Math.abs(moveValue) - Math.abs(rotateValue));
+// rightMotorSpeed = Math.signum(moveValue) * Math.max(Math.abs(moveValue), Math.abs(rotateValue));
+// } else {
+// leftMotorSpeed = Math.signum(moveValue) * Math.max(Math.abs(moveValue), Math.abs(rotateValue));
+// rightMotorSpeed = Math.signum(moveValue) * (Math.abs(moveValue) - Math.abs(rotateValue));
+// }
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/settings/RobotMap.java b/src/org/usfirst/frc/team3735/robot/settings/RobotMap.java
new file mode 100644
index 0000000..a88930e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/settings/RobotMap.java
@@ -0,0 +1,89 @@
+//values for the practice robot
+
+//package org.usfirst.frc.team3735.robot;
+//
+//public class RobotMap {
+//
+// public static class Drive{
+// public static int leftMotor1 = 4;
+// public static int leftMotor2 = 5;
+// public static int leftMotor3 = 6;
+//
+// public static int rightMotor1 = 1;
+// public static int rightMotor2 = 2;
+// public static int rightMotor3 = 3;
+// }
+// public static class GearIntake{
+// public static final boolean topRollerInverted = true;
+// public static int topRoller = 8;
+// public static int topFeedSolenoid = 1;
+// public static int liftSolenoid = 0;
+// }
+// public static class Shooter{
+// public static final boolean agitatorInverted = false;
+// public static final boolean drumInverted = false;
+// public static int drum = 7;
+// public static int drum2 = 9;
+// public static int agitator = 13;
+// }
+// public static class BallIntake{
+// public static final boolean rollerInverted = true;
+// public static int roller = 11;
+// }
+// public static class Scaler{
+// public static final boolean scalerInverted = true;
+// public static int motor = 10;
+// public static int motor2 = 12;
+//
+//
+// }
+//
+//}
+
+//values for the final robot
+
+package org.usfirst.frc.team3735.robot.settings;
+
+public class RobotMap {
+
+ public static class Drive{
+ public static int leftMotor1 = 3;//6
+ public static int leftMotor2 = 5;//4
+ public static int leftMotor3 = 6;//13
+
+ public static int rightMotor1 = 11;//11
+ public static int rightMotor2 = 10;//10
+ public static int rightMotor3 = 9;//12
+ }
+ public static class GearIntake{
+ public static int topRoller = 7;//8
+ public static final boolean topRollerInverted = true;
+ public static int liftSolenoid = 1;//2
+ }
+// public static class Shooter{
+// public static int drum = 2;
+// public static int drum2 = 7;
+// public static final boolean drumInverted = false;
+// public static int agitator = 13;
+// public static final boolean agitatorInverted = false;
+// }
+// public static class BallIntake{
+// public static int roller = 1;
+// public static final boolean rollerInverted = true;
+// }
+ public static class Scaler{
+ public static int motor = 0;//1
+ public static final int motor2 = 0 ;//2
+ public static final boolean scalerInverted = true;
+
+ }
+ public static class CubeIntake{
+ public static int pivotMotor = 8;
+ public static int rightMotor = 1;
+ public static int leftMotor = 2;
+
+ }
+
+ //1 2 5 7
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/settings/Waypoints.java b/src/org/usfirst/frc/team3735/robot/settings/Waypoints.java
new file mode 100644
index 0000000..c5571de
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/settings/Waypoints.java
@@ -0,0 +1,58 @@
+package org.usfirst.frc.team3735.robot.settings;
+
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+public class Waypoints {
+ public static final Location center = new Location(Dms.Field.HALFLENGTH, 0);
+ public static final Location topRight = new Location(Dms.Field.LENGTH, Dms.Field.HALFWIDTH);
+ public static final Location topLeft = new Location(0, Dms.Field.HALFWIDTH);
+ public static final Location bottomRight = new Location(Dms.Field.LENGTH, -Dms.Field.HALFWIDTH);
+ public static final Location bottomLeft = new Location(0, -Dms.Field.HALFWIDTH);
+
+ public static final Location Boiler = new Location(6, -155);
+ public static final Location MiddleRope = new Location(88, 0);
+ public static final Location upperRope = new Location(180, 53);
+ public static final Location BottomRope = new Location(180, 53);
+ public static final Location middlePeg = new Location(Dms.Field.AirShip.DISTANCEFROMWALL, 0);
+ public static final Location upperPeg = middlePeg
+ .appendYawDistance(-90, Dms.Field.AirShip.HALFSL)
+ .appendYawDistance(30, Dms.Field.AirShip.HALFSL);
+ public static final Location lowerPeg = middlePeg
+ .appendYawDistance(90, Dms.Field.AirShip.HALFSL)
+ .appendYawDistance(-30, Dms.Field.AirShip.HALFSL);
+
+ public static final Location middlePegLineup = middlePeg.appendYawDistance(180, 70);
+ public static final Location upperPegLineup = upperPeg.appendYawDistance(-120, 70);
+ public static final Location lowerPegLineup = lowerPeg.appendYawDistance(120, 70);
+
+
+ public static final Location pegSquareUpTop = center.appendXY(-100, 100);
+ public static final Location pegSquareUpBottom = center.appendXY(-100, -100);
+ public static final Location keySquareUpTop = center.appendXY(100, 100);
+ public static final Location keySquareUpBottom = center.appendXY(100, -100);
+
+ public static final Location keyLeft = topRight.appendXY(-100, -100);
+ public static final Location keyBottom = topRight.appendXY(-50, -200);
+ public static final Location keyBottomPath1 = bottomRight.appendXY(-200, 70);
+ public static final Location keyBottomPath2 = bottomRight.appendXY(-70, 200);
+
+ public static final Location[] centerFieldSquares = new Location[] {
+ pegSquareUpTop, pegSquareUpBottom, keySquareUpTop, keySquareUpBottom
+ };
+ public static final Location[] toTopGear= {pegSquareUpTop, upperPegLineup};
+ public static final Location[] toMiddleGearTop= {pegSquareUpTop, upperPegLineup, middlePegLineup};
+ public static final Location[] toMiddleGearBottom= {pegSquareUpBottom, lowerPegLineup, middlePegLineup};
+ public static final Location[] toBottomGear= {pegSquareUpBottom, lowerPegLineup};
+
+ public static final Location[] toKeyBottom= {keySquareUpBottom, keyBottomPath1, keyBottomPath2, keyBottom};
+ public static final Location[] toKeyLeft= {keySquareUpTop, keyLeft};
+
+
+ //generated from the PathDrawer program
+ public static final Location[] topGear = {new Location(33.8,103.8, true), new Location(61.0,100.4, true), new Location(80.6,94.2, true), new Location(95.3,83.9, true), new Location(108.1,68.1, true), new Location(121.9,45.3, true)};
+ public static final Location[] bottomGear = {new Location(33.8,-108.7, true), new Location(60.5,-103.5, true), new Location(79.7,-95.9, true), new Location(93.2,-86.1, true), new Location(103.2,-74.1, true), new Location(111.8,-59.9, true), new Location(121.1,-43.7, true)};
+ public static final Location[] topGearHopper = {new Location(95.6,75.0, true), new Location(86.3,92.8, true), new Location(83.2,107.0, true), new Location(86.1,117.8, true), new Location(94.9,125.8, true), new Location(109.5,131.3, true), new Location(129.8,134.5, true), new Location(155.7,135.9, true), new Location(187.0,135.9, true)};
+ public static final Location[] bottomGearHopper = {new Location(88.1,-81.5, true), new Location(70.7,-102.5, true), new Location(63.9,-118.6, true), new Location(66.8,-130.1, true), new Location(78.6,-137.3, true), new Location(98.4,-140.2, true), new Location(125.2,-139.2, true)};
+ public static final Location[] crossField = {new Location(83.2,-108.7, true), new Location(109.3,-110.4, true), new Location(133.4,-110.7, true), new Location(155.7,-109.7, true), new Location(176.3,-107.5, true), new Location(195.4,-104.2, true), new Location(213.0,-99.8, true), new Location(229.4,-94.5, true), new Location(244.6,-88.2, true), new Location(258.7,-81.1, true), new Location(271.9,-73.2, true), new Location(284.3,-64.6, true), new Location(296.1,-55.4, true), new Location(307.3,-45.6, true), new Location(318.1,-35.4, true), new Location(328.6,-24.7, true), new Location(339.0,-13.8, true), new Location(349.3,-2.5, true), new Location(359.8,8.9, true), new Location(370.4,20.5, true), new Location(381.4,32.1, true), new Location(392.9,43.7, true)};
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/subsystems/CubeIntake.java b/src/org/usfirst/frc/team3735/robot/subsystems/CubeIntake.java
new file mode 100644
index 0000000..b89202c
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/subsystems/CubeIntake.java
@@ -0,0 +1,56 @@
+package org.usfirst.frc.team3735.robot.subsystems;
+
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.settings.RobotMap;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class CubeIntake extends Subsystem {
+ WPI_TalonSRX left;
+ WPI_TalonSRX right;
+ WPI_TalonSRX pivot;
+
+ private boolean isDown = false;
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+ public CubeIntake(){
+ pivot = new WPI_TalonSRX(RobotMap.CubeIntake.pivotMotor);
+ left = new WPI_TalonSRX(RobotMap.CubeIntake.leftMotor);
+ right = new WPI_TalonSRX(RobotMap.CubeIntake.rightMotor);
+
+ pivot.setNeutralMode(NeutralMode.Brake);
+ pivot.setInverted(true);
+ left.setInverted(true);
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ //positive is out, negative is in
+ //actually flip that
+ public void setPivotSpeed(double speed){
+ pivot.set(speed);
+ }
+
+ public void setRollersSpeed(double speed){
+ left.set(speed);
+ right.set(speed);
+ }
+
+ public void log(){
+ }
+
+ public void debugLog() {
+
+
+ }
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/subsystems/Drive.java b/src/org/usfirst/frc/team3735/robot/subsystems/Drive.java
new file mode 100644
index 0000000..63d9698
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/subsystems/Drive.java
@@ -0,0 +1,505 @@
+package org.usfirst.frc.team3735.robot.subsystems;
+
+
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.commands.drive.DDxDrive;
+import org.usfirst.frc.team3735.robot.commands.drive.ExpDrive;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.settings.Dms;
+import org.usfirst.frc.team3735.robot.settings.RobotMap;
+import org.usfirst.frc.team3735.robot.util.settings.BooleanSetting;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+
+public class Drive extends Subsystem {
+
+ private WPI_TalonSRX l1;
+ private WPI_TalonSRX l2;
+ private WPI_TalonSRX l3;
+
+ private WPI_TalonSRX r1;
+ private WPI_TalonSRX r2;
+ private WPI_TalonSRX r3;
+
+ private static double dP = 1.0;
+ private static double dI = 0.0;
+ private static double dD = 0.0;
+ private static double dF = 0.0;
+ private static int iZone = 2;
+
+ //for speed profiling
+ public static final double slope = 0.00113174;
+ public static final double minPct = 0.0944854;
+ public static final double maxSpeed = (1-minPct)/slope; //about 800.11 rpm, 173 in/s
+
+
+
+ private double leftAddTurn = 0;
+ private double rightAddTurn = 0;
+ private double visionAssist = 0;
+ private double navxAssist = 0;
+
+ public static Setting moveExponent = new Setting("Move Exponent", Constants.Drive.moveExponent);
+ public static Setting turnExponent = new Setting("Turn Exponent", Constants.Drive.turnExponent);
+ public static Setting scaledMaxMove = new Setting("Scaled Max Move", Constants.Drive.scaledMaxMove);
+ public static Setting scaledMaxTurn = new Setting("Scaled Max Turn", Constants.Drive.scaledMaxTurn);
+
+ public static BooleanSetting brakeEnabled = new BooleanSetting("Brake Mode On", false) {
+
+ @Override
+ public void valueChanged(boolean val) {
+ if(Robot.drive != null) {
+ Robot.drive.setEnableBrake(val);
+ System.out.println("Brake mode " + val);
+
+ }
+ }
+
+ };
+
+
+ public Drive() {
+ l1 = new WPI_TalonSRX(RobotMap.Drive.leftMotor1);
+ l2 = new WPI_TalonSRX(RobotMap.Drive.leftMotor2);
+ l3 = new WPI_TalonSRX(RobotMap.Drive.leftMotor3);
+
+ r1 = new WPI_TalonSRX(RobotMap.Drive.rightMotor1);
+ r2 = new WPI_TalonSRX(RobotMap.Drive.rightMotor2);
+ r3 = new WPI_TalonSRX(RobotMap.Drive.rightMotor3);
+
+ initSensors();
+ setupSlaves();
+ setEnableBrake(true);
+ }
+
+ /*******************************
+ * Default Command For Driving
+ *******************************/
+ public void initDefaultCommand() {
+ setDefaultCommand(new DDxDrive());
+ }
+
+ /*******************************
+ * Setups for Position and Speed
+ *******************************/
+ public void setupForPositionControl() {
+ l1.configAllowableClosedloopError(0, 0, 0);
+ setLeftPIDF(dP,dI,dD,dF);
+ l1.config_IntegralZone(0, iZone, 0);
+
+ //slot, value, timeout
+ l1.configAllowableClosedloopError(0, 0, 0);
+ setLeftPIDF(dP,dI,dD,dF);
+ l1.config_IntegralZone(0, iZone, 0);
+
+ setEnableBrake(true);
+ }
+
+ /*******************************
+ * Speed Control Setup
+ *******************************/
+ public void setupDriveForSpeedControl() {
+ //setEnableBrake(false);
+
+ this.setNavxAssist(0);
+ this.setVisionAssist(0);
+ }
+
+ /*******************************
+ * Slaves Setup
+ *******************************/
+ public void setupSlaves() {
+ l2.follow(l1);
+ l3.follow(l1);
+
+ r2.follow(r1);
+ r3.follow(r1);
+ }
+
+ public void initSensors() {
+
+ //int absolutePosition = l1.getPulseWidthPosition() & 0xFFF;
+ int absolutePosition = l1.getSelectedSensorPosition(0) & 0xFFF;
+
+ //l1.reverseOutput(false); <--- setinverted does this instead
+
+ //l1.setEncPosition(absolutePosition);
+ l1.setSelectedSensorPosition(absolutePosition, 0, 0);
+
+ //l1.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
+ l1.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
+
+ //l1.reverseSensor(true);
+ l1.setSensorPhase(true);
+
+ //l1.configNominalOutputVoltage(0.0, -0.0);
+// l1.configNominalOutputForward(0, 0);
+// l1.configNominalOutputReverse(0, 0);
+// l1.configPeakOutputForward(1, 0);
+// l1.configPeakOutputReverse(-1, 0);
+ //l1.setPosition(0);
+
+ absolutePosition = r1.getSelectedSensorPosition(0) & 0xFFF;
+
+ //l1.reverseOutput(false); <--- setinverted does this instead
+
+ //l1.setEncPosition(absolutePosition);
+ r1.setSelectedSensorPosition(absolutePosition, 0, 0);
+
+ //l1.setFeedbackDevice(FeedbackDevice.CtreMagEncoder_Relative);
+ r1.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
+
+ //l1.reverseSensor(true);
+ r1.setSensorPhase(true);
+
+ //r1.setInverted(true);
+
+ //l1.configNominalOutputVoltage(0.0, -0.0);
+// r1.configNominalOutputForward(0, 0);
+// r1.configNominalOutputReverse(0, 0);
+// r1.configPeakOutputForward(1, 0);
+// r1.configPeakOutputReverse(-1, 0);
+
+
+
+
+ }
+
+ public void setPIDSettings(double kp, double ki, double kd){
+ setLeftPID(kp, ki, kd);
+ setRightPID(kp, ki, kd);
+ }
+
+ public void setPIDFSettings(double kp, double ki, double kd, double kf){
+ setLeftPIDF(kp, ki, kd, kf);
+ setRightPIDF(kp, ki, kd, kf);
+ }
+
+ public void setLeftPIDF(double kp, double ki, double kd, double kf) {
+ setLeftPID(kp,ki,kd);
+ l1.config_kF(0, kf, 0);
+ }
+
+ public void setRightPIDF(double kp, double ki, double kd, double kf) {
+ setRightPID(kp,ki,kd);
+ r1.config_kF(0, kf, 0);
+ }
+
+ public void setLeftPID(double kp, double ki, double kd){
+ l1.config_kP(0, kp, 0);
+ l1.config_kI(0, ki, 0);
+ l1.config_kD(0, kd, 0);
+ }
+ public void setRightPID(double kp, double ki, double kd){
+ r1.config_kP(0, kp, 0);
+ r1.config_kI(0, ki, 0);
+ r1.config_kD(0, kd, 0);
+ }
+// /******************************
+// * changing the Talon control mode
+// */
+// public void changeControlMode(TalonControlMode t){
+// l1.changeControlMode(t);
+// r1.changeControlMode(t);
+// }
+
+ /*********************************
+ * Configuring left and right PID Peak Voltages
+ */
+ public void setLeftPeakVoltage(double vol){
+ //l1.configPeakOutputVoltage(vol, -vol);
+ l1.configPeakOutputForward(vol, 0);
+ l1.configPeakOutputReverse(-vol, 0);
+
+ }
+
+ public void setRightPeakVoltage(double vol){
+ //r1.configPeakOutputVoltage(vol, -vol);
+ r1.configPeakOutputForward(vol, 0);
+ r1.configPeakOutputReverse(-vol, 0);
+ }
+
+ public void resetEncodersPositions(){
+ int absolutePosition = l1.getSelectedSensorPosition(0) & 0xFFF;
+ l1.setSelectedSensorPosition(absolutePosition, 0, 0);
+
+ absolutePosition = r1.getSelectedSensorPosition(0) & 0xFFF;
+ r1.setSelectedSensorPosition(absolutePosition, 0, 0);
+
+ }
+
+ /*******************************
+ * Drive Functions
+ *******************************/
+
+ /**
+ * Standard arcade drive from wpi's RobotDrive class
+ * @param move
+ * @param rotate
+ */
+ public void arcadeDrive(double move, double rotate) {
+ //copied from RobotDrive class. essentially lowers the speed of one motor first, rather than increases
+ //one and decreases the other at the same time.
+
+ double leftMotorSpeed;
+ double rightMotorSpeed;
+
+ double moveValue = move;
+ double rotateValue = rotate + getTurnAdditions();
+ if (moveValue > 0.0) {
+ if (rotateValue < 0.0) {
+ leftMotorSpeed = moveValue + rotateValue;
+ rightMotorSpeed = Math.max(moveValue, -rotateValue);
+ } else {
+ leftMotorSpeed = Math.max(moveValue, rotateValue);
+ rightMotorSpeed = moveValue - rotateValue;
+ }
+ } else {
+ if (rotateValue < 0.0) {
+ leftMotorSpeed = -Math.max(-moveValue, -rotateValue);
+ rightMotorSpeed = moveValue - rotateValue;
+ } else {
+ leftMotorSpeed = moveValue + rotateValue;
+ rightMotorSpeed = -Math.max(-moveValue, rotateValue);
+ }
+ }
+ setLeftRight(leftMotorSpeed, rightMotorSpeed);
+ }
+
+
+ public void normalDrive(double move, double rotate){
+ double rotateValue = rotate + getTurnAdditions();
+ setLeftRight(move + rotateValue, move - rotateValue);
+ }
+
+ /**
+ * Limits the left and right speeds so that rotation is consistent
+ * across all move values. Modifies speed for consistent rotation.
+ * @param move
+ * @param rotate
+ */
+ public void limitedDrive(double move, double rotate) {
+ double left = move + getTurnAdditions() + rotate;
+ double right = move - getTurnAdditions() - rotate;
+ double leftSpeed;
+ double rightSpeed;
+// if(left > 1) {
+// leftSpeed = 1;
+// rightSpeed = right - left + 1;
+// }else if(right > 1) {
+// rightSpeed = 1;
+// leftSpeed = left - right + 1;
+// }else if(left < -1) {
+// leftSpeed = -1;
+// rightSpeed = right - left - 1;
+// }else if(right < -1) {
+// rightSpeed = -1;
+// leftSpeed = left - right - 1;
+// }
+
+ if(Math.abs(left) > 1) {
+ leftSpeed = Math.signum(left);
+ rightSpeed = right - left + Math.signum(left);
+ }else if(Math.abs(right) > 1) {
+ leftSpeed = left - right + Math.signum(right);
+ rightSpeed = Math.signum(right);
+ }else {
+ leftSpeed = left;
+ rightSpeed = right;
+ }
+ setLeftRight(leftSpeed, rightSpeed);
+
+ }
+
+ /**
+ * Drives in a circle with a specified radius
+ * @param radius
+ * @param move
+ */
+ public void radialDrive(double radius, double move){
+ double left;
+ double right;
+ if(radius > 0){
+ radius = Math.abs(radius);
+ left = move;
+ right = move * (radius - Dms.Bot.HALFWIDTH)/
+ (radius + Dms.Bot.HALFWIDTH);
+ }else{
+ radius = Math.abs(radius);
+ right = move;
+ left = move * (radius - Dms.Bot.HALFWIDTH)/
+ (radius + Dms.Bot.HALFWIDTH);
+ }
+ setLeftRight(left, right);
+ }
+
+ public double getTurnAdditions() {
+ return leftAddTurn + rightAddTurn + visionAssist + navxAssist;
+ }
+
+ /*******************************
+ * Additive setters
+ *******************************/
+ public void setLeftTurn(double turn){
+ leftAddTurn = turn;
+ }
+ public void setRightTurn(double turn){
+ rightAddTurn = turn;
+ }
+ public void setVisionAssist(double j) {
+ visionAssist = j;
+ }
+ public void setNavxAssist(double error) {
+ this.navxAssist = (error/180.0) * Navigation.navCo.getValue();
+ }
+
+
+ /*******************************
+ * Brake Mode
+ *******************************/
+ public void setEnableBrake(boolean b) {
+ if(b) {
+ l1.setNeutralMode(NeutralMode.Brake);
+ l2.setNeutralMode(NeutralMode.Brake);
+ l3.setNeutralMode(NeutralMode.Brake);
+ r1.setNeutralMode(NeutralMode.Brake);
+ r2.setNeutralMode(NeutralMode.Brake);
+ r3.setNeutralMode(NeutralMode.Brake);
+ }else {
+ l1.setNeutralMode(NeutralMode.Coast);
+ l2.setNeutralMode(NeutralMode.Coast);
+ l3.setNeutralMode(NeutralMode.Coast);
+ r1.setNeutralMode(NeutralMode.Coast);
+ r2.setNeutralMode(NeutralMode.Coast);
+ r3.setNeutralMode(NeutralMode.Coast);
+ }
+
+ }
+
+ public double getLeftErrorInches(){
+ return l1.getClosedLoopError(0) * Constants.Drive.InchesPerTick ;
+ }
+
+ public double getRightErrorInches(){
+ return r1.getClosedLoopError(0)* Constants.Drive.InchesPerTick ;
+ }
+ /*********************************
+ * Left and Right position getters
+ *********************************/
+
+ public double getLeftPosition() {
+ return l1.getSelectedSensorPosition(0) * Constants.Drive.InchesPerTick;
+ }
+
+ public double getRightPosition() {
+ return r1.getSelectedSensorPosition(0) * Constants.Drive.InchesPerTick;
+ }
+
+ /*
+ * The return value is in units per 100ms for all sensor types.
+ * Sensor must be selected using
+ * configSelectedFeedbackSensor()/ Multiply by (600/SensorUnitsPerRotation) to convert into RPM.
+ *
+ */
+
+
+ public double getLeftSpeed() {
+ return l1.getSelectedSensorVelocity(0) * Constants.Drive.InchesPerTick * 10.0;
+ }
+
+ public double getRightSpeed() {
+ return r1.getSelectedSensorVelocity(0) * Constants.Drive.InchesPerTick * 10.0;
+ }
+
+ public double getAverageSpeed() {
+ return .5 * (getLeftSpeed() + getRightSpeed());
+ }
+
+ public void setLeftRight(double left, double right) {
+ //System.out.println("Left: " + left + " Right:" + right);
+ //l1.set(left);
+ l1.set(ControlMode.PercentOutput, left);
+
+
+ //r1.set(-1 * right);
+ r1.set(ControlMode.PercentOutput, -right);
+ }
+
+ /**
+ *
+ * @param spd the target speed in inches per second
+ * @return the percent, which converts spd into normal getspeed units (rpm), and then
+ * compensates for the deadzone using gathered data
+ */
+ public static double speedToPercent(double spd){
+ double speed = (Math.abs(spd) *60.0) /Constants.Drive.InchesPerTick;
+ return Math.copySign(slope*speed + minPct, spd);
+ }
+ public static double percentToSpeed(double pct){
+ return Math.copySign((pct - minPct) / slope, pct) * Constants.Drive.InchesPerTick/60.0;
+ }
+ /**
+ * @param percent [0,1] of the max speed to go
+ * @return the adjusted number to send to motors
+ */
+ public static double handleDeadband(double percent) {
+ return percent - (minPct * percent) + minPct;
+ }
+ public static double invHandleDeadband(double inv) {
+ return (inv-minPct)/(1-minPct);
+ }
+
+ /**
+ *
+ * @return the percentage of the max speed that the robot is going at
+ */
+ public double getCurrentPercentSpeed() {
+ return invHandleDeadband(speedToPercent(getAverageSpeed()));
+ }
+
+ /**
+ *
+ * @return the percentVBus to send to the motors to maintain the current speed
+ */
+ public double getCurrentPercent() {
+ return speedToPercent(getAverageSpeed());
+ }
+
+ public double getSpeedInchesFromCurrent() {
+ double pct = .5 * (l1.getOutputCurrent() + r1.getOutputCurrent());
+ return Math.abs(pct) > minPct ? (pct-Math.signum(pct)*minPct)/slope : 0;
+ }
+
+ /******************************************
+ * The Logs
+ ******************************************/
+ public void log() {
+
+ }
+
+ public void debugLog() {
+ SmartDashboard.putNumber("Drive Left Position", getLeftPosition());
+ SmartDashboard.putNumber("Drive Right Position", getRightPosition());
+
+ SmartDashboard.putNumber("Drive Left Speed", getLeftSpeed());
+ SmartDashboard.putNumber("Drive Right Speed", getRightSpeed());
+
+ SmartDashboard.putNumber("Drive Left Get", l1.get());
+ SmartDashboard.putNumber("Drive Right Get", r1.get());
+
+ SmartDashboard.putNumber("Drive avg speed inches", getAverageSpeed());
+ }
+
+
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/subsystems/GearIntake.java b/src/org/usfirst/frc/team3735/robot/subsystems/GearIntake.java
new file mode 100644
index 0000000..a4ad4f6
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/subsystems/GearIntake.java
@@ -0,0 +1,73 @@
+package org.usfirst.frc.team3735.robot.subsystems;
+
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.settings.RobotMap;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+
+public class GearIntake extends Subsystem {
+ WPI_TalonSRX roller;
+ //WPI_TalonSRX bottomRoller;
+ Solenoid liftSolenoid;
+
+ private boolean isDown = false;
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+ public GearIntake(){
+ roller = new WPI_TalonSRX(RobotMap.GearIntake.topRoller);
+ roller.setInverted(RobotMap.GearIntake.topRollerInverted);
+ liftSolenoid = new Solenoid(RobotMap.GearIntake.liftSolenoid);
+ liftUp();
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ //positive is out, negative is in
+ //actually flip that
+ public void setRollerSpeed(double speed){
+ roller.set(speed);
+ }
+
+
+
+ public void liftDown(){
+ liftSolenoid.set(true);
+ isDown = true;
+ }
+ public void liftUp(){
+ liftSolenoid.set(false);
+ isDown = false;
+ }
+
+ public void setIsDown(boolean down) {
+ liftSolenoid.set(down);
+ isDown = down;
+ }
+
+ public void switchLift(){
+ if(isDown){
+ liftUp();
+ }else{
+ liftDown();
+ }
+ }
+
+ public void log(){
+ }
+
+ public void debugLog() {
+
+
+ }
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/subsystems/Navigation.java b/src/org/usfirst/frc/team3735/robot/subsystems/Navigation.java
new file mode 100644
index 0000000..735fb7b
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/subsystems/Navigation.java
@@ -0,0 +1,357 @@
+package org.usfirst.frc.team3735.robot.subsystems;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import org.usfirst.frc.team3735.robot.Robot;
+
+//import org.usfirst.frc.team3735.robot.Robot.Side;
+import org.usfirst.frc.team3735.robot.settings.Dms;
+import org.usfirst.frc.team3735.robot.settings.Waypoints;
+import org.usfirst.frc.team3735.robot.util.PIDCtrl;
+import org.usfirst.frc.team3735.robot.util.bases.VortxIterative.Side;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+import org.usfirst.frc.team3735.robot.util.profiling.Position;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+
+
+public class Navigation extends Subsystem implements PIDSource, PIDOutput {
+ private static final int BUMP_THRESHOLD = 1;
+
+ private AHRS ahrs;
+
+ private PIDCtrl controller;
+ //PID Controller stuff
+ private static Setting outputExponent = new Setting("Nav Output Exponent", 1);
+ public static Setting iZone = new Setting("Turning IZone", 10);
+ public static Setting actingI = new Setting("Acting I Value", 0.004);
+
+ public static Setting verticalOffset = new Setting("Vertical Offset", 0);
+
+ public static Setting navCo = new Setting("Navx Assist Coeffecient", 5);
+ public static Setting navVisCo = new Setting("Navx Vision Assist Coeffecient", 5);
+
+
+ Position pos = new Position(0,0,0);
+ private Object posLock = new Object();
+
+ NetworkTable table;
+
+ private double prevLeft = 0;
+ private double prevRight = 0;
+ private double curLeft;
+ private double curRight;
+
+ public Navigation(){
+ table = NetworkTable.getTable("MAP");
+ ahrs = new AHRS(SPI.Port.kMXP);
+ controller = new PIDCtrl(.016,0.0,0.061,this,this);
+ controller.setOutputRange(-.5, .5);
+ controller.setInputRange(-180, 180);
+ controller.setContinuous();
+ controller.setIsUsingIZone(true);
+ controller.setIZone(10);
+ controller.setAbsoluteTolerance(1);
+
+ SmartDashboard.putData("Navigation Turning Controller", controller);
+
+ curLeft = Robot.drive.getLeftPosition();
+ curRight = Robot.drive.getRightPosition();
+ }
+
+ public synchronized void setPosition(Position p){
+ synchronized(posLock){
+ pos = p;
+ }
+ }
+
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+ public void initDefaultCommand() {
+ }
+
+ public synchronized void integrate(){
+ synchronized(posLock){
+ curLeft = Robot.drive.getLeftPosition();
+ curRight = Robot.drive.getRightPosition();
+
+ double dd = ((curLeft-prevLeft) + (curRight-prevRight)) * .5;
+ double angle = getYaw();
+ if(Robot.side.equals(Side.Left)){ //simplified version of getFieldYaw without limits
+ angle *= -1;
+ }else{
+ angle = (angle + 180) * -1;
+ }
+ pos.x += Math.cos(Math.toRadians(angle)) * dd;
+ pos.y += Math.sin(Math.toRadians(angle)) * dd;
+ pos.yaw = angle * -1;
+
+ prevLeft = curLeft;
+ prevRight = curRight;
+ }
+
+
+ }
+
+ public double getYaw(){
+ return ahrs.getYaw();
+ }
+
+ public double getFieldYaw() {
+ if(Robot.side.equals(Side.Left)) {
+ return getYaw();
+ }else {
+ return VortxMath.continuousLimit(getYaw() + 180, -180, 180);
+
+ }
+
+ }
+ public void zeroYaw(){
+ ahrs.zeroYaw();
+ }
+ public void resetAhrs(){
+ ahrs.reset();
+ }
+ public double getRate(){
+ return ahrs.getRate();
+ }
+
+ public AHRS getAHRS(){
+ return ahrs;
+ }
+ public void log(){
+ SmartDashboard.putNumber("Navigation Gyro Yaw", ahrs.getYaw());
+// SmartDashboard.putNumber("Gyro Acceleration X", ahrs.getWorldLinearAccelX());
+// SmartDashboard.putNumber("Gyro Acceleration Y", ahrs.getWorldLinearAccelY());
+// SmartDashboard.putNumber("Gyro Accel XY Vector", getXYAcceleration());
+
+ // displayDebugGyroData();
+
+ }
+
+ public void displayPosition(){
+ table.putNumberArray("centerX", new double[]{pos.x});
+ table.putNumberArray("centerY", new double[]{pos.y});
+ table.putNumberArray("angle", new double[]{pos.yaw});
+ }
+
+
+
+ public void displayDebugGyroData(){
+ SmartDashboard.putBoolean( "IMU_Connected", ahrs.isConnected());
+ SmartDashboard.putBoolean( "IMU_IsCalibrating", ahrs.isCalibrating());
+ SmartDashboard.putNumber( "IMU_Yaw", ahrs.getYaw());
+ SmartDashboard.putNumber( "IMU_Pitch", ahrs.getPitch());
+ SmartDashboard.putNumber( "IMU_Roll", ahrs.getRoll());
+
+ /* Display tilt-corrected, Magnetometer-based heading (requires */
+ /* magnetometer calibration to be useful) */
+ SmartDashboard.putNumber( "IMU_CompassHeading", ahrs.getCompassHeading());
+
+ /* Display 9-axis Heading (requires magnetometer calibration to be useful) */
+ SmartDashboard.putNumber( "IMU_FusedHeading", ahrs.getFusedHeading());
+
+ /* These functions are compatible w/the WPI Gyro Class, providing a simple */
+ /* path for upgrading from the Kit-of-Parts gyro to the navx MXP */
+ SmartDashboard.putNumber( "IMU_TotalYaw", ahrs.getAngle());
+ SmartDashboard.putNumber( "IMU_YawRateDPS", ahrs.getRate());
+
+ /* Display Processed Acceleration Data (Linear Acceleration, Motion Detect) */
+ SmartDashboard.putNumber( "IMU_Accel_X", ahrs.getWorldLinearAccelX());
+ SmartDashboard.putNumber( "IMU_Accel_Y", ahrs.getWorldLinearAccelY());
+ SmartDashboard.putBoolean( "IMU_IsMoving", ahrs.isMoving());
+ SmartDashboard.putBoolean( "IMU_IsRotating", ahrs.isRotating());
+
+
+ /* Display estimates of velocity/displacement. Note that these values are */
+ /* not expected to be accurate enough for estimating robot position on a */
+ /* FIRST FRC Robotics Field, due to accelerometer noise and the compounding */
+ /* of these errors due to single (velocity) integration and especially */
+ /* double (displacement) integration. */
+
+ SmartDashboard.putNumber( "Velocity_X", ahrs.getVelocityX());
+ SmartDashboard.putNumber( "Velocity_Y", ahrs.getVelocityY());
+ SmartDashboard.putNumber( "Displacement_X", ahrs.getDisplacementX());
+ SmartDashboard.putNumber( "Displacement_Y", ahrs.getDisplacementY());
+
+ SmartDashboard.putNumber("Displacement Total",
+ Math.sqrt(Math.pow(ahrs.getDisplacementX(),2) + Math.pow(ahrs.getDisplacementY(),2)));
+
+ /* Display Raw Gyro/Accelerometer/Magnetometer Values */
+ /* NOTE: These values are not normally necessary, but are made available */
+ /* for advanced users. Before using this data, please consider whether */
+ /* the processed data (see above) will suit your needs. */
+
+ SmartDashboard.putNumber( "RawGyro_X", ahrs.getRawGyroX());
+ SmartDashboard.putNumber( "RawGyro_Y", ahrs.getRawGyroY());
+ SmartDashboard.putNumber( "RawGyro_Z", ahrs.getRawGyroZ());
+ SmartDashboard.putNumber( "RawAccel_X", ahrs.getRawAccelX());
+ SmartDashboard.putNumber( "RawAccel_Y", ahrs.getRawAccelY());
+ SmartDashboard.putNumber( "RawAccel_Z", ahrs.getRawAccelZ());
+ SmartDashboard.putNumber( "RawMag_X", ahrs.getRawMagX());
+ SmartDashboard.putNumber( "RawMag_Y", ahrs.getRawMagY());
+ SmartDashboard.putNumber( "RawMag_Z", ahrs.getRawMagZ());
+ SmartDashboard.putNumber( "IMU_Temp_C", ahrs.getTempC());
+ SmartDashboard.putNumber( "IMU_Timestamp", ahrs.getLastSensorTimestamp());
+
+ /* Omnimount Yaw Axis Information */
+ /* For more info, see http://navx-mxp.kauailabs.com/installation/omnimount */
+ AHRS.BoardYawAxis yaw_axis = ahrs.getBoardYawAxis();
+ SmartDashboard.putString( "YawAxisDirection", yaw_axis.up ? "Up" : "Down" );
+ SmartDashboard.putNumber( "YawAxis", yaw_axis.board_axis.getValue() );
+
+ /* Sensor Board Information */
+ SmartDashboard.putString( "FirmwareVersion", ahrs.getFirmwareVersion());
+
+ /* Quaternion Data */
+ /* Quaternions are fascinating, and are the most compact representation of */
+ /* orientation data. All of the Yaw, Pitch and Roll Values can be derived */
+ /* from the Quaternions. If interested in motion processing, knowledge of */
+ /* Quaternions is highly recommended. */
+ SmartDashboard.putNumber( "QuaternionW", ahrs.getQuaternionW());
+ SmartDashboard.putNumber( "QuaternionX", ahrs.getQuaternionX());
+ SmartDashboard.putNumber( "QuaternionY", ahrs.getQuaternionY());
+ SmartDashboard.putNumber( "QuaternionZ", ahrs.getQuaternionZ());
+
+ /* Connectivity Debugging Support */
+ SmartDashboard.putNumber( "IMU_Byte_Count", ahrs.getByteCount());
+ SmartDashboard.putNumber( "IMU_Update_Count", ahrs.getUpdateCount());
+
+ }
+
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+
+ }
+
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return PIDSourceType.kDisplacement;
+ }
+
+
+ @Override
+ public double pidGet() {
+ return ahrs.getYaw();
+ }
+
+
+ public PIDCtrl getController() {
+ return controller;
+ }
+
+
+ @Override
+ public void pidWrite(double output) {
+ output = VortxMath.curve(output, outputExponent.getValue());
+ Robot.drive.setLeftRight(output, -output);
+ }
+
+
+ public boolean isBumped() {
+ return getXYAcceleration() > BUMP_THRESHOLD;
+ }
+
+ public double getXYAcceleration(){
+ return Math.hypot(ahrs.getWorldLinearAccelY(), ahrs.getWorldLinearAccelX());
+ }
+
+ public Position getStartingPosition() {
+ if(Robot.side.equals(Side.Left)){
+ return new Position(Dms.Bot.HALFLENGTH, verticalOffset.getValue(), 0);
+ }else{
+ return new Position(Dms.Field.LENGTH - Dms.Bot.HALFLENGTH, verticalOffset.getValue(), 180);
+ }
+ }
+
+ public void debugLog() {
+ // TODO Auto-generated method stub
+
+ }
+
+ public void resetPosition() {
+ zeroYaw();
+ Robot.retrieveSide();
+ setPosition(getStartingPosition());
+ Location.changeSide(Robot.side, Dms.Field.LENGTH);
+ System.out.println("Reseting Position...");
+ }
+
+ public synchronized Position getPosition() {
+ return pos;
+ }
+
+ /**
+ *
+ * @return the quadrant the robot is in
+ * follows the quadrant naming system of algebra, looking at the field from Field Map.PNG
+ */
+ public int getQuadrant() {
+ double xdif = pos.x - Waypoints.center.x;
+ double ydif = pos.y - Waypoints.center.y;
+ if(xdif > 0) {
+ if(ydif > 0) {
+ return 1;
+ }else {
+ return 4;
+ }
+ }else {
+ if(ydif > 0) {
+ return 2;
+ }else {
+ return 3;
+ }
+ }
+
+ }
+ public Location getClosestLocation(Location[] locs) {
+ if(locs != null && locs.length > 0) {
+ Location curPos = getPosition();
+ double least = curPos.distanceFrom(locs[0]);
+ int best = 0;
+ for(int i = 1; i < locs.length; i++) {
+ double dist = curPos.distanceFrom(locs[i]);
+ if(dist < least) {
+ least = dist;
+ best = i;
+ }
+ }
+ return locs[best];
+ }else {
+ System.out.println("Error in getting closest location");
+ return new Location(0,0);
+ }
+ }
+
+ public double getAngleToLocation(Location loc) {
+ return Math.toDegrees(-Math.atan2(loc.y - pos.y, loc.x - pos.x));
+ }
+
+ /**
+ *
+ * @param loc the Location to reference
+ * @return the Angle to the location, meant for using with the TurnTo Command,
+ * where the angle from getYaw is used for turning
+ */
+ public double getAngleToLocationCorrected(Location loc) {
+ double ans = Math.toDegrees(-Math.atan2(loc.y - pos.y, loc.x - pos.x));
+ if(Robot.side.equals(Side.Right)){
+ return VortxMath.continuousLimit(ans + 180, -180, 180);
+ }else {
+ return ans;
+ }
+
+ }
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/subsystems/Scaler.java b/src/org/usfirst/frc/team3735/robot/subsystems/Scaler.java
new file mode 100644
index 0000000..a0fd621
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/subsystems/Scaler.java
@@ -0,0 +1,93 @@
+package org.usfirst.frc.team3735.robot.subsystems;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.commands.scaler.ScalerOff;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.settings.RobotMap;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class Scaler extends Subsystem {
+
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+ WPI_TalonSRX motor;
+ WPI_TalonSRX motor2;
+
+
+ //private final double voltageChangeSpeed = 5;
+
+
+ private boolean isOverLoaded = false;
+ private double rampRate = Constants.Scaler.rampRate;
+ private double percent = 0;
+
+ public Scaler(){
+ motor = new WPI_TalonSRX(RobotMap.Scaler.motor);
+ //motor.setCloseLoopRampRate(.2);
+ motor.setInverted(RobotMap.Scaler.scalerInverted);
+ motor2 = new WPI_TalonSRX(RobotMap.Scaler.motor2);
+ //motor2.setCloseLoopRampRate(.2);
+ motor2.setInverted(RobotMap.Scaler.scalerInverted);
+
+ //motor.changeControlMode(WPI_TalonSRX.TalonControlMode.Voltage);
+ //motor.setVoltageCompensationRampRate(voltageChangeSpeed);
+ log();
+
+ }
+
+ public void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ //setDefaultCommand(new MySpecialCommand());
+ setDefaultCommand(new ScalerOff());
+ }
+
+ public void log(){
+
+ }
+
+ public void setCurrent(double current){
+ if(getPower() > Constants.Scaler.powerMax){
+ isOverLoaded = true;
+ percent = 0;
+ motor.set(percent);
+ motor2.set(-percent);
+ }else{
+ isOverLoaded = false;
+ if(current < percent){
+ percent -= rampRate;
+ }else if(current > percent){
+ percent += rampRate;
+ }
+ motor.set(percent);
+ motor2.set(-percent);
+
+ }
+ }
+
+ public void setPercent(double p){
+ percent = 0;
+ }
+
+ public double getPower(){
+ //return Math.abs(motor.getOutputCurrent() * motor.getOutputVoltage());
+ return 0;
+ }
+
+ public boolean getOverloaded(){
+ return isOverLoaded;
+ }
+
+ public void debugLog() {
+ SmartDashboard.putNumber("Scaler motor getPower", getPower());
+ //SmartDashboard.putNumber("Scaler motor joystick value", Robot.oi.getMainRightY());
+ SmartDashboard.putBoolean("Scaler overLoaded", isOverLoaded);
+ }
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/subsystems/Ultrasonic.java b/src/org/usfirst/frc/team3735/robot/subsystems/Ultrasonic.java
new file mode 100644
index 0000000..fe08ffe
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/subsystems/Ultrasonic.java
@@ -0,0 +1,66 @@
+package org.usfirst.frc.team3735.robot.subsystems;
+
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class Ultrasonic extends Subsystem {
+
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+ SerialPort serialPort;
+
+ public void initDefaultCommand() {
+ }
+
+ public Ultrasonic() {
+ try{
+ serialPort = new SerialPort(115200, SerialPort.Port.kUSB);
+ System.out.println("Created 0");
+ }catch(Exception e){
+ System.out.println(e);
+ try {
+ serialPort = new SerialPort(115200, SerialPort.Port.kUSB2);
+ } catch (Exception e2) {
+ System.out.println(e2);
+ System.out.println("No untrasonic sensor found");
+ }
+ }
+ }
+
+ public double getInchesDistance() {
+ double ret =-1;
+ if(serialPort != null){
+ String distanceString = serialPort.readString();
+ //System.out.println("Distance string is " + distanceString);
+ if(distanceString.length() >1){
+ int indexOfComma = distanceString.indexOf(',');
+ int indexOfI = distanceString.indexOf('I');
+ double distanceInches = Double.parseDouble(distanceString.substring(indexOfComma+1, indexOfI));
+ ret = distanceInches;
+ } else {
+ //System.out.println("Sensor string length is 0");
+ }
+ } else {
+ //System.out.println("Ultra is null");
+
+ }
+ return ret;
+ }
+
+ public void log() {
+// SmartDashboard.putNumber("Ultrasonic distance in Inches",
+// Double.valueOf(String.format("%.1f",getInchesDistance())));
+ }
+
+ public void debugLog() {
+ // TODO Auto-generated method stub
+
+ }
+
+}
+
diff --git a/src/org/usfirst/frc/team3735/robot/subsystems/Vision.java b/src/org/usfirst/frc/team3735/robot/subsystems/Vision.java
new file mode 100644
index 0000000..de95862
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/subsystems/Vision.java
@@ -0,0 +1,138 @@
+package org.usfirst.frc.team3735.robot.subsystems;
+
+import org.usfirst.frc.team3735.robot.pipelines.GearPipeline;
+import org.usfirst.frc.team3735.robot.pipelines.PegPipelineLSNTest4;
+import org.usfirst.frc.team3735.robot.pipelines.PegPipelineLSNTest5;
+import org.usfirst.frc.team3735.robot.pipelines.Test4ModdedMore;
+import org.usfirst.frc.team3735.robot.pipelines.Test4ModdedMorem;
+import org.usfirst.frc.team3735.robot.pipelines.TestModdedMoreTRI;
+import org.usfirst.frc.team3735.robot.subsystems.Vision.Pipes;
+import org.usfirst.frc.team3735.robot.util.settings.Setting;
+import org.usfirst.frc.team3735.robot.util.vision.ContoursOutputPipeline;
+import org.usfirst.frc.team3735.robot.util.vision.VisionHandler;
+import org.usfirst.frc.team3735.robot.util.vision.VisionHandler.VisionType;
+
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.vision.VisionPipeline;
+import edu.wpi.first.wpilibj.vision.VisionThread;
+
+/**
+ *
+ */
+public class Vision extends Subsystem {
+
+
+ private static final int IMG_WIDTH = 320;
+ private static final int IMG_HEIGHT = 240;
+ public static Setting dpp = new Setting("Vision Degrees per Pixel", 0.13125);
+
+ public static final double nullValue = -.0012345;
+
+ private UsbCamera camera1;
+ private UsbCamera camera2;
+
+
+ VisionHandler pegs;
+ VisionHandler gears;
+ VisionHandler mainHandler;
+
+ public Vision(){
+ camera1 = CameraServer.getInstance().startAutomaticCapture(0);
+ camera2 = CameraServer.getInstance().startAutomaticCapture(1);
+
+ camera1.setFPS(16);
+ //camera2.setFPS(16);
+
+ camera1.setResolution(IMG_WIDTH, IMG_HEIGHT);
+ camera2.setResolution(IMG_WIDTH, IMG_HEIGHT);
+
+ pegs = new VisionHandler(new TestModdedMoreTRI(), camera1, 2, "GRIP/PegTracker", VisionType.getCorrectRatio);
+ //pegs.startThread();
+
+ gears = new VisionHandler(new GearPipeline(), camera2, 1, "GRIP/GearTracker", VisionType.Normal);
+ //gears.startThread();
+
+ mainHandler = pegs;
+
+
+ }
+
+ public enum Pipes{
+ Gear,
+ Peg
+ }
+
+ public void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ //setDefaultCommand(new MySpecialCommand());
+ }
+
+ public void log(){
+
+
+ }
+
+ public double getRelativeCX(){
+// if(mainHandler.target.equals(VisionHandler.nullTarget)){
+// return nullValue;
+// }else{
+// return mainHandler.getCenterX() - IMG_WIDTH/2;
+// }
+ return 0;
+ }
+ public double getWidth(){
+// return mainHandler.getWidth();
+ return 0;
+ }
+
+
+
+ public VisionHandler getHandler(Pipes p){
+ switch(p){
+ case Peg:
+ return pegs;
+ case Gear:
+ return gears;
+ default:
+ return pegs;
+ }
+ }
+
+ public void setMainHandler(Pipes p){
+ mainHandler = getHandler(p);
+ }
+
+ public void pause(){
+ mainHandler.pause();
+
+ }
+
+ public void resume(){
+ mainHandler.resume();
+ }
+
+ public void debugLog() {
+// SmartDashboard.putNumber("CenterX", mainHandler.getCenterX());
+// SmartDashboard.putNumber("CenterY", mainHandler.getCenterY());
+ SmartDashboard.putNumber("Relative CX", getRelativeCX());
+// SmartDashboard.putNumber("height", mainHandler.getHeight());
+// SmartDashboard.putNumber("area", mainHandler.getArea());
+
+ SmartDashboard.putNumber("Gear CenterY", gears.getCenterY());
+ pegs.publishTarget();
+ pegs.publishAll();
+ gears.publishTarget();
+ }
+
+
+
+
+
+
+}
+
+
diff --git a/src/org/usfirst/frc/team3735/robot/triggers/Bumped.java b/src/org/usfirst/frc/team3735/robot/triggers/Bumped.java
new file mode 100644
index 0000000..f3b33ca
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/triggers/Bumped.java
@@ -0,0 +1,34 @@
+package org.usfirst.frc.team3735.robot.triggers;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.cmds.ComTrigger;
+
+public class Bumped extends ComTrigger{
+
+ private Double acc = new Double(1);
+
+ public Bumped(Double acc){
+ this.acc = acc;
+ }
+
+ public Bumped(double acc){
+ this(new Double(acc));
+ }
+
+ public Bumped(){
+ }
+
+ @Override
+ public boolean get() {
+ return Math.abs(Robot.navigation.getXYAcceleration()) > acc;
+ }
+
+ @Override
+ public String getHaltMessage() {
+ return "bumped " + acc + " acc";
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/triggers/DriveOverride.java b/src/org/usfirst/frc/team3735/robot/triggers/DriveOverride.java
new file mode 100644
index 0000000..593f000
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/triggers/DriveOverride.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team3735.robot.triggers;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.cmds.ComTrigger;
+
+public class DriveOverride extends ComTrigger{
+
+
+ public DriveOverride(){
+ }
+
+ @Override
+ public boolean get() {
+ return Robot.oi.isOverriddenByDrive();
+ }
+
+ @Override
+ public String getHaltMessage() {
+ return "overriden by drive";
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/triggers/HasMoved.java b/src/org/usfirst/frc/team3735/robot/triggers/HasMoved.java
new file mode 100644
index 0000000..0e1f92e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/triggers/HasMoved.java
@@ -0,0 +1,97 @@
+package org.usfirst.frc.team3735.robot.triggers;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.settings.Constants;
+import org.usfirst.frc.team3735.robot.util.calc.Integrator;
+import org.usfirst.frc.team3735.robot.util.cmds.ComTrigger;
+import org.usfirst.frc.team3735.robot.util.settings.Func;
+
+
+public class HasMoved extends ComTrigger{
+
+ private Func deltaDistance;
+ private Double rsd;
+ private Double lsd;
+
+ private boolean isIntegrating = false;
+ private Integrator integrator;
+
+ public HasMoved(Func distance){
+ this.deltaDistance = distance;
+ }
+
+ public HasMoved(double distance){
+ this(new Func() {
+ public double getValue() {
+ return distance;
+ }
+ });
+ }
+
+ @Override
+ public void initialize() {
+ if(Constants.Drive.isUsingLeftEncoders) {
+ lsd = Robot.drive.getLeftPosition();
+ }
+ if(Constants.Drive.isUsingRightEncoders) {
+ rsd = Robot.drive.getRightPosition();
+ }
+ if(lsd == null && rsd == null) {
+ isIntegrating = true;
+ integrator = new Integrator(0);
+ integrator.init(Robot.drive.getSpeedInchesFromCurrent());
+ }
+
+ }
+
+ @Override
+ public void execute() {
+ if(isIntegrating) {
+ integrator.feed(Robot.drive.getSpeedInchesFromCurrent());
+ }
+ }
+
+ @Override
+ public boolean get() {
+ if(deltaDistance.getValue() > 0){
+ return distanceTraveled() > deltaDistance.getValue();
+ }else{
+ return distanceTraveled() < deltaDistance.getValue();
+ }
+ }
+
+
+
+ public double distanceTraveled(){
+ if((rsd != null) && (lsd != null)) {
+ return .5 * ((Robot.drive.getLeftPosition() - lsd) + (Robot.drive.getRightPosition() - rsd));
+ }
+ if(lsd != null) {
+ return Robot.drive.getLeftPosition() - lsd;
+ }
+ if(rsd != null) {
+ return Robot.drive.getRightPosition() - lsd;
+ }
+ if(integrator != null) {
+ return integrator.total;
+ }
+ System.out.println("Has Moved Error: distanceTraveled() called but not initialized");
+ return 0;
+
+ }
+
+ public double distanceToGo(){
+ return deltaDistance.getValue() - distanceTraveled();
+ }
+
+ public double distance(){
+ return deltaDistance.getValue();
+ }
+
+ @Override
+ public String getHaltMessage() {
+ return "Moved " + deltaDistance.getValue() + " Inches";
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/triggers/HasPassedWaypoint.java b/src/org/usfirst/frc/team3735/robot/triggers/HasPassedWaypoint.java
new file mode 100644
index 0000000..540049a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/triggers/HasPassedWaypoint.java
@@ -0,0 +1,62 @@
+package org.usfirst.frc.team3735.robot.triggers;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.cmds.ComTrigger;
+import org.usfirst.frc.team3735.robot.util.profiling.Line;
+import org.usfirst.frc.team3735.robot.util.profiling.Location;
+
+public class HasPassedWaypoint extends ComTrigger{
+
+
+ private Location targetLocation;
+ private Location fromLocation;
+ public Line toCross;
+ private boolean isTopRightofLine;
+
+ public HasPassedWaypoint(Location loc){
+ this(loc, null);
+ }
+
+ public HasPassedWaypoint(Location target, Location from){
+ this.targetLocation = target;
+ fromLocation = from;
+ }
+
+
+ @Override
+ public void initialize() {
+ Location from;
+ if(fromLocation == null) {
+ from = Robot.navigation.getPosition();
+ }else {
+ from = fromLocation;
+ }
+ Line toWaypoint = new Line(from, targetLocation);
+ toCross = toWaypoint.getPerpendicular(targetLocation);
+ isTopRightofLine = evaluateLocation();
+ }
+
+
+
+ private boolean evaluateLocation() {
+ Location currentPos = Robot.navigation.getPosition();
+ if(toCross.isVertical) {
+ return currentPos.x > toCross.xOffset;
+ }else {
+ return currentPos.y > toCross.func(currentPos.x);
+ }
+ }
+
+
+
+ @Override
+ public boolean get() {
+ return evaluateLocation() != isTopRightofLine;
+ }
+
+ @Override
+ public String getHaltMessage() {
+ return "Passed Waypoint (" + targetLocation.x + ", " + targetLocation.y + ")";
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/triggers/HasReachedAngle.java b/src/org/usfirst/frc/team3735/robot/triggers/HasReachedAngle.java
new file mode 100644
index 0000000..aab94b6
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/triggers/HasReachedAngle.java
@@ -0,0 +1,81 @@
+package org.usfirst.frc.team3735.robot.triggers;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.calc.VortxMath;
+import org.usfirst.frc.team3735.robot.util.cmds.ComTrigger;
+
+public class HasReachedAngle extends ComTrigger{
+
+ private Double targetAngle;
+ private double startAngle;
+ private Double deltaAngle;
+ private boolean isRelative;
+
+ public HasReachedAngle(Double angle){
+ this(angle, false);
+ }
+
+ public HasReachedAngle(double angle){
+ this(new Double(angle));
+ }
+
+ public HasReachedAngle(Double angle, boolean isRelative){
+ this.isRelative = isRelative;
+ if(isRelative){
+ this.deltaAngle = angle;
+ }else{
+ this.targetAngle = angle;
+ }
+
+ }
+
+ public HasReachedAngle(double angle, boolean isRelative){
+ this(new Double(angle), isRelative);
+ }
+
+ @Override
+ public void initialize() {
+ startAngle = Robot.navigation.getYaw();
+ if(isRelative){
+ targetAngle = limit(startAngle + deltaAngle);
+ }else{
+ deltaAngle = degreesToGo();
+ }
+ }
+
+ @Override
+ public boolean get() {
+ if(deltaAngle > 0){
+ return degreesToGo() < 0;
+ }else{
+ return degreesToGo() > 0;
+ }
+ }
+
+ public double degreesTraveled(){
+ return limit(Robot.navigation.getYaw() - startAngle);
+ }
+
+ public double degreesToGo(){
+ return limit(targetAngle - Robot.navigation.getYaw());
+ }
+
+ public double limit(double a){
+ return VortxMath.continuousLimit(a, -180, 180);
+ }
+
+ public void setTargetAngle(double angle){
+ targetAngle = limit(angle);
+ }
+
+ public boolean isRelative(){
+ return isRelative;
+ }
+
+ @Override
+ public String getHaltMessage() {
+ return "Reached an Angle";
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/triggers/HasReachedSpeed.java b/src/org/usfirst/frc/team3735/robot/triggers/HasReachedSpeed.java
new file mode 100644
index 0000000..2c58d8f
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/triggers/HasReachedSpeed.java
@@ -0,0 +1,37 @@
+package org.usfirst.frc.team3735.robot.triggers;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.cmds.ComTrigger;
+
+public class HasReachedSpeed extends ComTrigger{
+
+ private Double targetSpeed;
+ private boolean isLessThan = true;
+
+ public HasReachedSpeed(Double spd){
+ this.targetSpeed = spd;
+ }
+
+ public HasReachedSpeed(double spd){
+ this(new Double(spd));
+ }
+
+ @Override
+ public void initialize() {
+ isLessThan = evaluateSpeed();
+ }
+
+ public boolean evaluateSpeed() {
+ return Robot.drive.getAverageSpeed() < targetSpeed;
+ }
+ @Override
+ public boolean get() {
+ return evaluateSpeed() != isLessThan;
+ }
+
+ @Override
+ public String getHaltMessage() {
+ return "Reached speed " + targetSpeed + "Inches/second";
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/triggers/TimeOut.java b/src/org/usfirst/frc/team3735/robot/triggers/TimeOut.java
new file mode 100644
index 0000000..c9dc0f0
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/triggers/TimeOut.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team3735.robot.triggers;
+
+import org.usfirst.frc.team3735.robot.Robot;
+import org.usfirst.frc.team3735.robot.util.cmds.ComTrigger;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class TimeOut extends ComTrigger{
+
+ private Double timeout;
+ private Command parent;
+
+ public TimeOut(Double timeout, Command p){
+ this.timeout = timeout;
+ parent = p;
+ }
+
+ public TimeOut(double acc, Command p){
+ this(new Double(acc), p);
+ }
+
+
+ @Override
+ public boolean get() {
+ return parent.timeSinceInitialized() > timeout;
+ }
+
+ @Override
+ public String getHaltMessage() {
+ return "timed out by " + timeout + "seconds";
+ }
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/PIDCtrl.java b/src/org/usfirst/frc/team3735/robot/util/PIDCtrl.java
new file mode 100644
index 0000000..aa2cadd
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/PIDCtrl.java
@@ -0,0 +1,80 @@
+package org.usfirst.frc.team3735.robot.util;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+
+public class PIDCtrl extends PIDController{
+
+ private double iZone = 0;
+ private double actingI;
+ private boolean isUsingIZone = false;
+
+ public PIDCtrl(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
+ super(Kp, 0, Kd, source, output, period);
+ actingI = Ki;
+ }
+ public PIDCtrl(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+ super(Kp, 0, Kd, source, output);
+ actingI = Ki;
+ }
+ public PIDCtrl(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output, double period) {
+ super(Kp, 0, Kd, Kf, source, output, period);
+ actingI = Ki;
+ }
+ public PIDCtrl(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
+ super(Kp, 0, Kd, Kf, source, output);
+ actingI = Ki;
+ }
+
+ /*
+ * these methods set and update the I-zone for the controller
+
+ */
+
+ public synchronized void setIZone(double izone){
+ iZone = izone;
+ }
+
+ public synchronized void setIsUsingIZone(boolean b){
+ isUsingIZone = b;
+ }
+
+ public synchronized void updateI(double i){
+ actingI = i;
+ if(isUsingIZone){
+ if(Math.abs(super.getError()) < iZone){
+ super.setPID(super.getP(), actingI, super.getD());
+ }else{
+ super.setPID(super.getP(), 0, super.getD());
+ }
+ }else{
+ super.setPID(super.getP(), actingI, super.getD());
+ }
+
+ }
+
+
+ /*
+ * these methods route to the controller, manipulating instead the acting I
+ */
+
+ public synchronized void setPID(double p, double i, double d) {
+ actingI = i;
+ super.setPID(p, 0, d);
+ }
+
+
+ public synchronized void setPID(double p, double i, double d, double f) {
+ actingI = i;
+ super.setPID(p, 0, d, f);
+ }
+
+
+
+
+
+
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/bases/TorqueIterative.java b/src/org/usfirst/frc/team3735/robot/util/bases/TorqueIterative.java
new file mode 100644
index 0000000..03d4f53
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/bases/TorqueIterative.java
@@ -0,0 +1,316 @@
+package org.usfirst.frc.team3735.robot.util.bases;
+
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.hal.HAL;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import java.util.Timer;
+import java.util.TimerTask;
+
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
+
+
+
+/**
+ * @author TexasTorque
+ *
+ * A modified version of the WPILIBJ IterativeRobot template that uses
+ * two threads.
+ *
+ * CPU usage on the roboRio may be higher than when using the regular
+ * IterativeRobot base class, but should not be a problem.
+ *
+ */
+public abstract class TorqueIterative extends RobotBase {
+
+ private volatile boolean m_disabledInitialized;
+ private volatile boolean m_autonomousInitialized;
+ private volatile boolean m_teleopInitialized;
+ private volatile boolean m_testInitialized;
+
+ Thread periodicThread;
+ Timer continousTimer;
+ // period is 1 / frequency
+ double continuousPeriod = 1.0 / 100.0;
+
+ /**
+ * Create a new iterative robot.
+ */
+ public TorqueIterative() {
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+
+ /**
+ * Unused.
+ */
+ protected void prestart() {
+ // Don't immediately say that the robot's ready to be enabled.
+ // See below.
+ }
+
+ /**
+ *
+ * Overrides method from RobotBase. This is called by main and runs the user
+ * robot code.
+ */
+ @Override
+ public void startCompetition() {
+ //UsageReporting.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
+ HAL.report(tResourceType.kResourceType_Framework,
+ tInstances.kFramework_Iterative);
+ robotInit();
+ //FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramStarting();
+ HAL.observeUserProgramStarting();
+
+ LiveWindow.setEnabled(false);
+
+ // Start a new Thread to run the regular Periodic code on.
+ periodicThread = new Thread(new Periodic());
+ periodicThread.start();
+
+ // Shcedule the second thread to run at the period specified above.
+ continousTimer = new Timer();
+ continousTimer.scheduleAtFixedRate(new Continuous(), 0, (long) (1000 * continuousPeriod));
+
+ // Prevent return from startcompetition
+ while (true) {
+ try {
+ Thread.sleep(1000);
+ } catch (InterruptedException ex) {
+ }
+ }
+ }
+
+ /**
+ * This class provides the default IterativeRobot functionality.
+ *
+ * It is synchronized to inputs coming from the driver station, and runs at
+ * about 50 hz.
+ */
+ private class Periodic implements Runnable {
+
+ @Override
+ public void run() {
+ while (true) {
+ m_ds.waitForData();
+
+ if (isDisabled()) {
+ if (!m_disabledInitialized) {
+ LiveWindow.setEnabled(false);
+ disabledInit();
+
+ m_disabledInitialized = true;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ if (nextPeriodReady()) {
+ //FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramDisabled();
+ HAL.observeUserProgramDisabled();
+
+ disabledPeriodic();
+ }
+ } else if (isTest()) {
+ if (!m_testInitialized) {
+ LiveWindow.setEnabled(true);
+ testInit();
+
+ m_testInitialized = true;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_disabledInitialized = false;
+ }
+ if (nextPeriodReady()) {
+ //FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramTest();
+ HAL.observeUserProgramTest();
+ testPeriodic();
+ }
+ } else if (isAutonomous()) {
+ if (!m_autonomousInitialized) {
+ LiveWindow.setEnabled(false);
+ autonomousInit();
+
+ m_autonomousInitialized = true;
+ m_testInitialized = false;
+ m_teleopInitialized = false;
+ m_disabledInitialized = false;
+ }
+ if (nextPeriodReady()) {
+ //FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramAutonomous();
+ HAL.observeUserProgramAutonomous();
+ autonomousPeriodic();
+ }
+ } else {
+ if (!m_teleopInitialized) {
+ LiveWindow.setEnabled(false);
+ teleopInit();
+
+ m_teleopInitialized = true;
+ m_testInitialized = false;
+ m_autonomousInitialized = false;
+ m_disabledInitialized = false;
+ }
+ if (nextPeriodReady()) {
+ //FRCNetworkCommunicationsLibrary.FRCNetworkCommunicationObserveUserProgramTeleop();
+ HAL.observeUserProgramTeleop();
+ teleopPeriodic();
+ }
+ }
+ }
+ }
+ }
+
+ /**
+ * This class provides an extra execution thread to take advantage of the
+ * two cores of the roboRIO.
+ *
+ * It is scheduled to run at 100hz.
+ */
+ private class Continuous extends TimerTask {
+
+ @Override
+ public void run() {
+ if (isAutonomous() && m_autonomousInitialized) {
+ autonomousContinuous();
+ robotContinuous();
+ } else if (isOperatorControl() && m_teleopInitialized) {
+ teleopContinuous();
+ robotContinuous();
+ } else if (isDisabled() && m_disabledInitialized) {
+ disabledContinuous();
+ robotContinuous();
+ }
+
+ }
+ }
+
+ private boolean nextPeriodReady() {
+ return m_ds.isNewControlData();
+ }
+
+ /* ----------- Overridable continuous code ----------------- */
+ private boolean tpcFirstRun = true;
+
+ public void teleopContinuous() {
+ if (tpcFirstRun) {
+ System.out.println("Default TorqueIterative.teleopContinuous() method... Overload me!");
+ tpcFirstRun = false;
+ }
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException ex) {
+ }
+ }
+
+ private boolean apcFirstRun = true;
+
+ public void autonomousContinuous() {
+ if (apcFirstRun) {
+ System.out.println("Default TorqueIterative.autonomousContinuous() method... Overload me!");
+ apcFirstRun = false;
+ }
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException ex) {
+ }
+ }
+
+ private boolean dcFirstRun = true;
+
+ public void disabledContinuous() {
+ if (dcFirstRun) {
+ System.out.println("Default TorqueIterative.disabledContinuous() method... Overload me!");
+ dcFirstRun = false;
+ }
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException ex) {
+ }
+ }
+
+ private boolean ecFirstRun = true;
+
+ public void robotContinuous() {
+ if (ecFirstRun) {
+ System.out.println("Default TorqueIterative.alwaysContinuous() method... Overload me!");
+ ecFirstRun = false;
+ }
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException ex) {
+ }
+ }
+
+ /* ----------- Overridable initialization code ----------------- */
+ public void robotInit() {
+ System.out.println("Default TorqueIterative.robotInit() method... Overload me!");
+ }
+
+ public void disabledInit() {
+ System.out.println("Default TorqueIterative.disabledInit() method... Overload me!");
+ }
+
+ public void autonomousInit() {
+ System.out.println("Default TorqueIterative.autonomousInit() method... Overload me!");
+ }
+
+ public void teleopInit() {
+ System.out.println("Default TorqueIterative.teleopInit() method... Overload me!");
+ }
+
+ public void testInit() {
+ System.out.println("Default TorqueIterative.testInit() method... Overload me!");
+ }
+
+ /* ----------- Overridable periodic code ----------------- */
+ private boolean dpFirstRun = true;
+
+ public void disabledPeriodic() {
+ if (dpFirstRun) {
+ System.out.println("Default TorqueIterative.disabledPeriodic() method... Overload me!");
+ dpFirstRun = false;
+ }
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException ex) {
+ }
+ }
+
+ private boolean apFirstRun = true;
+
+ public void autonomousPeriodic() {
+ if (apFirstRun) {
+ System.out.println("Default TorqueIterative.autonomousPeriodic() method... Overload me!");
+ apFirstRun = false;
+ }
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException ex) {
+ }
+ }
+
+ private boolean tpFirstRun = true;
+
+ public void teleopPeriodic() {
+ if (tpFirstRun) {
+ System.out.println("Default TorqueIterative.teleopPeriodic() method... Overload me!");
+ tpFirstRun = false;
+ }
+ try {
+ Thread.sleep(1);
+ } catch (InterruptedException ex) {
+ }
+ }
+
+ private boolean tmpFirstRun = true;
+
+ public void testPeriodic() {
+ if (tmpFirstRun) {
+ System.out.println("Default TorqueIterative.testPeriodic() method... Overload me!");
+ tmpFirstRun = false;
+ }
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/bases/VortxIterative.java b/src/org/usfirst/frc/team3735/robot/util/bases/VortxIterative.java
new file mode 100644
index 0000000..82977e5
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/bases/VortxIterative.java
@@ -0,0 +1,349 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package org.usfirst.frc.team3735.robot.util.bases;
+
+import java.util.Timer;
+import java.util.TimerTask;
+
+
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.wpilibj.hal.HAL;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * IterativeRobot implements a specific type of Robot Program framework, extending the RobotBase
+ * class.
+ *
+ * The IterativeRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ *
This class is intended to implement the "old style" default code, by providing the following
+ * functions which are called by the main loop, startCompetition(), at the appropriate times:
+ *
+ *
robotInit() -- provide for initialization at robot power-on
+ *
+ *
init() functions -- each of the following functions is called once when the appropriate mode
+ * is entered: - DisabledInit() -- called only when first disabled - AutonomousInit() -- called each
+ * and every time autonomous is entered from another mode - TeleopInit() -- called each and every
+ * time teleop is entered from another mode - TestInit() -- called each and every time test mode is
+ * entered from anothermode
+ *
+ *
Periodic() functions -- each of these functions is called iteratively at the appropriate
+ * periodic rate (aka the "slow loop"). The period of the iterative robot is synced to the driver
+ * station control packets, giving a periodic frequency of about 50Hz (50 times per second). -
+ * disabledPeriodic() - autonomousPeriodic() - teleopPeriodic() - testPeriodoc()
+ */
+public class VortxIterative extends RobotBase {
+ private boolean m_disabledInitialized;
+ private boolean m_autonomousInitialized;
+ private boolean m_teleopInitialized;
+ private boolean m_testInitialized;
+
+ /**
+ * Constructor for RobotIterativeBase.
+ *
+ *
The constructor initializes the instance variables for the robot to indicate the status of
+ * initialization for disabled, autonomous, and teleop code.
+ */
+ public VortxIterative() {
+ // set status for initialization of disabled, autonomous, and teleop code.
+ m_disabledInitialized = false;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+
+ public static enum Side{
+ Left,Right
+ }
+
+ /**
+ * Provide an alternate "main loop" via startCompetition().
+ */
+ public void startCompetition() {
+ HAL.report(tResourceType.kResourceType_Framework,
+ tInstances.kFramework_Iterative);
+
+ robotInit();
+
+ // Tell the DS that the robot is ready to be enabled
+ HAL.observeUserProgramStarting();
+
+ // loop forever, calling the appropriate mode-dependent function
+ LiveWindow.setEnabled(false);
+ Timer continousTimer = new Timer();
+ continousTimer.scheduleAtFixedRate(new Continuous(), 0, (long) (10));
+ while (true) {
+ // Wait for new data to arrive
+ m_ds.waitForData();
+ // Call the appropriate function depending upon the current robot mode
+ if (isDisabled()) {
+ // call DisabledInit() if we are now just entering disabled mode from
+ // either a different mode or from power-on
+ if (!m_disabledInitialized) {
+ LiveWindow.setEnabled(false);
+ disabledInit();
+ m_disabledInitialized = true;
+ // reset the initialization flags for the other modes
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_testInitialized = false;
+ }
+ HAL.observeUserProgramDisabled();
+ disabledPeriodic();
+ } else if (isTest()) {
+ // call TestInit() if we are now just entering test mode from either
+ // a different mode or from power-on
+ if (!m_testInitialized) {
+ LiveWindow.setEnabled(true);
+ testInit();
+ m_testInitialized = true;
+ m_autonomousInitialized = false;
+ m_teleopInitialized = false;
+ m_disabledInitialized = false;
+ }
+ HAL.observeUserProgramTest();
+ testPeriodic();
+ } else if (isAutonomous()) {
+ // call Autonomous_Init() if this is the first time
+ // we've entered autonomous_mode
+ if (!m_autonomousInitialized) {
+ LiveWindow.setEnabled(false);
+ // KBS NOTE: old code reset all PWMs and relays to "safe values"
+ // whenever entering autonomous mode, before calling
+ // "Autonomous_Init()"
+ autonomousInit();
+ m_autonomousInitialized = true;
+ m_testInitialized = false;
+ m_teleopInitialized = false;
+ m_disabledInitialized = false;
+ }
+ HAL.observeUserProgramAutonomous();
+ autonomousPeriodic();
+ } else {
+ // call Teleop_Init() if this is the first time
+ // we've entered teleop_mode
+ if (!m_teleopInitialized) {
+ LiveWindow.setEnabled(false);
+ teleopInit();
+ m_teleopInitialized = true;
+ m_testInitialized = false;
+ m_autonomousInitialized = false;
+ m_disabledInitialized = false;
+ }
+ HAL.observeUserProgramTeleop();
+ teleopPeriodic();
+ }
+ robotPeriodic();
+ }
+ }
+
+ /* ----------- Overridable initialization code ----------------- */
+
+ /**
+ * Robot-wide initialization code should go here.
+ *
+ *
Users should override this method for default Robot-wide initialization which will be called
+ * when the robot is first powered on. It will be called exactly one time.
+ *
+ *
Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
+ * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
+ * never indicate that the code is ready, causing the robot to be bypassed in a match.
+ */
+ public void robotInit() {
+ System.out.println("Default IterativeRobot.robotInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for disabled mode should go here.
+ *
+ *
Users should override this method for initialization code which will be called each time the
+ * robot enters disabled mode.
+ */
+ public void disabledInit() {
+ System.out.println("Default IterativeRobot.disabledInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for autonomous mode should go here.
+ *
+ *
Users should override this method for initialization code which will be called each time the
+ * robot enters autonomous mode.
+ */
+ public void autonomousInit() {
+ System.out.println("Default IterativeRobot.autonomousInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for teleop mode should go here.
+ *
+ *
Users should override this method for initialization code which will be called each time the
+ * robot enters teleop mode.
+ */
+ public void teleopInit() {
+ System.out.println("Default IterativeRobot.teleopInit() method... Overload me!");
+ }
+
+ /**
+ * Initialization code for test mode should go here.
+ *
+ *
Users should override this method for initialization code which will be called each time the
+ * robot enters test mode.
+ */
+ @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+ public void testInit() {
+ System.out.println("Default IterativeRobot.testInit() method... Overload me!");
+ }
+
+ /* ----------- Overridable periodic code ----------------- */
+
+ private boolean m_rpFirstRun = true;
+
+ /**
+ * Periodic code for all robot modes should go here.
+ *
+ *
This function is called each time a new packet is received from the driver station.
+ *
+ *
Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to
+ * network timing variability and the function may not be called at all if the Driver Station is
+ * disconnected. For most use cases the variable timing will not be an issue. If your code does
+ * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+ public void robotPeriodic() {
+ if (m_rpFirstRun) {
+ System.out.println("Default IterativeRobot.robotPeriodic() method... Overload me!");
+ m_rpFirstRun = false;
+ }
+ }
+
+ private boolean m_dpFirstRun = true;
+
+ /**
+ * Periodic code for disabled mode should go here.
+ *
+ *
Users should override this method for code which will be called each time a new packet is
+ * received from the driver station and the robot is in disabled mode.
+ *
+ *
Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to
+ * network timing variability and the function may not be called at all if the Driver Station is
+ * disconnected. For most use cases the variable timing will not be an issue. If your code does
+ * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+ public void disabledPeriodic() {
+ if (m_dpFirstRun) {
+ System.out.println("Default IterativeRobot.disabledPeriodic() method... Overload me!");
+ m_dpFirstRun = false;
+ }
+ }
+
+ private boolean m_apFirstRun = true;
+
+ /**
+ * Periodic code for autonomous mode should go here.
+ *
+ *
Users should override this method for code which will be called each time a new packet is
+ * received from the driver station and the robot is in autonomous mode.
+ *
+ *
Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to
+ * network timing variability and the function may not be called at all if the Driver Station is
+ * disconnected. For most use cases the variable timing will not be an issue. If your code does
+ * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+ public void autonomousPeriodic() {
+ if (m_apFirstRun) {
+ System.out.println("Default IterativeRobot.autonomousPeriodic() method... Overload me!");
+ m_apFirstRun = false;
+ }
+ }
+
+ private boolean m_tpFirstRun = true;
+
+ /**
+ * Periodic code for teleop mode should go here.
+ *
+ *
Users should override this method for code which will be called each time a new packet is
+ * received from the driver station and the robot is in teleop mode.
+ *
+ *
Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to
+ * network timing variability and the function may not be called at all if the Driver Station is
+ * disconnected. For most use cases the variable timing will not be an issue. If your code does
+ * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+ public void teleopPeriodic() {
+ if (m_tpFirstRun) {
+ System.out.println("Default IterativeRobot.teleopPeriodic() method... Overload me!");
+ m_tpFirstRun = false;
+ }
+ }
+
+ private boolean m_tmpFirstRun = true;
+
+ /**
+ * Periodic code for test mode should go here.
+ *
+ *
Users should override this method for code which will be called each time a new packet is
+ * received from the driver station and the robot is in test mode.
+ *
+ *
Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to
+ * network timing variability and the function may not be called at all if the Driver Station is
+ * disconnected. For most use cases the variable timing will not be an issue. If your code does
+ * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
+ */
+ @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+ public void testPeriodic() {
+ if (m_tmpFirstRun) {
+ System.out.println("Default IterativeRobot.testPeriodic() method... Overload me!");
+ m_tmpFirstRun = false;
+ }
+ }
+
+ /**
+ * This class provides an extra execution thread to take advantage of the
+ * two cores of the roboRIO.
+ *
+ * It is scheduled to run at 100hz.
+ */
+ private class Continuous extends TimerTask {
+
+ @Override
+ public void run() {
+ if (isAutonomous() && m_autonomousInitialized) {
+ autonomousContinuous();
+ robotContinuous();
+ } else if (isOperatorControl() && m_teleopInitialized) {
+ teleopContinuous();
+ robotContinuous();
+ } else if (isDisabled() && m_disabledInitialized) {
+ disabledContinuous();
+ robotContinuous();
+ }
+
+ }
+ }
+
+ public void autonomousContinuous() {
+ // TODO Auto-generated method stub
+
+ }
+
+ public void disabledContinuous() {
+ // TODO Auto-generated method stub
+
+ }
+
+ public void teleopContinuous() {
+ // TODO Auto-generated method stub
+
+ }
+
+ public void robotContinuous() {
+ // TODO Auto-generated method stub
+
+ }
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/AccLimiter.java b/src/org/usfirst/frc/team3735/robot/util/calc/AccLimiter.java
new file mode 100644
index 0000000..4b65357
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/AccLimiter.java
@@ -0,0 +1,17 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+public class AccLimiter {
+ public double value;
+ private Range range;
+
+ public AccLimiter(double val, Range lim){
+ this.value = val;
+ this.range = lim;
+ }
+
+ public double feed(double newVal){
+ double dy = newVal - value;
+ value+= range.limit(dy);
+ return value;
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/DDxLimiter.java b/src/org/usfirst/frc/team3735/robot/util/calc/DDxLimiter.java
new file mode 100644
index 0000000..3e32e6e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/DDxLimiter.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+import java.util.Arrays;
+
+public class DDxLimiter {
+ public double value;
+ public double dyValue;
+ private Range range;
+
+ private DDxLimiter derivative;
+
+ public DDxLimiter(double val, Range... ranges){
+ this.value = val;
+ range = ranges[0];
+ if(ranges.length > 1){
+ Range[] newRanges = new Range[ranges.length-1];
+ for(int i = 0; i < newRanges.length; i++){
+ newRanges[i] = ranges[i+1];
+ }
+ derivative = new DDxLimiter(0, newRanges);
+ }
+ }
+
+ public double feed(double newVal){
+
+ if(derivative != null){
+ dyValue = derivative.feed(newVal - value);
+ }else{
+ dyValue = newVal - value;
+ }
+
+ value += range.limit(dyValue);
+
+ return value;
+ }
+
+ public void reset(){
+ reset(0);
+ }
+
+ public void reset(double num) {
+ value = num;
+ if(derivative != null){
+ derivative.reset();
+ }
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/Integrator.java b/src/org/usfirst/frc/team3735/robot/util/calc/Integrator.java
new file mode 100644
index 0000000..5977cbc
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/Integrator.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class Integrator {
+ public double total = 0;
+ public double lastTime;
+ public double lastVal;
+
+
+ public Integrator(double start) {
+ total = start;
+ }
+
+ public Integrator() {
+ this(0);
+ }
+
+ public void init(double val) {
+ lastVal = val;
+ lastTime = Timer.getFPGATimestamp();
+ }
+
+ /**
+ * Uses right sum integration currently. LastVal is in there if we wish to add triangles
+ * to integration pattern later.
+ * @param value the value to integrate
+ */
+ public void feed(double value) {
+ double curTime = Timer.getFPGATimestamp();
+ total += value * (curTime - lastTime);
+
+ lastVal = value;
+ lastTime = curTime;
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/JerkLimiter.java b/src/org/usfirst/frc/team3735/robot/util/calc/JerkLimiter.java
new file mode 100644
index 0000000..8d7e98e
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/JerkLimiter.java
@@ -0,0 +1,35 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+public class JerkLimiter {
+ public double value;
+ public double dyValue;
+ private Range range;
+ private Range accRange;
+
+ public JerkLimiter(double val, Range lim, Range acclim){
+ this.value = val;
+ this.range = lim;
+ this.accRange = acclim;
+ }
+
+ public double feed(double newVal){
+ double dy = newVal - value;
+
+ double ddy = dy - dyValue;
+ dyValue += accRange.limit(ddy);
+
+ value += range.limit(dyValue);
+
+ return value;
+ }
+
+ public void reset(){
+ value = 0;
+ dyValue = 0;
+ }
+
+ public void reset(double val){
+ value = val;
+ dyValue = 0;
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/Range.java b/src/org/usfirst/frc/team3735/robot/util/calc/Range.java
new file mode 100644
index 0000000..037eaae
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/Range.java
@@ -0,0 +1,60 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+import org.usfirst.frc.team3735.robot.util.settings.Func;
+
+public class Range {
+
+ Func lower, upper;
+
+ public Range(double lower, double upper){
+ this.lower = new Func(){
+ public double getValue(){
+ return lower;
+ }
+ };
+
+ this.upper = new Func(){
+ public double getValue(){
+ return upper;
+ }
+ };
+ }
+ public Range(Func lower, Func upper){
+ this.lower = lower;
+ this.upper = upper;
+ }
+
+ public Range(boolean flag, double upper){
+ this.lower = Func.getFunc(Double.NEGATIVE_INFINITY);
+ this.upper = Func.getFunc(upper);
+ }
+
+ public Range(double lower, boolean flag){
+ this.lower = Func.getFunc(lower);
+ this.upper = Func.getFunc(Double.POSITIVE_INFINITY);
+ }
+
+ public Range(double lim){
+ this(-lim, lim);
+ }
+
+ public Range(Func lim){
+ lower = new Func(){
+ @Override
+ public double getValue(){
+ return lim.getValue() * -1;
+ }
+ };
+ upper = lim;
+ }
+
+ public double limit(double num){
+ if(num < lower.getValue()){
+ return lower.getValue();
+ }
+ if(num > upper.getValue()){
+ return upper.getValue();
+ }
+ return num;
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/RollingAverage.java b/src/org/usfirst/frc/team3735/robot/util/calc/RollingAverage.java
new file mode 100644
index 0000000..2649d36
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/RollingAverage.java
@@ -0,0 +1,51 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+public class RollingAverage {
+
+ private static final double FRAMERATE = 50;
+ private Double[] numbers;
+ int i;
+ int size;
+ double average;
+ double sum;
+
+ public RollingAverage(){
+ this(1);
+ }
+
+ public RollingAverage(double time){
+ size = (int)Math.round(FRAMERATE * time);
+ numbers = new Double[size];
+ i = 0;
+ sum = 0;
+ }
+
+
+ public void compute(){
+ if(numbers[i] != null){
+ sum-= numbers[i];
+ }
+ numbers[i] = get();
+ sum += numbers[i];
+
+ i++;
+ if(i >= size){
+ i = 0;
+ }
+ }
+
+ public double getAverage(){
+ return sum / (double)size;
+ }
+
+ public void reset(){
+ numbers = new Double[size];
+ sum = 0;
+ }
+
+ public double get(){
+ return 0;
+ }
+
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/TrapProfile.java b/src/org/usfirst/frc/team3735/robot/util/calc/TrapProfile.java
new file mode 100644
index 0000000..a4e8dc3
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/TrapProfile.java
@@ -0,0 +1,67 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+public class TrapProfile {
+ private final double fps = 50;
+
+ private double acc;
+ private double cruiseSpeed;
+ private double currentSpeed;
+
+ //0 = accelerating, 1 = cruising, 2 = decelerating, 3 = finished
+ private int stage;
+
+ private double start;
+ private double finish;
+ private double direction = 1;
+ private double accThresh = 0;
+
+ public TrapProfile(double start, double finish, double v, double a){
+ this.start = start;
+ this.finish = finish;
+ cruiseSpeed = v;
+ acc = a/fps;
+ currentSpeed = 0;
+ stage = 0;
+ if(finish < start){
+ direction = -1;
+ }
+ }
+
+ public void step(double input){
+ if(stage == 0){
+ currentSpeed += acc;
+ if(currentSpeed >= cruiseSpeed){
+ currentSpeed = cruiseSpeed;
+ accThresh = input - start;
+ stage = 1; //start cruising
+ }
+ }else if(stage == 1){
+ if(finish - input < accThresh){
+ stage = 2;
+ }
+ }else if(stage == 2){
+ currentSpeed -= acc;
+ if(currentSpeed <= 0){
+ currentSpeed = 0;
+ stage = 3; //its finished
+ }
+ }
+ }
+
+ public double getOutput(){
+ return speedToOutput(currentSpeed) * direction;
+ }
+
+ public boolean isFinished(){
+ return (stage == 3);
+ }
+ public boolean isCruising(){
+ return (stage == 1);
+ }
+
+ //Override Me!!!
+ public double speedToOutput(double v){
+ return v;
+ }
+
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/calc/VortxMath.java b/src/org/usfirst/frc/team3735/robot/util/calc/VortxMath.java
new file mode 100644
index 0000000..b790725
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/calc/VortxMath.java
@@ -0,0 +1,83 @@
+package org.usfirst.frc.team3735.robot.util.calc;
+
+public class VortxMath {
+
+ /**
+ * Checks if a value is by a target
+ *
+ * @param value the value to check
+ * @param target the target that the value should be near
+ * @param threshold the maximum distance from the target the value can be
+ */
+ public static boolean isWithinThreshold(double value, double target, double threshold){
+ return Math.abs(target - value) < threshold;
+ }
+
+ /**
+ * limits a value to a certain range
+ *
+ * @param value the value to limit with the bounds
+ * @param lower the lower bound
+ * @param upper the upper bound
+ */
+ public static double limit(double value, double lower, double upper) {
+ if(lower > upper){
+ System.out.println("Vortx Math Limit Error");
+ }
+ if(value < lower){
+ value = lower;
+ }else if(value > upper){
+ value = upper;
+ }
+ return value;
+ }
+
+ /**
+ * Modifies a number to meet a certain range. For example, a motor
+ * that needs angle values in the range (0,360)
+ * @param value the value to be modified
+ * @param minValue the minimum value for the continuous range
+ * @param maxValue the maximum value for the continuous range. Equivalent to the minValue
+ * @return the same value but in the limited range
+ */
+ public static double continuousLimit(double value, double minValue, double maxValue){
+ while(value < minValue){
+ value += (maxValue - minValue);
+ }
+ while(value > maxValue){
+ value -= (maxValue - minValue);
+ }
+ return value;
+ }
+
+ public static double navLimit(double value) {
+ return continuousLimit(value, -180, 180);
+ }
+
+ /**
+ * Curves a value with fixed points at -1 and 1
+ * Higher exponents reduce the curve below the y = x line
+ * Lower exponents raise the curve
+ *
+ * @param value the value to be curved
+ * @param exponent the exponent to raise it to
+ * @return the modified value
+ */
+ public static double curve(double value, double exponent){
+ return value * Math.pow(Math.abs(value), exponent - 1);
+ //return Math.signum(value) * Math.pow(Math.abs(value), exponent);
+ }
+
+ /**
+ * Does the same as {@link #curve(double, double)}, except that stagnant is
+ * the stationary value.
+ *
+ * @param value the value to be modified
+ * @param exponent the exponent to raise it to
+ * @param stagnant the the stationary value. Can be positive or negative
+ * @return the curved value
+ */
+ public static double curveAround(double value, double exponent, double stagnant){
+ return curve(value / stagnant, exponent) * stagnant;
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/cmds/ComAssist.java b/src/org/usfirst/frc/team3735/robot/util/cmds/ComAssist.java
new file mode 100644
index 0000000..00fcb38
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/cmds/ComAssist.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3735.robot.util.cmds;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+public class ComAssist {
+
+ public ArrayList requirements = new ArrayList();
+
+ public void initialize(){
+
+ }
+ public void execute(){
+
+ }
+ public void end(){
+
+ }
+
+ public void requires(Subsystem s){
+ requirements.add(s);
+ }
+}
diff --git a/src/org/usfirst/frc/team3735/robot/util/cmds/ComTrigger.java b/src/org/usfirst/frc/team3735/robot/util/cmds/ComTrigger.java
new file mode 100644
index 0000000..00d180a
--- /dev/null
+++ b/src/org/usfirst/frc/team3735/robot/util/cmds/ComTrigger.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team3735.robot.util.cmds;
+
+import java.util.ArrayList;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/*
+ * A class that can be added to a VortxCommand.
+ * when get returns true, the command will halt.
+ * For example, a trigger for moving a certain distance would record the initial
+ * distances on initialize, and get() will return true when it is finished
+ */
+public abstract class ComTrigger {
+
+ public ArrayList requirements = new ArrayList