autonomousMode = new SendableChooser<>();
+ SendableChooser autonomousMode = new SendableChooser<>();
+ SendableChooser allianceSelection = new SendableChooser<>();
+
Command autonomousCommand;
+ Command allianceCommand;
Preferences prefs;
-
+ int AllianceSelectionInt;
+
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
@@ -73,19 +104,38 @@ public void robotInit() {
// Subsystem Initialization
- drivetrain = new Drivetrain();
- shooter = new Shooter();
- loader = new Loader();
- Intake_Gear = new Intake_Gear();
- Intake_Ball = new Intake_Ball();
- Climber = new Climber();
-
- oi = new OI(); // Initializes the OI.
- // This MUST BE LAST or a NullPointerException will be thrown
-
-// UsbCamera ucamera=CameraServer.getInstance().startAutomaticCapture("cam0", 0);
-// AxisCamera camera=CameraServer.getInstance().addAxisCamera("cam1", "10.7.8.11");
+ drivetrain = new Drivetrain();
+ shooter = new Shooter();
+ intake_ball = new Intake_Ball();
+ intake_gear = new Intake_Gear();
+ pivot_gear = new Pivot_Gear();
+ feeder = new Feeder();
+ led1 = new LED();
+ climber = new Climber();
+ visionLift = new VisionLift();
+ visionBoiler = new VisionBoiler();
+
+ oi = new OI(); // Initializes the OI.
+ // This MUST BE LAST or a NullPointerException will be thrown
+
+ UsbCamera ucamera=CameraServer.getInstance().startAutomaticCapture("cam1", 1);
+ ucamera.setResolution(180, 240);
+
+// pwr0 = new Solenoid(RobotMap.PWR0);
+// pwr1 = new Solenoid(RobotMap.PWR1);
+// pwr2 = new Solenoid(RobotMap.PWR2);
+// pwr3 = new Solenoid(RobotMap.PWR3);
+// gearLight = new Solenoid(RobotMap.GEARLIGHT);
+// boilerLight = new Solenoid(RobotMap.BOILERLIGHT);
+//
+// pwr0.set(true);
+// pwr1.set(true);
+// pwr2.set(true);
+// pwr3.set(true);
+// gearLight.set(true);
+// boilerLight.set(true);
+
sendDashboardSubsystems(); // Sends each subsystem's currently running command to the Smart Dashboard
queueAutonomousModes(); // Adds autonomous modes to the selection box
}
@@ -97,6 +147,32 @@ public void disabledPeriodic() {
Scheduler.getInstance().run();
sendStatistics();
prefs = Preferences.getInstance();
+ /*try {
+ if (ds.isSysActive()){
+ if (ds.isFMSAttached())
+ {
+ alliance = ds.getAlliance();
+ if (ds.getAlliance() == Alliance.Blue){
+ led1.send_to_led(Constants.SET_ALLIANCE_BLUE);
+ allianceColor = Constants.ALLIANCE_BLUE;
+ ledAllianceColor = Constants.SET_ALLIANCE_BLUE;
+ }
+ else if (ds.getAlliance() == Alliance.Red){
+ led1.send_to_led(Constants.SET_ALLIANCE_RED);
+ allianceColor = Constants.ALLIANCE_RED;
+ ledAllianceColor = Constants.SET_ALLIANCE_RED;
+ }
+ else {
+ led1.send_to_led(Constants.SET_ALLIANCE_INVALID);
+ allianceColor = 0;
+ }
+ }
+ }
+ }
+ catch (Exception e)
+ {
+ led1.send_to_led(Constants.MAX_LED_CODE);
+ }*/
}
/**
@@ -104,12 +180,24 @@ public void disabledPeriodic() {
*/
public void autonomousInit() {
-// turnDirection = prefs.getDouble("TurnDirection", 4.0);
-
- // schedule the autonomous command (example)
+// AllianceSelectionDouble = (Double)AllianceSelection.getSelected();
+
+ // schedule the autonomous command (example)
+ allianceCommand = (Command)allianceSelection.getSelected();
+ SmartDashboard.putString("From Alliance Command", allianceCommand.getName());
+
+ if(allianceCommand.getName().equals("BlueAlliance"))
+ allianceColor = Constants.ALLIANCE_BLUE;
+ else
+ allianceColor = Constants.ALLIANCE_RED;
+
+ if (allianceCommand != null)
+ allianceCommand.start();
autonomousCommand = (Command)autonomousMode.getSelected();
if (autonomousCommand != null) autonomousCommand.start();
+ Robot.drivetrain.setGearLight(true);
+ Robot.drivetrain.setBoilerLight(true);
}
/**
@@ -128,7 +216,12 @@ public void teleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
+
if (autonomousCommand != null) autonomousCommand.cancel();
+ drivetrain.toggleBrakeMode();
+
+ Robot.drivetrain.setGearLight(false);
+ Robot.drivetrain.setBoilerLight(false);
}
/**
@@ -136,7 +229,6 @@ public void teleopInit() {
* You can use it to reset subsystems before shutting down.
*/
public void disabledInit() {
-
}
/**
@@ -159,32 +251,55 @@ public void testPeriodic() {
* Sends data from each subsystem periodically as set by sendStatsIntervalSec
*/
private void sendStatistics() {
- if (statsTimer.get() >= Constants.SEND_STATS_INTERVAL) {
- statsTimer.reset();
+ // if (statsTimer.get() >= Constants.SEND_STATS_INTERVAL) {
+ // statsTimer.reset();
// Various debug information
drivetrain.sendToDashboard();
- loader.sendToDashboard();
+ feeder.sendToDashboard();
shooter.sendToDashboard();
- Intake_Ball.sendToDashboard();
- Intake_Gear.sendToDashboard();
- Climber.sendToDashbaord();
-
-// visionProcessor.sendToDashboard();
- }
+ led1.sendToDashboard();
+ climber.sendToDashboard();
+ intake_ball.sendToDashboard();
+ intake_gear.sendToDashboard();
+ // pivot_gear.sendToDashboard();
+// visionProcessor.sendToDashboard();
+ visionLift.sendToDashboard();
+ visionBoiler.sendToDashboard();
+// }
}
-
- /**
+
+ /**
* Adds every autonomous mode to the selection box and adds the box to the Smart Dashboard
*/
private void queueAutonomousModes() {
- autonomousMode.addObject("Drive Straight for time", new DriveStraightForTime(.5, 3));
- autonomousMode.addDefault("Do Nothing", new DoNothing());
-// autonomousMode.addObject("Find Target", new DriveToTarget());
- autonomousMode.addObject("Drive in Square", new DriveInSquare());
+ autonomousMode.addDefault("Do Nothing", new DoNothing());
+ autonomousMode.addObject("Drive Over Line", new driveDistance());
+ autonomousMode.addObject("One Gear Center", new OneGearCenter());
+ autonomousMode.addObject("One Gear Open Side", new OneGearLeft());
+ autonomousMode.addObject("10 Ball", new TenBalls());
+ autonomousMode.addObject("Just 10 Ball", new JustTenBalls());
+ autonomousMode.addObject("60 Ball", new SixtyBalls());
+
+ autonomousMode.addObject("Drive To Lift", new RotateAndDriveToLift());
+ autonomousMode.addObject("Drive to Boiler", new RotateAndDriveToBoiler(AutoConstants.DISTANCE_TO_BOILER_LOCATION2));
+
+// autonomousMode.addObject("Drive to Boiler Location 2", new RotateAndDriveToBoiler(AutoConstants.DISTANCE_TO_BOILER_LOCATION2));
+// autonomousMode.addObject("Rotate And Drive To Gear", new RotateAndDriveToGear());
+
+// autonomousMode.addObject("Drive Straight for time", new DriveStraightForTime(.5, 3));
+// autonomousMode.addObject("Find Target", new FindTarget());
+// autonomousMode.addObject("Drive in Square", new DriveInSquare());
+// autonomousMode.addObject("turn", new turn(allianceColor));
+// autonomousMode.addObject("Realease Gear Test", new AutoGearTest());
+
+ allianceSelection.addDefault("RED", new RedAlliance());
+ allianceSelection.addObject("BLUE", new BlueAlliance());
+
+ SmartDashboard.putData("Alliance Color", allianceSelection);
+ SmartDashboard.putData("Autonomous Selection", autonomousMode);
- SmartDashboard.putData("Autonomous Selection", autonomousMode);
}
/**
@@ -192,12 +307,15 @@ private void queueAutonomousModes() {
*/
private void sendDashboardSubsystems() {
SmartDashboard.putData(shooter);
- SmartDashboard.putData(loader);
+ SmartDashboard.putData(feeder);
SmartDashboard.putData(drivetrain);
- SmartDashboard.putData(Intake_Ball);
- SmartDashboard.putData(Intake_Gear);
- SmartDashboard.putData(Climber);
+ SmartDashboard.putData(led1);
+ SmartDashboard.putData(intake_ball);
+ SmartDashboard.putData(intake_gear);
+ SmartDashboard.putData(pivot_gear);
+// SmartDashboard.putData(visionProcessor);
+ SmartDashboard.putData(visionLift);
+ SmartDashboard.putData(visionBoiler);
+ SmartDashboard.putData(climber);
}
-}
-
-
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/RobotMap.java b/src/org/usfirst/frc/team708/robot/RobotMap.java
index 49e402b..5746308 100644
--- a/src/org/usfirst/frc/team708/robot/RobotMap.java
+++ b/src/org/usfirst/frc/team708/robot/RobotMap.java
@@ -21,7 +21,7 @@ public class RobotMap {
// public static final int = 1;
// public static final int = 2;
// public static final int = 3;
-// public static final int = 4;
+ public static final int hoodAngle = 4;
// public static final int = 5;
// public static final int = 6;
// public static final int = 7;
@@ -38,8 +38,9 @@ public class RobotMap {
public static final int climberMotor = 21;
// Intake CAN Device IDs
- public static final int intakeMotorBall = 31;
- public static final int intakeMotorGear = 32;
+ public static final int intakeMotorBall = 31;
+ public static final int intakeMotorGear = 32;
+ public static final int pivotGearMotor = 33;
// Grappler Grabber CAN Device IDs
public static final int shooterMotorMaster = 41;
@@ -47,18 +48,17 @@ public class RobotMap {
// Shooter CAN Device ID
public static final int feederMotor = 51;
-
- // Hopper CAN Device ID
- public static final int loaderMotor = 61;
-
+
+// public static final int loaderMotor = 61;
+
// Digital IO
public static final int drivetrainEncoderARt = 0;
public static final int drivetrainEncoderBRt = 1;
public static final int drivetrainEncoderALeft = 2;
public static final int drivetrainEncoderBLeft = 3;
- public static final int shooterEncoderA = 4;
- public static final int shooterEncoderB = 5;
- public static final int climberSwitchA = 6;
+// public static final int shooterEncoderA = 4;
+// public static final int shooterEncoderB = 5;
+ public static final int gearSensorSwitch = 5;
// public static final int = 7;
// public static final int = 8;
// public static final int = 9;
@@ -75,14 +75,14 @@ public class RobotMap {
// public static final int = 3;
// PCM Ports
-// public static final int = 0;
-// public static final int = 1;
-// public static final int = 2;
-// public static final int = 3;
-// public static final int = 4;
-// public static final int = 5;
-// public static final int = 6;
-// public static final int = 7;
+ public static final int PWR0 = 0;
+ public static final int PWR1 = 1;
+ public static final int PWR2 = 2;
+ public static final int PWR3 = 3;
+ public static final int GEARLIGHT = 4;
+ public static final int BOILERLIGHT = 5;
+// public static final int PWR6 = 6;
+// public static final int PWR7 = 7;
// PDP Board Mappings
diff --git a/src/org/usfirst/frc/team708/robot/commands/AllianceSelection/BlueAlliance.java b/src/org/usfirst/frc/team708/robot/commands/AllianceSelection/BlueAlliance.java
new file mode 100644
index 0000000..b5ad95b
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/AllianceSelection/BlueAlliance.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team708.robot.commands.AllianceSelection;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import edu.wpi.first.wpilibj.command.Command;
+
+ public class BlueAlliance extends Command {
+
+ public BlueAlliance() {
+ }
+
+ protected void initialize() {
+ //Robot.drivetrain.setAlliance(Constants.ALLIANCE_BLUE);
+ Robot.ledAllianceColor = Constants.SET_ALLIANCE_BLUE;
+ Robot.led1.send_to_led(Constants.SET_ALLIANCE_BLUE);
+ }
+
+ protected void execute() {
+ }
+
+ protected boolean isFinished() {
+ return true;
+ }
+
+ protected void end() {
+ }
+
+ protected void interrupted() {
+ end();
+ }
+ }
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/AllianceSelection/RedAlliance.java b/src/org/usfirst/frc/team708/robot/commands/AllianceSelection/RedAlliance.java
new file mode 100644
index 0000000..fe440d1
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/AllianceSelection/RedAlliance.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team708.robot.commands.AllianceSelection;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import edu.wpi.first.wpilibj.command.Command;
+
+ public class RedAlliance extends Command {
+
+ public RedAlliance() {
+ }
+
+ protected void initialize() {
+ //Robot.drivetrain.setAlliance(Constants.ALLIANCE_RED);
+ Robot.ledAllianceColor = Constants.SET_ALLIANCE_RED;
+ Robot.led1.send_to_led(Constants.SET_ALLIANCE_RED);
+ }
+
+ protected void execute() {
+ }
+
+ protected boolean isFinished() {
+ return true;
+ }
+
+ protected void end() {
+ }
+
+ protected void interrupted() {
+ end();
+ }
+ }
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/Climber/ClimbDown.java b/src/org/usfirst/frc/team708/robot/commands/Climber/ClimbDown.java
new file mode 100644
index 0000000..0092de0
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/Climber/ClimbDown.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team708.robot.commands.Climber;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.*;
+import org.usfirst.frc.team708.robot.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+/**
+ ** @author James Makovics
+ **/
+public class ClimbDown extends Command{
+
+ public ClimbDown(){
+// requires(Robot.climber); //Requires Climber from the IO.Java
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.climber.manualMove(Constants.CLIMB_REVERSE); //Defines move speed from the operator's controller
+ }
+
+ // 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;
+ }
+
+ protected void end() {
+ Robot.climber.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+ }
+
diff --git a/src/org/usfirst/frc/team708/robot/commands/Climber/ClimbUp.java b/src/org/usfirst/frc/team708/robot/commands/Climber/ClimbUp.java
new file mode 100644
index 0000000..c111260
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/Climber/ClimbUp.java
@@ -0,0 +1,41 @@
+ package org.usfirst.frc.team708.robot.commands.Climber;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.*;
+import org.usfirst.frc.team708.robot.Constants;
+import edu.wpi.first.wpilibj.command.Command;
+/**
+ ** @author James Makovics
+ **/
+public class ClimbUp extends Command {
+
+ public ClimbUp(){
+// requires(Robot.climber); //Gets Climber from IO.Java
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.climber.manualMove(Constants.CLIMB_FORWARD); //Defines move speed from the operator's controller
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+// double moveSpeed = OI.operatorGamepad.getAxis(Gamepad.leftStick_Y); //Gets Input from operator's controller
+ }
+
+ // 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.climber.stop();
+ }
+
+ // 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/team708/robot/commands/Climber/ManualMoveClimber.java b/src/org/usfirst/frc/team708/robot/commands/Climber/ManualMoveClimber.java
new file mode 100644
index 0000000..4b51173
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/Climber/ManualMoveClimber.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team708.robot.commands.Climber;
+
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Constants;
+
+import edu.wpi.first.wpilibj.command.Command;
+/**
+ ** @author James Makovics
+ **/
+public class ManualMoveClimber extends Command {
+ public ManualMoveClimber() {
+// requires(Robot.climber);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ double moveSpeed = OI.operatorGamepad.getAxis(Gamepad.leftStick_Y);
+
+ //check if joystick axis is in deadzone. Change movespeed to 0 if it is
+ if(moveSpeed <= Constants.AXIS_DEAD_ZONE && moveSpeed >= -Constants.AXIS_DEAD_ZONE){
+ moveSpeed = 0.0;
+ }
+
+ Robot.climber.manualMove(moveSpeed);
+ }
+
+ // 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/team708/robot/commands/autonomous/AutoFireBalls.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/AutoFireBalls.java
new file mode 100644
index 0000000..4125586
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/AutoFireBalls.java
@@ -0,0 +1,35 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.feeder.SpinFeeder;
+import org.usfirst.frc.team708.robot.commands.shooter.StopShooter;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+
+public class AutoFireBalls extends CommandGroup {
+
+ protected void initialize() {
+
+ }
+ public AutoFireBalls() {
+ addSequential(new DriveStraightToEncoderDistance(48, .4, true));
+
+// addSequential(new DriveStraightToEncoderDistance(1, .3, false));
+
+ addSequential(new SpinFeeder(6));
+ addSequential(new StopShooter());
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return true;
+ }
+
+ protected void end() {
+ }
+
+ protected void interrupted() {
+ }
+
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/autonomous/DoNothing.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/DoNothing.java
index 363b756..f287c5c 100644
--- a/src/org/usfirst/frc/team708/robot/commands/autonomous/DoNothing.java
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/DoNothing.java
@@ -1,8 +1,14 @@
package org.usfirst.frc.team708.robot.commands.autonomous;
import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.drivetrain.ToggleBrakeMode;
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
import edu.wpi.first.wpilibj.command.Command;
@@ -10,11 +16,12 @@
/**
* this does nothing
*/
-public class DoNothing extends Command {
+public class DoNothing extends CommandGroup {
public DoNothing() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
+ addSequential(new ToggleBrakeMode());
}
// Called just before this Command runs the first time
@@ -24,26 +31,6 @@ protected void initialize() {
Robot.drivetrain.resetGyro();
}
- // Called repeatedly when this Command is scheduled to run
- protected void execute() {
- // Add Commands here:
- // e.g. addSequential(new Command1());
- // addSequential(new Command2());
- // these will run in order.
-
- // To run multiple commands at the same time,
- // use addParallel()
- // e.g. addParallel(new Command1());
- // addSequential(new Command2());
- // Command1 and Command2 will run in parallel.
-
- // A command group will require all of the subsystems that each member
- // would require.
- // e.g. if Command1 requires chassis, and Command2 requires arm,
- // a CommandGroup containing them would require both the chassis and the
- // arm.
- }
-
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return false;
diff --git a/src/org/usfirst/frc/team708/robot/commands/autonomous/DriveInSquare.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/DriveInSquare.java
index 50f6a49..6682b1e 100644
--- a/src/org/usfirst/frc/team708/robot/commands/autonomous/DriveInSquare.java
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/DriveInSquare.java
@@ -11,22 +11,23 @@
public class DriveInSquare extends CommandGroup {
+
private static final double driveStraightSpeed = 0.4;
private static final double driveStraightTime = 2;
- private static final double turnSpeed = 0.4;
+ private static final double turnSpeed = -0.4;
private static final double turnDegrees = 90;
// Called just before this Command runs the first time
protected void initialize() {
- Robot.drivetrain.resetEncoder();
- Robot.drivetrain.resetEncoder2();
- Robot.drivetrain.resetGyro();
+// Robot.drivetrain.resetEncoder();
+// Robot.drivetrain.resetEncoder2();
+// Robot.drivetrain.resetGyro();
}
public DriveInSquare() {
-
+
addSequential(new WaitCommand(4.0));
addSequential(new DriveStraightForTime(driveStraightSpeed, driveStraightTime));
addSequential(new WaitCommand(0.1));
@@ -43,8 +44,6 @@ public DriveInSquare() {
addSequential(new DriveStraightForTime(driveStraightSpeed, driveStraightTime));
addSequential(new WaitCommand(0.1));
addSequential(new TurnToDegrees(turnSpeed, turnDegrees));
-
-
}
// Make this return true when this Command no longer needs to run execute()
diff --git a/src/org/usfirst/frc/team708/robot/commands/autonomous/JustTenBalls.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/JustTenBalls.java
new file mode 100644
index 0000000..6b5d1fb
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/JustTenBalls.java
@@ -0,0 +1,70 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightForTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToBoiler;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToLift;
+import org.usfirst.frc.team708.robot.commands.drivetrain.Send;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegreesAlliance;
+import org.usfirst.frc.team708.robot.commands.feeder.FeederOff;
+import org.usfirst.frc.team708.robot.commands.feeder.SpinFeeder;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Down;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Off;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Out;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Up;
+//import org.usfirst.frc.team708.robot.commands.intake_gear.ReleaseGear;
+import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.commands.shooter.StopShooter;
+import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class JustTenBalls extends CommandGroup {
+
+ /*
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ public JustTenBalls() {
+ addSequential(new Send("In Just 10 Ball"));
+
+// go to baseline
+ addSequential(new DriveStraightToEncoderDistance(85, .4, false));
+
+// target Boiler
+ addSequential(new WaitCommand(1.0));
+ addSequential(new SetLED(Constants.SET_TARGETING));
+ addSequential(new RotateAndDriveToBoiler(111), 3);
+
+// unload balls
+ addParallel(new SpinShooter(8));
+
+ addSequential(new DriveStraightToEncoderDistanceOrTime(90, .5, true, 4));
+ addSequential(new WaitCommand(1.0));
+ addSequential(new SpinFeeder(6));
+ addSequential(new StopShooter());
+ }
+
+ // 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/team708/robot/commands/autonomous/OneGearCenter.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/OneGearCenter.java
new file mode 100644
index 0000000..4eee7d8
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/OneGearCenter.java
@@ -0,0 +1,103 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightForTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToBoiler;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToLift;
+import org.usfirst.frc.team708.robot.commands.drivetrain.Send;
+//import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToGear;
+import org.usfirst.frc.team708.robot.commands.drivetrain.ToggleBrakeMode;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegreesAlliance;
+import org.usfirst.frc.team708.robot.commands.feeder.SpinFeeder;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Off;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Out;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Up;
+//import org.usfirst.frc.team708.robot.commands.intake_gear.ReleaseGear;
+import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+//import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+import org.usfirst.frc.team708.robot.commands.shooter.StopShooter;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Down;
+//import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_In;
+//import org.usfirst.frc.team708.robot.commands.intake_gear.AquireGear;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class OneGearCenter extends CommandGroup {
+
+ /*
+ protected void initialize() {
+// Robot.drivetrain.resetEncoder();
+// Robot.drivetrain.resetEncoder2();
+// Robot.drivetrain.resetGyro();
+ }
+
+ public OneGearCenter() {
+ addSequential(new Send("In OneGearCenter"));
+
+// target lift
+ addSequential(new Send("running drive to lift"));
+
+ addSequential(new SetLED(Constants.SET_HAS_GEAR_TARGETING));
+ addSequential(new WaitCommand(.5)); //was working at 1.0
+ addSequential(new Intake_Gear_Up());
+
+ addSequential(new RotateAndDriveToLift());
+
+// place gear on lever and back away
+ addSequential(new Send("running Gear out"));
+
+ addSequential(new Intake_Gear_Out());
+ addParallel(new Intake_Gear_Down());
+
+// get off lever and go for some balls
+ addSequential(new DriveStraightToEncoderDistance(15, .4, true)); //put this back in!!!!
+
+// turn toward boiler
+ addSequential(new Send("running turn to boiler"));
+
+ addSequential(new TurnToDegreesAlliance(.5, 47, Constants.COUNTERCLOCKWISE));
+
+// target Boiler
+ addSequential(new Send("running target boiler"));
+
+ addSequential(new WaitCommand(.5)); //was working at 1.0
+ addSequential(new SetLED(Constants.SET_TARGETING));
+ addSequential(new RotateAndDriveToBoiler(111), 3);
+
+// unload balls
+ addSequential(new Send("running spin shooter"));
+
+ addParallel(new SpinShooter(12));
+
+ addSequential(new DriveStraightToEncoderDistanceOrTime(110, .5, true, 5));
+
+// addSequential(new WaitCommand(.5)); //commented out - check to see if we need to pause a bit
+ addSequential(new Send("running spin feeder "));
+ addSequential(new SpinFeeder(6)); //then shoot
+ addSequential(new StopShooter());
+
+ addSequential(new Send("finished One Gear"));
+ }
+
+ // 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/team708/robot/commands/autonomous/OneGearLeft.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/OneGearLeft.java
new file mode 100644
index 0000000..546834e
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/OneGearLeft.java
@@ -0,0 +1,88 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.commands.intake_gear.ReleaseGear;
+import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+import org.usfirst.frc.team708.robot.commands.shooter.StopShooter;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightForTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToBoiler;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToLift;
+import org.usfirst.frc.team708.robot.commands.drivetrain.Send;
+import org.usfirst.frc.team708.robot.commands.drivetrain.ToggleBrakeMode;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegreesAlliance;
+import org.usfirst.frc.team708.robot.commands.feeder.SpinFeeder;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Down;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Off;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Out;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Up;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class OneGearLeft extends CommandGroup {
+
+ protected void initialize() {
+ }
+
+ public OneGearLeft() {
+ addSequential(new Send("In OneGearLeft"));
+
+// go to lever
+ addSequential(new DriveStraightToEncoderDistance(70, .4, false));
+ addSequential(new TurnToDegreesAlliance(.5, 45, Constants.CLOCKWISE));
+
+// target lever
+ addSequential(new SetLED(Constants.SET_HAS_GEAR_TARGETING));
+ addSequential(new WaitCommand(1.0)); //was 1.0
+ addSequential(new Intake_Gear_Up());
+ addSequential(new RotateAndDriveToLift());
+
+// place gear on lever and back away
+ addSequential(new Intake_Gear_Out());
+ addParallel(new Intake_Gear_Down());
+
+// get off lever and go for some balls
+ addSequential(new DriveStraightToEncoderDistance(42, .4, true));
+
+// turn toward boiler
+// addSequential(new TurnToDegreesAlliance(.5, 85, Constants.COUNTERCLOCKWISE));
+// addSequential(new DriveStraightToEncoderDistance(47, .5, true));
+
+// addSequential(new TurnToDegreesAlliance(.5, 30, Constants.COUNTERCLOCKWISE));
+
+
+// target Boiler
+// addSequential(new WaitCommand(.75));
+// addSequential(new SetLED(Constants.SET_TARGETING));
+// addSequential(new RotateAndDriveToBoiler(AutoConstants.DISTANCE_TO_BOILER_LOCATION2));
+
+// unload balls
+// addParallel(new SpinShooter(8));
+
+// addSequential(new AutoFireBalls());
+// addSequential(new DriveStraightToEncoderDistanceOrTime(14, .5, true, 2));
+// addSequential(new WaitCommand(1.0));
+// addSequential(new SpinFeeder(6));
+// addSequential(new StopShooter());
+ }
+
+ // 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/team708/robot/commands/autonomous/SixtyBalls.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/SixtyBalls.java
new file mode 100644
index 0000000..395bb0e
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/SixtyBalls.java
@@ -0,0 +1,118 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightForTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToBoiler;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToLift;
+import org.usfirst.frc.team708.robot.commands.drivetrain.Send;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegreesAlliance;
+import org.usfirst.frc.team708.robot.commands.feeder.FeederOff;
+import org.usfirst.frc.team708.robot.commands.feeder.SpinFeeder;
+import org.usfirst.frc.team708.robot.commands.intake_ball.Intake_Ball_In;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Down;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Up;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Off;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Out;
+import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+//import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.commands.shooter.StopShooter;
+import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SixtyBalls extends CommandGroup {
+
+ /*
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ public SixtyBalls() {
+ addSequential(new Send("In Sixty Ball"));
+
+// goto Hopper
+// addSequential(new DriveStraightToEncoderDistance(100, .4, false)); //RED to far hopper
+ addSequential(new DriveStraightToEncoderDistance(100, .4, false)); //BLUE to far hopper
+
+ addSequential(new Send("running turning to hopper"));
+
+// addSequential(new TurnToDegreesAlliance(.5, 85, Constants.COUNTERCLOCKWISE)); //red
+ addSequential(new TurnToDegreesAlliance(.5, 80, Constants.COUNTERCLOCKWISE)); //blue
+
+ addSequential(new Send("running drive to hopper"));
+
+ addSequential(new DriveStraightToEncoderDistanceOrTime(45, .5, true, 2)); //25
+ addParallel(new Intake_Ball_In(7));
+
+ addSequential(new WaitCommand(.2)); //if we can start getting balls from hopper increase this
+
+// back off hopper and turn toward boiler
+ addSequential(new Send("running back away from hopper"));
+
+ addSequential(new DriveStraightToEncoderDistance(40, .4, false)); //30
+ addSequential(new WaitCommand(.5));
+
+ addSequential(new Send("running turn to boiler"));
+ addSequential(new TurnToDegreesAlliance(.5, 58, Constants.CLOCKWISE)); //50 bigboard in way
+
+// addSequential(new DriveStraightToEncoderDistance(40, .4, true));
+
+// target Boiler
+ addSequential(new Send("running target boiler"));
+
+ addSequential(new WaitCommand(.5)); //was 1.0
+ addSequential(new SetLED(Constants.SET_TARGETING));
+ addSequential(new RotateAndDriveToBoiler(111), 3);
+
+// unload balls
+ addSequential(new Send("running spin shooter"));
+
+ addParallel(new SpinShooter(12));
+
+ addSequential(new DriveStraightToEncoderDistanceOrTime(110, .5, true, 3)); //48
+
+ addSequential(new Send("running shoot"));
+
+// addSequential(new WaitCommand(1.0));
+ addSequential(new SpinFeeder(8));
+ addSequential(new StopShooter());
+
+ addSequential(new Send("finished sitxy ball"));
+
+// go to lever
+// addSequential(new TurnToDegreesAlliance(.4, 20, Constants.COUNTERCLOCKWISE));
+
+// target lever
+// addSequential(new Intake_Gear_Up());
+// addSequential(new SetLED(Constants.SET_HAS_GEAR_TARGETING));
+// addSequential(new WaitCommand(.75));
+// addSequential(new RotateAndDriveToLift());
+//
+//// place gear on lever and back away
+// addSequential(new Intake_Gear_Out());
+// addParallel(new Intake_Gear_Down());
+// addSequential(new DriveStraightToEncoderDistance(15, .4, true));
+ }
+
+ // 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/team708/robot/commands/autonomous/TenBalls.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/TenBalls.java
new file mode 100644
index 0000000..5b6b417
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/TenBalls.java
@@ -0,0 +1,102 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightForTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToBoiler;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToLift;
+import org.usfirst.frc.team708.robot.commands.drivetrain.Send;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegreesAlliance;
+import org.usfirst.frc.team708.robot.commands.feeder.FeederOff;
+import org.usfirst.frc.team708.robot.commands.feeder.SpinFeeder;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Down;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Off;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Out;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Up;
+//import org.usfirst.frc.team708.robot.commands.intake_gear.ReleaseGear;
+import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.commands.shooter.StopShooter;
+import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class TenBalls extends CommandGroup {
+
+ /*
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ public TenBalls() {
+ addSequential(new Send("In Ten Ball"));
+
+// go to lever
+ addSequential(new DriveStraightToEncoderDistance(60, .4, false)); //was 73 moved bot over
+
+ addSequential(new Send("running turn to lift"));
+
+ addSequential(new TurnToDegreesAlliance(.5, 43, Constants.COUNTERCLOCKWISE));
+
+// target lever
+ addSequential(new Send("running drive to lift"));
+
+ addSequential(new SetLED(Constants.SET_HAS_GEAR_TARGETING));
+ addSequential(new WaitCommand(1.0)); //was 1.0 do we need to shorten this?
+ addSequential(new Intake_Gear_Up());
+
+ addSequential(new RotateAndDriveToLift());
+
+// place gear on lever and back away
+ addSequential(new Send("running release gear"));
+
+ addSequential(new Intake_Gear_Out());
+ addParallel(new Intake_Gear_Down());
+
+// get off lever and go for some balls
+ addSequential(new DriveStraightToEncoderDistance(10, .4, true));
+ addSequential(new TurnToDegreesAlliance(.5, 15, Constants.CLOCKWISE));
+
+// target Boiler
+ addSequential(new Send("running target boiler"));
+
+ addSequential(new WaitCommand(.5)); //was 1.0
+ addSequential(new SetLED(Constants.SET_TARGETING));
+ addSequential(new RotateAndDriveToBoiler(111), 3);
+
+// unload balls
+ addSequential(new Send("running spin shooter"));
+
+ addParallel(new SpinShooter(12));
+
+ addSequential(new DriveStraightToEncoderDistanceOrTime(100, .5, true, 4));
+// addSequential(new WaitCommand(1.0)); //do we need to add this back
+
+ addSequential(new Send("running shoot"));
+ addSequential(new SpinFeeder(7)); //was 6
+ addSequential(new StopShooter());
+
+ addSequential(new Send("finished 10 ball"));
+ }
+
+ // 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/team708/robot/commands/autonomous/driveDistance.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/driveDistance.java
new file mode 100644
index 0000000..d6bafb7
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/driveDistance.java
@@ -0,0 +1,71 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistance;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightForTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.RotateAndDriveToBoiler;
+import org.usfirst.frc.team708.robot.commands.drivetrain.ToggleBrakeMode;
+import org.usfirst.frc.team708.robot.commands.drivetrain.Send;
+import org.usfirst.frc.team708.robot.commands.feeder.SpinFeeder;
+import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+import org.usfirst.frc.team708.robot.commands.shooter.StopShooter;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Up;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Down;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_In;
+import org.usfirst.frc.team708.robot.commands.intake_gear.Intake_Gear_Out;
+import org.usfirst.frc.team708.robot.commands.intake_gear.ReleaseGear;
+import org.usfirst.frc.team708.robot.commands.autonomous.AutoFireBalls;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class driveDistance extends CommandGroup {
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+// Robot.drivetrain.resetEncoder();
+// Robot.drivetrain.resetEncoder2();
+// Robot.drivetrain.resetGyro();
+
+ }
+
+ public driveDistance() {
+
+ addSequential(new Send("In DriveDistance"));
+
+ addSequential(new Send("Calling wait 2"));
+ addSequential(new WaitCommand(1.0));
+
+ addSequential(new RotateAndDriveToBoiler(111), 3);
+
+// addSequential(new RotateAndDriveToBoiler(111));
+
+ addSequential(new DriveStraightForTime(.3, 3));
+
+
+// addSequential(new WaitCommand(2.0));
+// addSequential(new Send("drive for 1 sec"));
+// addSequential(new DriveStraightToEncoderDistanceOrTime(10, .4, false, 1));
+
+// addSequential(new SpinShooter(8));
+// addSequential(new AutoFireBalls());
+
+ addSequential(new Send("finished"));
+ }
+
+ // 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/team708/robot/commands/autonomous/turn.java b/src/org/usfirst/frc/team708/robot/commands/autonomous/turn.java
new file mode 100644
index 0000000..c5f4cd4
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/autonomous/turn.java
@@ -0,0 +1,37 @@
+package org.usfirst.frc.team708.robot.commands.autonomous;
+
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightForTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegrees;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegreesAlliance;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class turn extends CommandGroup {
+
+ protected void initialize() {
+
+ }
+ public turn(int c) {
+
+// addSequential(new TurnToDegrees(.3, -90)); // turn counter clockwise
+// addSequential(new TurnToDegrees(.3, 90)); // turn clockwise
+// addSequential(new WaitCommand(2));
+ addSequential(new TurnToDegreesAlliance(0.5, 15, 1)); //red clock blue clounter
+// addSequential(new TurnToDegreesAlliance(0.5, 15, -1)); //blue clockwise, red counter
+// System.out.println();System.out.println();System.out.println();System.out.println();System.out.println();System.out.println();System.out.println();System.out.println();
+// addSequential(new TurnToDegrees(-.5, 90)); //add alliance direction
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return false;
+ }
+ protected void end() {
+ }
+ protected void interrupted() {
+ }
+
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistance.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistance.java
index 427db5a..1df12d3 100644
--- a/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistance.java
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistance.java
@@ -26,7 +26,7 @@ public DriveStraightToEncoderDistance(double distance, double speed) {
public DriveStraightToEncoderDistance(double distance, double speed, boolean goForward) {
// Use requires() here to declare subsystem dependencies
- requires(Robot.drivetrain);
+ // requires(Robot.drivetrain);
if(goForward) {
targetDistance = distance;
@@ -43,9 +43,10 @@ public DriveStraightToEncoderDistance(double distance, double speed, boolean goF
// Enables the PIDController (if it was not already) before attempting to drive straight
protected void initialize() {
Robot.drivetrain.resetEncoder();
+ Robot.drivetrain.resetEncoder2();
Robot.drivetrain.resetGyro();
// Robot.drivetrain.enable();
- Robot.drivetrain.disable();
+// Robot.drivetrain.disable(); //JNP disabled
}
// Called repeatedly when this Command is scheduled to run
@@ -58,17 +59,17 @@ protected void execute() {
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
if(goForward) {
- return (Robot.drivetrain.getEncoderDistance() >= targetDistance);
+ return ( Robot.drivetrain.getEncoderDistance2() >= targetDistance);
} else {
- return (Robot.drivetrain.getEncoderDistance() <= targetDistance);
+ return ( Robot.drivetrain.getEncoderDistance2() <= targetDistance);
}
}
// Called once after isFinished returns true
protected void end() {
- Robot.drivetrain.disable();
+// Robot.drivetrain.disable();
Robot.drivetrain.stop();
- Robot.drivetrain.resetEncoder();
+// Robot.drivetrain.resetEncoder();
}
// Called when another command which requires one or more of the same
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTime.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTime.java
index 28afd0d..f9a4147 100644
--- a/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTime.java
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTime.java
@@ -10,9 +10,9 @@
*/
public class DriveStraightToEncoderDistanceOrTime extends Command {
- private double targetDistance;
+ private double targetDistance;
private final double rotate = 0.0;
- private double speed;
+ private double speed;
private double maxTime;
@@ -46,9 +46,10 @@ public DriveStraightToEncoderDistanceOrTime(double distance, double speed, boole
// Enables the PIDController (if it was not already) before attempting to drive straight
protected void initialize() {
Robot.drivetrain.resetEncoder();
+ Robot.drivetrain.resetEncoder2();
Robot.drivetrain.resetGyro();
// Robot.drivetrain.enable();
- Robot.drivetrain.disable();
+// Robot.drivetrain.disable();
}
// Called repeatedly when this Command is scheduled to run
@@ -61,17 +62,17 @@ protected void execute() {
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
if(goForward) {
- return (Robot.drivetrain.getEncoderDistance() >= targetDistance) || isTimedOut();
+ return (Robot.drivetrain.getEncoderDistance2() >= targetDistance) || isTimedOut();
} else {
- return (Robot.drivetrain.getEncoderDistance() <= targetDistance) || isTimedOut();
+ return (Robot.drivetrain.getEncoderDistance2() <= targetDistance) || isTimedOut();
}
}
// Called once after isFinished returns true
protected void end() {
- Robot.drivetrain.disable();
+// Robot.drivetrain.disable();
Robot.drivetrain.stop();
- Robot.drivetrain.resetEncoder();
+// Robot.drivetrain.resetEncoder();
}
// Called when another command which requires one or more of the same
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTimeOrGear.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTimeOrGear.java
new file mode 100644
index 0000000..5214fe9
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTimeOrGear.java
@@ -0,0 +1,86 @@
+package org.usfirst.frc.team708.robot.commands.drivetrain;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DriveStraightToEncoderDistanceOrTimeOrGear extends Command {
+
+ private double targetDistance;
+ private final double rotate = 0.0;
+ private double speed;
+
+ private double maxTime;
+
+ private boolean goForward;
+
+ public DriveStraightToEncoderDistanceOrTimeOrGear(double distance, double maxTime, boolean goForward) {
+ this(distance, Constants.DRIVE_MOTOR_MAX_SPEED, goForward, maxTime);
+ }
+
+// public DriveStraightToEncoderDistanceOrTimeOrGear(double distance, double speed, double maxTime) {
+// this(distance, speed, true, maxTime);
+// }
+
+ public DriveStraightToEncoderDistanceOrTimeOrGear(double distance, double speed, boolean goForward, double maxTime) {
+ // Use requires() here to declare subsystem dependencies
+// requires(Robot.drivetrain);
+// requires(Robot.intake_gear);
+
+ if(goForward) {
+ targetDistance = distance;
+ this.speed = speed;
+ }
+ else {
+ targetDistance = -distance;
+ this.speed = -speed;
+ }
+ this.goForward = goForward;
+
+ this.setTimeout(maxTime);
+ }
+
+ // Called just before this Command runs the first time
+ // Enables the PIDController (if it was not already) before attempting to drive straight
+ protected void initialize() {
+ Robot.drivetrain.resetEncoder();
+ Robot.drivetrain.resetGyro();
+// Robot.drivetrain.enable();
+ Robot.drivetrain.disable();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.drivetrain.haloDrive(speed, rotate, false);
+// Robot.drivetrain.haloDrive(Math708.getPercentError
+// (Robot.drivetrain.getEncoderDistance(), targetDistance), rotate);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ if(goForward) {
+ return (Robot.drivetrain.getEncoderDistance() >= targetDistance) || isTimedOut() || Robot.intake_gear.hasGear();
+ }
+ else {
+ return (Robot.drivetrain.getEncoderDistance() <= targetDistance) || isTimedOut() || Robot.intake_gear.hasGear();
+ }
+
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.drivetrain.disable();
+ Robot.drivetrain.stop();
+ Robot.drivetrain.resetEncoder();
+ }
+
+ // 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/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTimeOrHasGear.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTimeOrHasGear.java
new file mode 100644
index 0000000..0c1939e
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveStraightToEncoderDistanceOrTimeOrHasGear.java
@@ -0,0 +1,82 @@
+package org.usfirst.frc.team708.robot.commands.drivetrain;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class DriveStraightToEncoderDistanceOrTimeOrHasGear extends Command {
+
+ private double targetDistance;
+ private final double rotate = 0.0;
+ private double speed;
+
+ private double maxTime;
+
+ private boolean goForward;
+
+ public DriveStraightToEncoderDistanceOrTimeOrHasGear(double distance, double maxTime) {
+ this(distance, Constants.DRIVE_MOTOR_MAX_SPEED, maxTime);
+ }
+
+ public DriveStraightToEncoderDistanceOrTimeOrHasGear(double distance, double speed, double maxTime) {
+ this(distance, speed, true, maxTime);
+ }
+
+ public DriveStraightToEncoderDistanceOrTimeOrHasGear(double distance, double speed, boolean goForward, double maxTime) {
+ // Use requires() here to declare subsystem dependencies
+ //requires(Robot.drivetrain);
+
+ if(goForward) {
+ targetDistance = distance;
+ this.speed = speed;
+ } else {
+ targetDistance = -distance;
+ this.speed = -speed;
+ }
+ this.goForward = goForward;
+
+ this.setTimeout(maxTime);
+ }
+
+ // Called just before this Command runs the first time
+ // Enables the PIDController (if it was not already) before attempting to drive straight
+ protected void initialize() {
+ Robot.drivetrain.resetEncoder();
+ Robot.drivetrain.resetGyro();
+// Robot.drivetrain.enable();
+ Robot.drivetrain.disable();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.drivetrain.haloDrive(speed, rotate, false);
+// Robot.drivetrain.haloDrive(Math708.getPercentError
+// (Robot.drivetrain.getEncoderDistance(), targetDistance), rotate);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ if(goForward) {
+ return (Robot.drivetrain.getEncoderDistance() >= targetDistance) || isTimedOut();
+ } else {
+ return (Robot.drivetrain.getEncoderDistance() <= targetDistance) || isTimedOut();
+ }
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+// Robot.drivetrain.disable();
+ Robot.drivetrain.stop();
+// Robot.drivetrain.resetEncoder();
+ }
+
+ // 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/team708/robot/commands/drivetrain/DriveToIRDistance.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveToIRDistance.java
index f7a4c5e..d4c17cd 100644
--- a/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveToIRDistance.java
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveToIRDistance.java
@@ -39,9 +39,9 @@ protected void initialize() {
// Called repeatedly when this Command is scheduled to run
protected void execute() {
- moveSpeed = Robot.drivetrain.moveByIR(targetDistance, minValue, maxValue, tolerance);
+// moveSpeed = Robot.drivetrain.moveByIR(targetDistance, minValue, maxValue, tolerance);
// if (targetDistance)
- Robot.drivetrain.haloDrive(moveSpeed, 0.0, false);
+// Robot.drivetrain.haloDrive(moveSpeed, 0.0, false);
}
// Make this return true when this Command no longer needs to run execute()
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveToShooterLocation.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveToShooterLocation.java
new file mode 100644
index 0000000..71a4d22
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/DriveToShooterLocation.java
@@ -0,0 +1,96 @@
+//package org.usfirst.frc.team708.robot.commands.drivetrain;
+//
+//import org.usfirst.frc.team708.robot.AutoConstants;
+//import org.usfirst.frc.team708.robot.Robot;
+//import edu.wpi.first.wpilibj.command.Command;
+//
+//
+//
+///**
+// * DriveToShooterLocation
+// * this command will utilize the vision data to drive the robot to the closest shooting location :P
+// */
+//public class DriveToShooterLocation extends Command {
+//
+// private double moveSpeed;
+// private double rotate;
+// private int moveDirection;
+// /**
+// * Constructor
+// * @param targetDistance - the distance to stop in front of the target
+// */
+//
+// public DriveToShooterLocation() {
+// // Use requires() here to declare subsystem dependencies
+// requires(Robot.drivetrain);
+// requires(Robot.visionBoiler);
+// }
+//
+// // Called just before this Command runs the first time
+// protected void initialize() {
+// Robot.visionBoiler.putIsCentered(false);
+// Robot.visionBoiler.putHasTarget(false);
+// Robot.visionBoiler.putAtDistance(false);
+// Robot.visionBoiler.putCounter(0);
+// Robot.visionBoiler.putCurrentCenter(0);
+// }
+//
+// // Called repeatedly when this Command is scheduled to run
+// protected void execute() {
+// if(Robot.visionBoiler.isCentered()) {
+//
+// if(Robot.visionBoiler.getClosestLocation() == AutoConstants.DISTANCE_TO_BOILER_LOCATION1) {
+// moveDirection = 1;
+// }
+// else {
+// moveDirection = -1;
+// }
+// Robot.visionBoiler.processData();
+// rotate = Robot.visionBoiler.getRotate();
+// moveSpeed = moveDirection*AutoConstants.DRIVE_MOVE_MAX;
+//
+//
+// Robot.drivetrain.haloDrive(moveSpeed, rotate, false);
+//
+// }
+// else {
+//
+// Robot.visionBoiler.processData();
+// rotate = Robot.visionBoiler.getRotate();
+// moveSpeed = Robot.visionBoiler.getMove();
+//
+//
+// Robot.drivetrain.haloDrive(moveSpeed, rotate, false);
+// }
+// }
+//
+// // Make this return true when this Command no longer needs to run execute()
+// protected boolean isFinished() {
+// if (Robot.visionBoiler.getCounter() >= AutoConstants.SWEEP3_MAX){
+//
+// return true;
+// }
+// //Check if the sonar distance is less then the target Distance, end
+//// if (Robot.drivetrain.getSonarDistance() < targetDistance && Robot.visionProcessor.wasCentered()){
+// if (Robot.visionBoiler.isAtDistance() && Robot.visionBoiler.isCentered()){
+// return true;
+// }
+//// else if (Robot.drivetrain.getSonarDistance() < targetDistance && Robot.visionProcessor.isHasTarget()) {
+// else {
+// return false;
+// }
+//
+// }
+//
+// // Called once after isFinished returns true
+// protected void end() {
+// Robot.drivetrain.stop();
+// Robot.visionBoiler.putCounter(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/team708/robot/commands/drivetrain/JoystickDrive.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/JoystickDrive.java
index 476ee8b..d086546 100644
--- a/src/org/usfirst/frc/team708/robot/commands/drivetrain/JoystickDrive.java
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/JoystickDrive.java
@@ -15,6 +15,9 @@ public class JoystickDrive extends Command {
public JoystickDrive() {
// Use requires() here to declare subsystem dependencies
requires(Robot.drivetrain);
+// requires(Robot.shooter);
+// requires(Robot.feeder);
+
}
// Called just before this Command runs the first time
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/RotateAndDriveToBoiler.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/RotateAndDriveToBoiler.java
new file mode 100644
index 0000000..8bd58b7
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/RotateAndDriveToBoiler.java
@@ -0,0 +1,100 @@
+package org.usfirst.frc.team708.robot.commands.drivetrain;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+
+/**
+ *RotateAndDriveToBoiler
+ * this command will utilize the vision data to drive the robot to the center of the boiler
+ * and stop when it is at the any of the target stop at distances
+ */
+public class RotateAndDriveToBoiler extends Command {
+
+ private double moveSpeed;
+ private double rotate;
+
+ private int count;
+
+ /**
+ * Constructor
+ * @param targetDistance - the distance to stop in front of the target
+ */
+
+// public RotateAndDriveToBoiler(double bstopAtDistance){
+ public RotateAndDriveToBoiler(double bstopHeight){
+
+// requires(Robot.drivetrain);
+// requires(Robot.visionBoiler);
+
+ // save the distance
+// Robot.visionBoiler.putBoilerStopAtDistance(bstopAtDistance);
+
+ // save the height
+// Robot.visionBoiler.putBoilerStopAtHeight(bstopHeight);
+ Robot.visionBoiler.putBoilerStopAtHeight(AutoConstants.STOP_AT_BOILER_HEIGHT);
+
+
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.visionBoiler.putBoilerIsCentered(false);
+ Robot.visionBoiler.putBoilerHasTarget(false);
+ Robot.visionBoiler.putBoilerAtDistance(false);
+ Robot.visionBoiler.putBoilerAtHeight(false);
+ Robot.visionBoiler.putBoilerCounter(0);
+ Robot.visionBoiler.putBoilerCurrentCenter(0);
+ Robot.visionBoiler.putBoilerCurrentHeight(0);
+ count = 0 ;
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+
+ // Robot.visionBoiler.boilerProcessData();
+ rotate = Robot.visionBoiler.boilerGetRotate();
+// moveSpeed = Robot.visionBoiler.boilerGetMove();
+
+
+// Robot.drivetrain.haloDrive(moveSpeed, rotate, false);
+ Robot.drivetrain.haloDrive(0, rotate, false);
+
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ if (Robot.visionBoiler.boilerIsCentered()){
+// if (Robot.visionBoiler.boilerIsAtHeight() && Robot.visionBoiler.boilerIsCentered()){
+ SmartDashboard.putBoolean("boiler is done", true);
+ Robot.led1.send_to_led(Constants.SET_TARGET_FOUND);
+ return true;
+ }
+// else if (Robot.visionBoiler.getBoilerCounter() >= AutoConstants.SWEEP3_MAX){
+// SmartDashboard.putBoolean("boiler is in sweep", true);
+// return true;
+// }
+ else {
+// SmartDashboard.putNumber("boiler is running", count++);
+
+ return false;
+ }
+
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.drivetrain.stop();
+ Robot.visionBoiler.putBoilerCounter(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/team708/robot/commands/drivetrain/RotateAndDriveToGear.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/RotateAndDriveToGear.java
new file mode 100644
index 0000000..9c67f1c
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/RotateAndDriveToGear.java
@@ -0,0 +1,85 @@
+//package org.usfirst.frc.team708.robot.commands.drivetrain;
+//
+//import org.usfirst.frc.team708.robot.AutoConstants;
+//import org.usfirst.frc.team708.robot.Robot;
+//import edu.wpi.first.wpilibj.command.Command;
+//
+//
+//
+///**
+// *RotateAndDriveToGear
+// * this command will utilize the vision data to drive the robot to the center of the gear
+// * and stop when it is at the gear stop at target distance
+// */
+//public class RotateAndDriveToGear extends Command {
+//
+// private double moveSpeed;
+// private double rotate;
+// /**
+// * Constructor
+// * @param targetDistance - the distance to stop in front of the target
+// */
+//// VIET ARE WE STILL USING THE TARGET DISTANCE HERE - I THINK WE ARE ACTUALLY CALCULATING IT IN THE SUBSYSTEM
+// public RotateAndDriveToGear() {
+// // Use requires() here to declare subsystem dependencies
+//// requires(Robot.drivetrain);
+//// requires(Robot.visionLiftGear);
+//
+// }
+//
+// // Called just before this Command runs the first time
+// protected void initialize() {
+// Robot.visionLiftGear.putGearIsCentered(false);
+// Robot.visionLiftGear.putGearHasTarget(false);
+// Robot.visionLiftGear.putGearAtDistance(false);
+// Robot.visionLiftGear.putGearCounter(0);
+// Robot.visionLiftGear.putGearCurrentCenter(0);
+//
+// Robot.visionLiftGear.setGearCamera();
+// }
+//
+// // Called repeatedly when this Command is scheduled to run
+// protected void execute() {
+//
+// Robot.visionLiftGear.gearProcessData();
+// rotate = Robot.visionLiftGear.gearGetRotate();
+// moveSpeed = Robot.visionLiftGear.gearGetMove(); // was + made -
+//
+//
+// Robot.drivetrain.haloDrive(-1 * moveSpeed, rotate, false);
+//
+// }
+//
+// // Make this return true when this Command no longer needs to run execute()
+// protected boolean isFinished() {
+// if (Robot.visionLiftGear.getGearCounter() >= AutoConstants.SWEEP3_MAX){
+//
+// return true;
+// }
+// //Check if the sonar distance is less then the target Distance, end
+//// if (Robot.drivetrain.getSonarDistance() < targetDistance && Robot.visionProcessor.wasCentered()){
+//// if (Robot.visionProcessor.isAtY() && Robot.visionProcessor.wasCentered()){
+// else if ((Robot.visionLiftGear.gearIsAtDistance() && Robot.visionLiftGear.gearIsCentered()) || Robot.intake_gear.hasGear()) {
+// return true;
+// }
+//// else if (Robot.drivetrain.getSonarDistance() < targetDistance && Robot.visionProcessor.isHasTarget()) {
+//// else if (Robot.visionLiftGear.gearIsAtDistance() && Robot.visionLiftGear.gearIsHasTarget()) {
+//// return false;
+//// }
+// else
+// return false;
+//
+// }
+//
+// // Called once after isFinished returns true
+// protected void end() {
+// Robot.drivetrain.stop();
+// Robot.visionLiftGear.putGearCounter(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/team708/robot/commands/drivetrain/RotateAndDriveToLift.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/RotateAndDriveToLift.java
new file mode 100644
index 0000000..f4ddf6b
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/RotateAndDriveToLift.java
@@ -0,0 +1,90 @@
+package org.usfirst.frc.team708.robot.commands.drivetrain;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+
+
+/**
+ *RotateAndDriveToLift
+ * this command will utilize the vision data to drive the robot to the center of the lift target
+ * and stop when it is at the lift stop at target distance
+ */
+public class RotateAndDriveToLift extends Command {
+
+ private double moveSpeed;
+ private double rotate;
+ /**
+ * Constructor
+ * @param targetDistance - the distance to stop in front of the target
+ */
+// VIET ARE WE STILL USING THE TARGET DISTANCE HERE - I THINK WE ARE ACTUALLY CALCULATING IT IN THE SUBSYSTEM
+// Mreh mreh mreh, I'm Mrs. P, I want to delete the targetDistance, mreh mreh mreh.
+ public RotateAndDriveToLift() {
+
+ // requires(Robot.drivetrain);
+// requires(Robot.visionLiftGear);
+
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.visionLift.putLiftIsCentered(false);
+ Robot.visionLift.putLiftHasTarget(false);
+ Robot.visionLift.putLiftAtDistance(false);
+ Robot.visionLift.putLiftCounter(0);
+ Robot.visionLift.putLiftCurrentCenter(0);
+
+// Robot.visionLift.setLiftCamera();
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+
+ // Robot.visionLift.liftProcessData();
+ rotate = Robot.visionLift.liftGetRotate();
+ moveSpeed = Robot.visionLift.liftGetMove();
+
+
+ Robot.drivetrain.haloDrive(-1 * moveSpeed, rotate, false);
+
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ //Check if the sonar distance is less then the target Distance, end
+// if (Robot.drivetrain.getSonarDistance() < targetDistance && Robot.visionProcessor.wasCentered()){
+// if (Robot.visionProcessor.isAtY() && Robot.visionProcessor.wasCentered()){
+ if (Robot.visionLift.liftIsCentered() && Robot.visionLift.liftIsAtDistance() ){
+ return true;
+ }
+ else if (Robot.visionLift.getLiftCounter() >= AutoConstants.SWEEP3_MAX){
+ Robot.led1.send_to_led(Constants.SET_TARGET_FOUND);
+ return true;
+ }
+
+// else if (Robot.drivetrain.getSonarDistance() < targetDistance && Robot.visionProcessor.isHasTarget()) {
+// else if (Robot.visionLiftGear.liftIsAtDistance() && Robot.visionLiftGear.liftIsHasTarget()) {
+// return false;
+// }
+ else
+ return false;
+
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.drivetrain.stop();
+ Robot.visionLift.putLiftCounter(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/team708/robot/commands/drivetrain/Send.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/Send.java
new file mode 100644
index 0000000..d71ad1c
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/Send.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team708.robot.commands.drivetrain;
+
+import org.usfirst.frc.team708.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Send extends Command {
+
+ String output;
+
+ public Send(String output) {
+ this.output = output;
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ SmartDashboard.putString("Event", output);
+ }
+
+ // 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() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/ToggleBrakeMode.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/ToggleBrakeMode.java
index ffd479a..34e39cd 100644
--- a/src/org/usfirst/frc/team708/robot/commands/drivetrain/ToggleBrakeMode.java
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/ToggleBrakeMode.java
@@ -16,11 +16,11 @@ public ToggleBrakeMode() {
// Called just before this Command runs the first time
protected void initialize() {
+ Robot.drivetrain.toggleBrakeMode();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
- Robot.drivetrain.toggleBrakeMode();
}
// Make this return true when this Command no longer needs to run execute()
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/TurnToDegrees.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/TurnToDegrees.java
index ef78d09..b0bf58b 100644
--- a/src/org/usfirst/frc/team708/robot/commands/drivetrain/TurnToDegrees.java
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/TurnToDegrees.java
@@ -33,9 +33,9 @@ protected void initialize() {
// Called repeatedly when this Command is scheduled to run
protected void execute() {
if (goalDegrees >= 0) {
- Robot.drivetrain.haloDrive(0.0, rotationSpeed, true);
+ Robot.drivetrain.haloDrive(0.0, -rotationSpeed, false);
} else {
- Robot.drivetrain.haloDrive(0.0, -rotationSpeed, true);
+ Robot.drivetrain.haloDrive(0.0, rotationSpeed, false);
}
}
diff --git a/src/org/usfirst/frc/team708/robot/commands/drivetrain/TurnToDegreesAlliance.java b/src/org/usfirst/frc/team708/robot/commands/drivetrain/TurnToDegreesAlliance.java
new file mode 100644
index 0000000..8993af6
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/drivetrain/TurnToDegreesAlliance.java
@@ -0,0 +1,86 @@
+package org.usfirst.frc.team708.robot.commands.drivetrain;
+
+import org.usfirst.frc.team708.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class TurnToDegreesAlliance extends Command {
+
+ private double rotationSpeed;
+ private double goalDegrees;
+ private double localColor;
+ private int direction;
+
+ /**
+ * Constructor
+ * @param rotationSpeed
+ * @param goalDegrees
+ */
+ public TurnToDegreesAlliance(double rotationSpeed, double goalDegrees, int direction) {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.drivetrain);
+
+ this.rotationSpeed = rotationSpeed;
+ this.goalDegrees = goalDegrees;
+ this.direction = direction;
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.drivetrain.resetGyro();
+// goalDegrees = goalDegrees * SmartDashboard.getInt("AllianceColor");
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+// System.out.println("Color from dashbaord= " + SmartDashboard.getInt("AllianceColor"));
+// System.out.println("GoalDegress passed in = " + goalDegrees);
+// System.out.println("local color = " + localColor);
+// System.out.println("robot.allianceColor = " + Robot.allianceColor);
+ if (localColor != Robot.allianceColor){
+ System.out.println("resetting goal degress");
+ localColor = Robot.allianceColor;
+ goalDegrees = Math.abs(goalDegrees) * Robot.allianceColor;
+ goalDegrees = goalDegrees * direction;
+// goalDegrees = goalDegrees * Robot.allianceColor;
+ }
+
+// System.out.println("local color after = " + localColor);
+// System.out.println("goalDegress after =" + goalDegrees);
+
+ if (goalDegrees >= 0) {
+ Robot.drivetrain.haloDrive(0.0, -rotationSpeed, false);
+ } else {
+ Robot.drivetrain.haloDrive(0.0, rotationSpeed, false);
+ }
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ if (goalDegrees > 0) {
+// System.out.println("returning goal degress >=, " + goalDegrees + " return = " + (Robot.drivetrain.getAngle() >= goalDegrees));
+ return (Robot.drivetrain.getAngle() >= goalDegrees);
+ } else if (goalDegrees < 0){
+// System.out.println("returning goal degress <=, " + goalDegrees + " return = " + (Robot.drivetrain.getAngle() < goalDegrees));
+ return (Robot.drivetrain.getAngle() < goalDegrees);
+ } else {
+// System.out.println("returening goalDegress = 0???");
+ return false;
+ }
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.drivetrain.stop();
+ }
+
+ // 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/team708/robot/commands/feeder/FeederOff.java b/src/org/usfirst/frc/team708/robot/commands/feeder/FeederOff.java
new file mode 100644
index 0000000..a094041
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/feeder/FeederOff.java
@@ -0,0 +1,39 @@
+package org.usfirst.frc.team708.robot.commands.feeder;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.subsystems.Feeder;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class FeederOff extends Command {
+
+ public FeederOff() {
+// requires(Robot.feeder);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.feeder.manualMove(Constants.MOTOR_OFF);
+ }
+
+ // 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.feeder.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/feeder/ManualFeeder.java b/src/org/usfirst/frc/team708/robot/commands/feeder/ManualFeeder.java
new file mode 100644
index 0000000..d606110
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/feeder/ManualFeeder.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc.team708.robot.commands.feeder;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class ManualFeeder extends Command {
+
+
+ public ManualFeeder() {
+// requires(Robot.feeder);
+// requires(Robot.intake_ball);
+// requires(Robot.drivetrain);
+// requires(Robot.shooter);
+ }
+
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.feeder.manualMove(Constants.FEEDER_MOTOR_FORWARD);
+ Robot.intake_ball.moveMotor(Constants.INTAKE_FORWARD);
+ Robot.led1.send_to_led(Constants.SET_TARGET_FOUND);
+ }
+
+ // 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.feeder.stop();
+ Robot.intake_ball.stop();
+ Robot.led1.send_to_led(Robot.ledAllianceColor);
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/feeder/SpinFeeder.java b/src/org/usfirst/frc/team708/robot/commands/feeder/SpinFeeder.java
new file mode 100644
index 0000000..e3b662a
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/feeder/SpinFeeder.java
@@ -0,0 +1,57 @@
+package org.usfirst.frc.team708.robot.commands.feeder;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.subsystems.Feeder;
+import org.usfirst.frc.team708.robot.subsystems.Intake_Ball;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class SpinFeeder extends Command {
+
+ private double maxTime;
+
+ public SpinFeeder(double maxTime) {
+// requires(Robot.feeder);
+// requires(Robot.intake_ball);
+// requires(Robot.drivetrain);
+// requires(Robot.shooter);
+
+ this.setTimeout(maxTime);
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+// Robot.feeder.manualMove(Constants.FEEDER_MOTOR_FORWARD);
+// Robot.intake_ball.moveMotor(Constants.INTAKE_FORWARD);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+// if (timeSinceInitialized() > 1.0){
+ Robot.feeder.manualMove(Constants.FEEDER_MOTOR_FORWARD);
+ Robot.intake_ball.moveMotor(Constants.INTAKE_FORWARD);
+// }
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+// return(false);
+ return (isTimedOut());
+
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.feeder.stop();
+ Robot.intake_ball.stop();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/feeder/SpinFeederBack.java b/src/org/usfirst/frc/team708/robot/commands/feeder/SpinFeederBack.java
new file mode 100644
index 0000000..5dbfa6b
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/feeder/SpinFeederBack.java
@@ -0,0 +1,42 @@
+package org.usfirst.frc.team708.robot.commands.feeder;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.subsystems.Feeder;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class SpinFeederBack extends Command {
+
+ public SpinFeederBack() {
+// requires(Robot.feeder);
+ }
+
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.feeder.manualMove(Constants.FEEDER_MOTOR_REVERSE);
+ }
+
+ // 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.feeder.stop();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_ball/Intake_Ball_In.java b/src/org/usfirst/frc/team708/robot/commands/intake_ball/Intake_Ball_In.java
new file mode 100644
index 0000000..5cd41c7
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_ball/Intake_Ball_In.java
@@ -0,0 +1,37 @@
+package org.usfirst.frc.team708.robot.commands.intake_ball;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class Intake_Ball_In extends Command {
+
+ private double maxTime;
+
+ public Intake_Ball_In(double maxTime) {
+// requires(Robot.intake_ball);
+ this.setTimeout(maxTime);
+ }
+
+ protected void initialize() {
+ Robot.intake_ball.moveMotor(Constants.INTAKE_FORWARD);
+ }
+
+ protected void execute() {
+ }
+
+ protected boolean isFinished() {
+ return(isTimedOut());
+ }
+
+ protected void end() {
+ Robot.intake_ball.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+
+}
+
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_ball/Intake_Ball_Out.java b/src/org/usfirst/frc/team708/robot/commands/intake_ball/Intake_Ball_Out.java
new file mode 100644
index 0000000..4e307a1
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_ball/Intake_Ball_Out.java
@@ -0,0 +1,40 @@
+package org.usfirst.frc.team708.robot.commands.intake_ball;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class Intake_Ball_Out extends Command {
+
+ public Intake_Ball_Out() {
+// requires(Robot.intake_ball);
+ }
+
+ protected void initialize() {
+ Robot.intake_ball.moveMotor(Constants.INTAKE_REVERSE);
+ }
+
+ protected void execute() {
+
+ // if (OI.driverGamepad.getAxis(Gamepad.shoulderAxisRight) >= Constants.AXIS_DEAD_ZONE){
+ // Robot.intake_ball.moveMotor(Constants.INTAKE_REVERSE);
+ // }
+ }
+
+
+ protected boolean isFinished() {
+ return(false);
+ }
+
+ protected void end() {
+ Robot.intake_ball.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_ball/ManualIntake_Ball.java b/src/org/usfirst/frc/team708/robot/commands/intake_ball/ManualIntake_Ball.java
new file mode 100644
index 0000000..9caf5d0
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_ball/ManualIntake_Ball.java
@@ -0,0 +1,57 @@
+package org.usfirst.frc.team708.robot.commands.intake_ball;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import edu.wpi.first.wpilibj.command.Command;
+
+
+public class ManualIntake_Ball extends Command {
+
+ private boolean isdone = false;
+
+ public ManualIntake_Ball() {
+ }
+
+ // Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ boolean R_Shoulderpressed = OI.driverGamepad.getButton(Gamepad.button_R_Shoulder);
+
+ if (R_Shoulderpressed == true){
+ Robot.intake_ball.moveMotor(Constants.INTAKE_FORWARD);
+ isdone = false;
+ }
+ else
+ if (OI.driverGamepad.getAxis(Gamepad.shoulderAxisRight) >= Constants.AXIS_DEAD_ZONE){
+ Robot.intake_ball.moveMotor(Constants.INTAKE_REVERSE);
+ isdone = false;
+ }
+ else {
+ Robot.intake_ball.moveMotor(Constants.INTAKE_OFF);
+ isdone = true;
+ }
+
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return(false || isdone);
+// return(false);
+ }
+
+ // Called once after isFinished returns true
+ protected void end() {
+ Robot.intake_ball.stop();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/AquireGear.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/AquireGear.java
new file mode 100644
index 0000000..989204a
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/AquireGear.java
@@ -0,0 +1,47 @@
+//package org.usfirst.frc.team708.robot.commands.intake_gear;
+//
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTimeOrGear;
+//import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegrees;
+//
+//import edu.wpi.first.wpilibj.command.CommandGroup;
+//import edu.wpi.first.wpilibj.command.WaitCommand;
+//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+//
+//public class AquireGear extends CommandGroup {
+//
+//
+// private static final double driveStraightSpeed = 0.4;
+// private static final double driveStraightTime = 2;
+//
+// private static final double turnSpeed = -0.4;
+// private static final double turnDegrees = 90;
+//
+// // Called just before this Command runs the first time
+// protected void initialize() {
+//// Robot.drivetrain.resetEncoder();
+//// Robot.drivetrain.resetEncoder2();
+//// Robot.drivetrain.resetGyro();
+//
+// }
+//
+// public AquireGear() {
+//
+// addParallel(new DriveStraightToEncoderDistanceOrTimeOrGear(24, .4, false, 3));
+// addParallel(new Intake_Gear_In());
+// }
+//
+// // 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/team708/robot/commands/intake_gear/GearAdjust.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/GearAdjust.java
new file mode 100644
index 0000000..345b614
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/GearAdjust.java
@@ -0,0 +1,71 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class GearAdjust extends Command {
+
+ public GearAdjust() {
+// requires(Robot.intake_gear);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ // protected void execute() {
+ // double gearAngle = OI.operatorGamepad.getAxis(Gamepad.leftStick_Y); //Gets Input from operator's controller
+// double gearAnglex = OI.operatorGamepad.getAxis(Gamepad.leftStick_X); //Gets Input from operator's controller
+
+// if ((gearAnglex>0))
+// {
+// Robot.intake_gear.moveMotor(Constants.GEAR_IN);
+// }
+// else if (gearAnglex<0)
+// {
+// Robot.intake_gear.moveMotor(Constants.GEAR_OUT);
+// }
+
+ // if ((gearAngle >0)
+ // && (!Robot.pivot_gear.isFwdSwitch())
+ // )
+ // Robot.pivot_gear.moveMotor(.8);
+ // else if ((gearAngle <0)
+ // && (!Robot.pivot_gear.isRevSwitch())
+ // )
+ // Robot.pivot_gear.moveMotor(-.8); //Defines move speed from the operator's controller
+ // }
+
+
+ // 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.pivot_gear.stop();
+// Robot.intake_gear.stop();
+
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/GearIntake.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/GearIntake.java
new file mode 100644
index 0000000..0118b25
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/GearIntake.java
@@ -0,0 +1,60 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class GearIntake extends Command {
+
+ public GearIntake() {
+// requires(Robot.intake_gear);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ double gearAngle = OI.operatorGamepad.getAxis(Gamepad.leftStick_X); //Gets Input from operator's controller
+
+/// Robot.intake_gear.moveMotor(gearAngle); //Defines move speed from the operator's controller
+
+// if ((!Robot.intake_gear.hasGear()) && (gearAngle>0))
+ if ((gearAngle>0))
+ {
+ Robot.intake_gear.moveMotor(Constants.GEAR_IN);
+ }
+ else if (gearAngle<0)
+ {
+ Robot.intake_gear.moveMotor(Constants.GEAR_OUT);
+ }
+ else
+ {
+// Robot.intake_gear.stop();
+// Robot.pivot_gear.moveMotor(Constants.GEAR_UP);
+ }
+
+// if (Robot.intake_gear.hasGear())
+// Robot.led1.send_to_led(Constants.SET_HAS_GEAR_TARGETING);
+// else
+// Robot.led1.send_to_led(Robot.ledAllianceColor);
+ }
+
+ // 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.intake_gear.stop();
+// Robot.pivot_gear.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Down.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Down.java
new file mode 100644
index 0000000..ef2c3fa
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Down.java
@@ -0,0 +1,45 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import edu.wpi.first.wpilibj.command.Command;
+
+public class Intake_Gear_Down extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+ /*
+ public Intake_Gear_Down() {
+// requires(Robot.pivot_gear);
+ this.setTimeout(1.0);
+ }
+
+ protected void initialize() {
+ }
+
+ protected void execute() {
+ if (!Robot.pivot_gear.isRevSwitch())
+ Robot.pivot_gear.moveMotor(Constants.INTAKE_REVERSE);
+ else
+ Robot.pivot_gear.stop();
+ }
+
+ protected boolean isFinished() {
+ return Robot.pivot_gear.isRevSwitch()|| isTimedOut();
+ }
+
+ protected void end() {
+ Robot.pivot_gear.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+ */
+}
+
+
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_In.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_In.java
new file mode 100644
index 0000000..9fcc25c
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_In.java
@@ -0,0 +1,45 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.subsystems.Pivot_Gear;
+import org.usfirst.frc.team708.robot.commands.intake_gear.*;
+
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class Intake_Gear_In extends Command {
+
+ public Intake_Gear_In() {
+// requires(Robot.intake_gear);
+// requires(Robot.pivot_gear);
+ }
+
+ protected void initialize() {
+ }
+
+ protected void execute() {
+// if (!Robot.intake_gear.hasGear())
+ Robot.intake_gear.moveMotor(Constants.GEAR_IN);
+// else
+// {
+// Robot.intake_gear.stop();
+// Robot.pivot_gear.moveMotor(Constants.GEAR_UP);
+// }
+ }
+
+ protected boolean isFinished() {
+ return(false);
+ }
+
+ protected void end() {
+ Robot.intake_gear.stop();
+// Robot.pivot_gear.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+
+}
+
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Off.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Off.java
new file mode 100644
index 0000000..35b6c4a
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Off.java
@@ -0,0 +1,35 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class Intake_Gear_Off extends Command {
+
+ public Intake_Gear_Off() {
+// requires(Robot.intake_gear);
+ }
+
+ protected void initialize() {
+ Robot.intake_gear.stop();
+ }
+
+ protected void execute() {
+ }
+
+ protected boolean isFinished() {
+ return(true);
+ }
+
+ protected void end() {
+ Robot.intake_gear.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Out.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Out.java
new file mode 100644
index 0000000..3ae8b0c
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Out.java
@@ -0,0 +1,36 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class Intake_Gear_Out extends Command {
+
+ public Intake_Gear_Out() {
+// requires(Robot.intake_gear);
+ this.setTimeout(.5);
+ }
+
+ protected void initialize() {
+ Robot.intake_gear.moveMotor(-.3);
+ }
+
+ protected void execute() {
+ }
+
+ protected boolean isFinished() {
+ return(isTimedOut());
+// return false;
+ }
+
+ protected void end() {
+ Robot.intake_gear.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Up.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Up.java
new file mode 100644
index 0000000..27e062d
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/Intake_Gear_Up.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import edu.wpi.first.wpilibj.command.Command;
+
+public class Intake_Gear_Up extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+/*
+ public Intake_Gear_Up() {
+// requires(Robot.pivot_gear);
+ this.setTimeout(1.0);
+ }
+
+ protected void initialize() {
+ }
+
+ protected void execute() {
+ if (!Robot.pivot_gear.isFwdSwitch())
+ Robot.pivot_gear.moveMotor(.8);
+ else
+ Robot.pivot_gear.stop();
+ }
+
+ protected boolean isFinished() {
+// if (Robot.pivot_gear.isFwdSwitch())
+// return(true);
+// else
+ return(isTimedOut());
+ }
+
+ protected void end() {
+ Robot.pivot_gear.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+ */
+}
+
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/ManualIntake_Gear.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/ManualIntake_Gear.java
new file mode 100644
index 0000000..86206bd
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/ManualIntake_Gear.java
@@ -0,0 +1,57 @@
+//package org.usfirst.frc.team708.robot.commands.intake_gear;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.util.Gamepad;
+//import edu.wpi.first.wpilibj.command.Command;
+//
+//
+//public class ManualIntake_Gear extends Command {
+//
+// public ManualIntake_Gear() {
+// requires(Robot.intake_gear);
+// }
+//
+//
+// // Called just before this Command runs the first time
+// protected void initialize() {
+//
+// }
+//
+// // Called repeatedly when this Command is scheduled to run
+// protected void execute() {
+//
+// boolean L_Shoulderpressed = OI.driverGamepad.getButton(Gamepad.button_L_Shoulder);
+//
+// // LOADER_IN_BUTTON = Gamepad.Button_L_Shoulder;
+// // LOADER_OUT_BUTTON = Gamepad.shoulderAxisLeft;
+//
+//// if (L_Shoulderpressed == true){
+//// Robot.intake_gear.moveMotor(Constants.GEAR_IN);
+//// }
+//// else if (OI.driverGamepad.getAxis(Gamepad.shoulderAxisLeft) != 0.0){
+//// Robot.intake_gear.moveMotor(Constants.GEAR_DOWN);
+//// }
+//// else {
+//// Robot.intake_gear.moveMotor(Constants.GEAR_OFF);
+//// }
+//
+// }
+//
+// // 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.intake_gear.stop();
+// }
+//
+// // Called when another command which requires one or more of the same
+// // subsystems are scheduled to run
+// protected void interrupted() {
+// end();
+// }
+//}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/ManualPivot_Gear.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/ManualPivot_Gear.java
new file mode 100644
index 0000000..9ff9551
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/ManualPivot_Gear.java
@@ -0,0 +1,57 @@
+//package org.usfirst.frc.team708.robot.commands.intake_gear;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.util.Gamepad;
+//import edu.wpi.first.wpilibj.command.Command;
+//
+//
+//public class ManualPivot_Gear extends Command {
+//
+// public ManualPivot_Gear() {
+// requires(Robot.pivot_gear);
+// }
+//
+//
+// // Called just before this Command runs the first time
+// protected void initialize() {
+//
+// }
+//
+// // Called repeatedly when this Command is scheduled to run
+// protected void execute() {
+//
+// boolean B_Buttonpressed = OI.driverGamepad.getButton(Gamepad.button_B);
+// boolean A_Buttonpressed = OI.driverGamepad.getButton(Gamepad.button_A);
+//
+// // LOADER_IN_BUTTON = Gamepad.Button_L_Shoulder;
+// // LOADER_OUT_BUTTON = Gamepad.shoulderAxisLeft;
+//
+//
+// if (B_Buttonpressed == true){
+// Robot.pivot_gear.moveMotor(Constants.INTAKE_FORWARD);
+// }
+// else if (A_Buttonpressed == true){
+// Robot.pivot_gear.moveMotor(Constants.INTAKE_REVERSE);
+// }
+// else {
+// Robot.pivot_gear.moveMotor(Constants.INTAKE_OFF);
+// }
+//
+// }
+//
+// // 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.pivot_gear.stop();
+// }
+//
+// protected void interrupted() {
+// end();
+// }
+//}
diff --git a/src/org/usfirst/frc/team708/robot/commands/intake_gear/ReleaseGear.java b/src/org/usfirst/frc/team708/robot/commands/intake_gear/ReleaseGear.java
new file mode 100644
index 0000000..7d254e7
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/intake_gear/ReleaseGear.java
@@ -0,0 +1,41 @@
+package org.usfirst.frc.team708.robot.commands.intake_gear;
+
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTime;
+import org.usfirst.frc.team708.robot.commands.drivetrain.DriveStraightToEncoderDistanceOrTimeOrGear;
+import org.usfirst.frc.team708.robot.commands.drivetrain.TurnToDegrees;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ReleaseGear extends CommandGroup {
+
+ protected void initialize() {
+ this.setTimeout(1.0);
+ }
+
+ public ReleaseGear() {
+
+ // place gear on lever and back away
+ addSequential(new Intake_Gear_Out());
+ addParallel(new Intake_Gear_Down());
+
+ // get off lever and go for some balls
+ addSequential(new DriveStraightToEncoderDistanceOrTime(10, .4, true, 1));
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return(isTimedOut());
+ }
+
+ // 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/team708/robot/commands/led_out/LED_out.java b/src/org/usfirst/frc/team708/robot/commands/led_out/LED_out.java
new file mode 100644
index 0000000..823049d
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/led_out/LED_out.java
@@ -0,0 +1,63 @@
+package org.usfirst.frc.team708.robot.commands.led_out;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.subsystems.LED;
+import org.usfirst.frc.team708.robot.subsystems.Drivetrain;
+
+
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.SerialPort.Port;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class LED_out extends Command {
+
+//static public byte count = 0x00;
+
+ public LED_out() {
+// requires(Robot.led1);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.led1.count++;
+ if (Robot.led1.count > Constants.SET_OFF) Robot.led1.count = 0x00;
+ Robot.led1.send_to_led(Robot.led1.count);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+// count++;
+// if (count > Constants.SET_OFF) count = 0x00;
+// Robot.led1.send_to_led(count);
+
+// if (count == 0x02) Robot.drivetrain.setGearLight(false);
+// if (count == 0x03) Robot.drivetrain.setBoilerLight(false);
+ }
+ // 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 are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/led_out/SetLED.java b/src/org/usfirst/frc/team708/robot/commands/led_out/SetLED.java
new file mode 100644
index 0000000..62a8f8f
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/led_out/SetLED.java
@@ -0,0 +1,59 @@
+package org.usfirst.frc.team708.robot.commands.led_out;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.subsystems.LED;
+import org.usfirst.frc.team708.robot.subsystems.Drivetrain;
+
+
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.SerialPort.Port;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class SetLED extends Command {
+
+ static public byte ledcolor;
+
+ public SetLED(byte ledcolor) {
+// requires(Robot.led1);
+ this.ledcolor = ledcolor;
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.led1.send_to_led(ledcolor);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+
+// Robot.led1.send_to_led(ledcolor);
+ }
+
+ // 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 are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/loader/LoaderOff.java b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderOff.java
index d7d3431..9a04c2d 100644
--- a/src/org/usfirst/frc/team708/robot/commands/loader/LoaderOff.java
+++ b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderOff.java
@@ -1,49 +1,38 @@
-package org.usfirst.frc.team708.robot.commands.loader;
-
-import org.usfirst.frc.team708.robot.Constants;
-import org.usfirst.frc.team708.robot.OI;
-import org.usfirst.frc.team708.robot.Robot;
-import org.usfirst.frc.team708.robot.subsystems.Loader;
-import org.usfirst.frc.team708.robot.util.Gamepad;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class LoaderOff extends Command {
-
-
- public LoaderOff() {
- requires(Robot.loader);
- }
-
-
- // 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.loader.manualMove(Constants.LOADER_OFF);
-
-
- }
-
- // 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.loader.stop();
- }
-
- // Called when another command which requires one or more of the same
- // subsystems are scheduled to run
- protected void interrupted() {
- end();
- }
-}
+//package org.usfirst.frc.team708.robot.commands.loader;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.subsystems.Loader;
+//import org.usfirst.frc.team708.robot.util.Gamepad;
+//
+//import edu.wpi.first.wpilibj.command.Command;
+//
+///**
+// *
+// */
+//public class LoaderOff extends Command {
+//
+// public LoaderOff() {
+// requires(Robot.loader);
+// }
+//
+//
+// // 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.loader.manualMove(Constants.LOADER_OFF);
+// }
+//
+//
+// @Override
+// protected boolean isFinished() {
+// // TODO Auto-generated method stub
+// return false;
+// }
+//
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpin.java b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpin.java
new file mode 100644
index 0000000..600cbe8
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpin.java
@@ -0,0 +1,54 @@
+//package org.usfirst.frc.team708.robot.commands.loader;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.subsystems.Loader;
+//import org.usfirst.frc.team708.robot.util.Gamepad;
+//
+//import edu.wpi.first.wpilibj.command.Command;
+//
+///**
+// *
+// */
+//public class LoaderSpin extends Command {
+//
+// public LoaderSpin() {
+// requires(Robot.loader);
+// }
+//
+//
+// // Called just before this Command runs the first time
+// protected void initialize() {
+// }
+//
+// // Called repeatedly when this Command is scheduled to run
+// protected void execute() {
+// if (Robot.loader.spinForward())
+// {
+// Robot.loader.manualMove(Constants.LOADER_MOTOR_FORWARD);
+// }
+// else
+// {
+// Robot.loader.manualMove(Constants.LOADER_MOTOR_REVERSE);
+// }
+//
+// }
+//
+// // 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() {
+//// Robot.loader.stop(); //runs till you hit the off button
+// Robot.loader.toggleSpin();
+// }
+//
+// // Called when another command which requires one or more of the same
+// // subsystems are scheduled to run
+// protected void interrupted() {
+// end();
+// }
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinIn.java b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinIn.java
index bbbff3d..7e34837 100644
--- a/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinIn.java
+++ b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinIn.java
@@ -1,49 +1,49 @@
-package org.usfirst.frc.team708.robot.commands.loader;
-
-import org.usfirst.frc.team708.robot.Constants;
-import org.usfirst.frc.team708.robot.OI;
-import org.usfirst.frc.team708.robot.Robot;
-import org.usfirst.frc.team708.robot.subsystems.Loader;
-import org.usfirst.frc.team708.robot.util.Gamepad;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class LoaderSpinIn extends Command {
-
-
- public LoaderSpinIn() {
-
- requires(Robot.loader);
-
- }
-
-
- // 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.loader.manualMove(Constants.LOADER_MOTOR_FORWARD);
- }
-
- // 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.loader.stop();
- }
-
- // Called when another command which requires one or more of the same
- // subsystems are scheduled to run
- protected void interrupted() {
- end();
- }
-}
+//package org.usfirst.frc.team708.robot.commands.loader;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.subsystems.Loader;
+//import org.usfirst.frc.team708.robot.util.Gamepad;
+//
+//import edu.wpi.first.wpilibj.command.Command;
+//
+///**
+// *
+// */
+//public class LoaderSpinIn extends Command {
+//
+//
+// public LoaderSpinIn() {
+//
+// requires(Robot.loader);
+//
+// }
+//
+//
+// // 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.loader.manualMove(Constants.LOADER_MOTOR_FORWARD);
+// }
+//
+// // 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.loader.stop();
+// }
+//
+// // Called when another command which requires one or more of the same
+// // subsystems are scheduled to run
+// protected void interrupted() {
+// end();
+// }
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinOut.java b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinOut.java
index 7f33595..a761e32 100644
--- a/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinOut.java
+++ b/src/org/usfirst/frc/team708/robot/commands/loader/LoaderSpinOut.java
@@ -1,49 +1,49 @@
-package org.usfirst.frc.team708.robot.commands.loader;
-
-import org.usfirst.frc.team708.robot.Constants;
-import org.usfirst.frc.team708.robot.OI;
-import org.usfirst.frc.team708.robot.Robot;
-import org.usfirst.frc.team708.robot.subsystems.Loader;
-import org.usfirst.frc.team708.robot.util.Gamepad;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class LoaderSpinOut extends Command {
-
-
- public LoaderSpinOut() {
- requires(Robot.loader);
- }
-
-
- // 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.loader.manualMove(Constants.LOADER_MOTOR_REVERSE);
-
-
- }
-
- // 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.loader.stop();
- }
-
- // Called when another command which requires one or more of the same
- // subsystems are scheduled to run
- protected void interrupted() {
- end();
- }
-}
+//package org.usfirst.frc.team708.robot.commands.loader;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.subsystems.Loader;
+//import org.usfirst.frc.team708.robot.util.Gamepad;
+//
+//import edu.wpi.first.wpilibj.command.Command;
+//
+///**
+// *
+// */
+//public class LoaderSpinOut extends Command {
+//
+//
+// public LoaderSpinOut() {
+// requires(Robot.loader);
+// }
+//
+//
+// // 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.loader.manualMove(Constants.LOADER_MOTOR_REVERSE);
+//
+//
+// }
+//
+// // 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.loader.stop();
+// }
+//
+// // Called when another command which requires one or more of the same
+// // subsystems are scheduled to run
+// protected void interrupted() {
+// end();
+// }
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/loader/ManualLoader.java b/src/org/usfirst/frc/team708/robot/commands/loader/ManualLoader.java
index 3b923d2..2a9eefe 100644
--- a/src/org/usfirst/frc/team708/robot/commands/loader/ManualLoader.java
+++ b/src/org/usfirst/frc/team708/robot/commands/loader/ManualLoader.java
@@ -1,66 +1,61 @@
-package org.usfirst.frc.team708.robot.commands.loader;
-
-import org.usfirst.frc.team708.robot.Constants;
-import org.usfirst.frc.team708.robot.OI;
-import org.usfirst.frc.team708.robot.Robot;
-import org.usfirst.frc.team708.robot.util.Gamepad;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class ManualLoader extends Command {
-
-
- public ManualLoader() {
- requires(Robot.loader);
- }
-
-
- // Called just before this Command runs the first time
- protected void initialize() {
-
- }
-
- // Called repeatedly when this Command is scheduled to run
- protected void execute() {
-
- boolean Apressed = OI.operatorGamepad.getButton(Gamepad.button_A);
- boolean Xpressed = OI.operatorGamepad.getButton(Gamepad.button_X);
- boolean Ypressed = OI.operatorGamepad.getButton(Gamepad.button_Y);
-
-// LOADER_IN_BUTTON = Gamepad.button_Y;
-// LOADER_OUT_BUTTON = Gamepad.button_A;
-// LOADER_OFF_BUTTON = Gamepad.button_X;
-
- if (Ypressed == true){
- Robot.loader.manualMove(Constants.MOTOR_FORWARD);
- }
- else
- if (Apressed == true){
- Robot.loader.manualMove(Constants.MOTOR_REVERSE);
- }
- else
- if (Xpressed == true){
- Robot.loader.manualMove(Constants.MOTOR_OFF);
- }
-
- }
-
- // 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.loader.stop();
- }
-
- // Called when another command which requires one or more of the same
- // subsystems are scheduled to run
- protected void interrupted() {
- end();
- }
-}
+//package org.usfirst.frc.team708.robot.commands.loader;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.util.Gamepad;
+//
+//import edu.wpi.first.wpilibj.command.Command;
+//
+///**
+// *
+// */
+//public class ManualLoader extends Command {
+//
+//
+// public ManualLoader() {
+// requires(Robot.loader);
+// }
+//
+//
+// // Called just before this Command runs the first time
+// protected void initialize() {
+// }
+//
+// // Called repeatedly when this Command is scheduled to run
+// protected void execute() {
+//
+// boolean Apressed = OI.operatorGamepad.getButton(Gamepad.button_A);
+// boolean Xpressed = OI.operatorGamepad.getButton(Gamepad.button_X);
+// boolean Ypressed = OI.operatorGamepad.getButton(Gamepad.button_Y);
+//
+// if (Ypressed == true){
+// Robot.loader.manualMove(Constants.MOTOR_FORWARD);
+// }
+// else
+// if (Apressed == true){
+// Robot.loader.manualMove(Constants.MOTOR_REVERSE);
+// }
+// else
+// if (Xpressed == true){
+// Robot.loader.manualMove(Constants.MOTOR_OFF);
+// }
+//
+// }
+//
+// // 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.loader.stop();
+// }
+//
+// // Called when another command which requires one or more of the same
+// // subsystems are scheduled to run
+// protected void interrupted() {
+// end();
+// }
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/commands/shooter/HoodAdjust.java b/src/org/usfirst/frc/team708/robot/commands/shooter/HoodAdjust.java
new file mode 100644
index 0000000..a5b38fa
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/shooter/HoodAdjust.java
@@ -0,0 +1,60 @@
+package org.usfirst.frc.team708.robot.commands.shooter;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class HoodAdjust extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+ /*
+
+ public HoodAdjust() {
+// requires(Robot.shooter);
+// requires(Robot.feeder);
+// requires(Robot.drivetrain);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ double hoodAngle = OI.operatorGamepad.getAxis(Gamepad.rightStick_Y);
+ Robot.shooter.hoodAdjust(hoodAngle);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+// double hoodAngle = OI.operatorGamepad.getAxis(Gamepad.rightStick_Y);
+// Robot.shooter.hoodAdjust(hoodAngle);
+ }
+
+ // 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() {
+// Robot.shooter.stop();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+ */
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/shooter/ManualShoot.java b/src/org/usfirst/frc/team708/robot/commands/shooter/ManualShoot.java
new file mode 100644
index 0000000..34d5b2e
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/shooter/ManualShoot.java
@@ -0,0 +1,78 @@
+package org.usfirst.frc.team708.robot.commands.shooter;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+//import org.usfirst.frc.team708.robot.commands.led_out.SetLED;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class ManualShoot extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+ /*
+
+ public ManualShoot() {
+// requires(Robot.feeder);
+// requires(Robot.intake_ball);
+// requires(Robot.drivetrain);
+// requires(Robot.shooter);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+// if (Robot.drivetrain.getSonarDistance() > Constants.SHOOTER_CLOSE_SHOT)
+// {
+// Robot.shooter.moveHood(Constants.HOOD_LEVER);
+// Robot.shooter.manualRPM(Constants.SHOOTER_MOTOR_SPEED_LEVER);
+// Robot.shooter.setSpinSpeed(Constants.SHOOTER_MOTOR_SPEED_LEVER);
+// Robot.led1.send_to_led(Constants.SET_TARGETING);
+// }
+// else
+ {
+ Robot.shooter.manualRPM(Constants.SHOOTER_MOTOR_SPEED_BOILER);
+ Robot.shooter.moveHood(Constants.HOOD_BOILER);
+ Robot.shooter.setSpinSpeed(Constants.SHOOTER_MOTOR_SPEED_BOILER);
+ Robot.led1.send_to_led(Constants.SET_TARGETING);
+ }
+
+// Robot.shooter.setFgain(Constants.SHOOTER_F);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.shooter.manualRPM(Robot.shooter.getSpinSpeed());
+ }
+
+ // 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.shooter.stop();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+ */
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/shooter/MoveHoodHigh.java b/src/org/usfirst/frc/team708/robot/commands/shooter/MoveHoodHigh.java
new file mode 100644
index 0000000..fb9815b
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/shooter/MoveHoodHigh.java
@@ -0,0 +1,55 @@
+package org.usfirst.frc.team708.robot.commands.shooter;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class MoveHoodHigh extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+ /*
+
+ public MoveHoodHigh() {
+// requires(Robot.shooter);
+// requires(Robot.feeder);
+// requires(Robot.drivetrain);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.shooter.moveHood(Constants.HOOD_LEVER); // 2000 is upper bounds
+ Robot.shooter.setSpinSpeed(Constants.SHOOTER_MOTOR_SPEED_LEVER);
+ }
+
+ // Called repeatedly 50 times/sec
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ protected boolean isFinished() {
+ return true;
+ }
+
+ protected void end() {
+ //Robot.shooter.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+ */
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/shooter/MoveHoodLow.java b/src/org/usfirst/frc/team708/robot/commands/shooter/MoveHoodLow.java
new file mode 100644
index 0000000..9b7e7ba
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/shooter/MoveHoodLow.java
@@ -0,0 +1,56 @@
+package org.usfirst.frc.team708.robot.commands.shooter;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+public class MoveHoodLow extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+
+ /*
+
+ public MoveHoodLow() {
+// requires(Robot.shooter);
+// requires(Robot.feeder);
+// requires(Robot.drivetrain);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.shooter.moveHood(Constants.HOOD_BOILER); // 25 is the lower bounds
+ Robot.shooter.setSpinSpeed(Constants.SHOOTER_MOTOR_SPEED_BOILER);
+ }
+
+ // Called repeatedly 50 times/sec 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;
+ }
+
+ protected void end() {
+ //Robot.shooter.stop();
+ }
+
+ protected void interrupted() {
+ end();
+ }
+ */
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/shooter/SpinShooter.java b/src/org/usfirst/frc/team708/robot/commands/shooter/SpinShooter.java
index 550e50f..b8f82ea 100644
--- a/src/org/usfirst/frc/team708/robot/commands/shooter/SpinShooter.java
+++ b/src/org/usfirst/frc/team708/robot/commands/shooter/SpinShooter.java
@@ -1,6 +1,7 @@
package org.usfirst.frc.team708.robot.commands.shooter;
import org.usfirst.frc.team708.robot.Constants;
import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
import org.usfirst.frc.team708.robot.Robot;
import org.usfirst.frc.team708.robot.OI;
@@ -17,34 +18,61 @@
*/
public class SpinShooter extends Command {
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
- public SpinShooter() {
+ /*
+ private double maxTime;
+
+ public SpinShooter(double maxTime) {
requires(Robot.shooter);
+// requires(Robot.drivetrain);
+ this.setTimeout(maxTime);
}
// Called just before this Command runs the first time
protected void initialize() {
-
+// Robot.shooter.setFgain(Constants.SHOOTER_F);
+// if (Robot.drivetrain.getSonarDistance() > 60)
+// {
+// Robot.shooter.moveHood(Constants.HOOD_LEVER);
+// Robot.shooter.manualRPM(Constants.SHOOTER_MOTOR_SPEED_LEVER);
+// }
+// else
+// {
+// Robot.shooter.manualRPM(Constants.SHOOTER_MOTOR_SPEED_BOILER);
+// Robot.shooter.moveHood(Constants.HOOD_BOILER);
+// }
}
- // Called repeatedly when this Command is scheduled to run
- protected void execute() {
- Robot.shooter.manualSpeed(Constants.SHOOTER_MOTOR_SPEED_HIGH);
+ // Called repeatedly 50 times/sec when this Command is scheduled to run
+ protected void execute() {
+// if (Robot.drivetrain.getSonarDistance() > 60)
+// {
+// Robot.shooter.moveHood(Constants.HOOD_LEVER);
+// Robot.shooter.manualRPM(Constants.SHOOTER_MOTOR_SPEED_LEVER);
+// }
+// else
+// {
+ Robot.shooter.manualRPM(Constants.SHOOTER_MOTOR_SPEED_BOILER);
+ Robot.shooter.moveHood(Constants.HOOD_BOILER);
+// }
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
- return false;
+ return (isTimedOut());
}
- // Called once after isFinished returns true
protected void end() {
Robot.shooter.stop();
}
- // Called when another command which requires one or more of the same
- // subsystems are scheduled to run
protected void interrupted() {
end();
}
+ */
}
diff --git a/src/org/usfirst/frc/team708/robot/commands/shooter/SpinShooterBack.java b/src/org/usfirst/frc/team708/robot/commands/shooter/SpinShooterBack.java
new file mode 100644
index 0000000..b4cea91
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/shooter/SpinShooterBack.java
@@ -0,0 +1,58 @@
+package org.usfirst.frc.team708.robot.commands.shooter;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class SpinShooterBack extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+ /*
+
+ public SpinShooterBack() {
+// requires(Robot.shooter);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+ Robot.shooter.manualSpeed(Constants.SHOOTER_MOTOR_BACKWARD);
+ }
+
+ // 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.shooter.stop();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+ */
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/shooter/StopShooter.java b/src/org/usfirst/frc/team708/robot/commands/shooter/StopShooter.java
new file mode 100644
index 0000000..594d958
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/shooter/StopShooter.java
@@ -0,0 +1,58 @@
+package org.usfirst.frc.team708.robot.commands.shooter;
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Gamepad;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.OI;
+
+//import org.team708.robot.OI;
+//import org.team708.robot.subsystems.Loader;
+//import org.team708.robot.util.Gamepad;
+//import org.team708.robot.commands.shooter.Fire;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class StopShooter extends Command {
+
+ @Override
+ protected boolean isFinished() {
+ // TODO Auto-generated method stub
+ return false;
+ }
+
+/*
+ public StopShooter() {
+ requires(Robot.shooter);
+ }
+
+// Called just before this Command runs the first time
+ protected void initialize() {
+// Robot.shooter.manualRPM(Constants.SHOOTER_MOTOR_OFF);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ protected void execute() {
+ Robot.shooter.manualSpeed(Constants.SHOOTER_MOTOR_OFF);
+ }
+
+ // 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() {
+ Robot.shooter.stop();
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems are scheduled to run
+ protected void interrupted() {
+ end();
+ }
+ */
+}
diff --git a/src/org/usfirst/frc/team708/robot/commands/visionProcessor/FindTarget.java b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/FindTarget.java
index 8ae1eaf..79ac4fe 100644
--- a/src/org/usfirst/frc/team708/robot/commands/visionProcessor/FindTarget.java
+++ b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/FindTarget.java
@@ -1,41 +1,40 @@
-package org.usfirst.frc.team708.robot.commands.visionProcessor;
-
-import org.usfirst.frc.team708.robot.Robot;
-
-
-import edu.wpi.first.wpilibj.command.Command;
-
-/**
- *
- */
-public class FindTarget extends Command {
-
- public FindTarget() {
- // Use requires() here to declare subsystem dependencies
- requires(Robot.visionProcessor);
- }
-
- // 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.visionProcessor.processData();
- // Robot.drivetrain.haloDrive(Robot.visionProcessor.getMove(0.5), Robot.visionProcessor.getRotate());
- }
-
- // 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() {
- }
-}
+//package org.usfirst.frc.team708.robot.commands.visionProcessor;
+//
+//import org.usfirst.frc.team708.robot.Robot;
+//
+//
+//import edu.wpi.first.wpilibj.command.Command;
+//
+///**
+// *
+// */
+//public class FindTarget extends Command {
+//
+// public FindTarget() {
+//// requires(Robot.visionProcessor);
+// }
+//
+// // 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.visionProcessor.processData();
+// Robot.drivetrain.haloDrive(Robot.visionProcessor.getMove(), Robot.visionProcessor.getRotate());
+// }
+//
+// // Make this return true when this Command no longer needs to run execute()
+// protected boolean isFinished() {
+// return(Robot.visionProcessor.isCentered() && Robot.visionProcessor.isAtY());
+// }
+//
+// // 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/team708/robot/commands/visionProcessor/GripPipelineBoiler.java b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineBoiler.java
new file mode 100644
index 0000000..0ec7a3f
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineBoiler.java
@@ -0,0 +1,187 @@
+package org.usfirst.frc.team708.robot.commands.visionProcessor;
+
+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.*;
+
+/**
+* GripPipelineBoiler class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class GripPipelineBoiler implements VisionPipeline {
+
+ //Outputs
+ private Mat rgbThresholdOutput = new Mat();
+ private ArrayList findContoursOutput = 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 RGB_Threshold0:
+ Mat rgbThresholdInput = source0;
+ double[] rgbThresholdRed = {0.0, 0.0};
+ double[] rgbThresholdGreen = {15.0, 255.0};
+ double[] rgbThresholdBlue = {0.0, 0.0};
+ rgbThreshold(rgbThresholdInput, rgbThresholdRed, rgbThresholdGreen, rgbThresholdBlue, rgbThresholdOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = rgbThresholdOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = findContoursOutput;
+ double filterContoursMinArea = 60.0;
+ double filterContoursMinPerimeter = 40.0;
+ double filterContoursMinWidth = 20.0;
+ double filterContoursMaxWidth = 100.0;
+ double filterContoursMinHeight = 10.0;
+ double filterContoursMaxHeight = 1000.0;
+ double[] filterContoursSolidity = {0, 100};
+ double filterContoursMaxVertices = 1000000.0;
+ double filterContoursMinVertices = 0.0;
+ double filterContoursMinRatio = 0.0;
+ double filterContoursMaxRatio = 50.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 RGB_Threshold.
+ * @return Mat output from RGB_Threshold.
+ */
+ public Mat rgbThresholdOutput() {
+ return rgbThresholdOutput;
+ }
+
+ /**
+ * 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 Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Segment an image based on color ranges.
+ * @param input The image on which to perform the RGB threshold.
+ * @param red The min and max red.
+ * @param green The min and max green.
+ * @param blue The min and max blue.
+ * @param output The image in which to store the output.
+ */
+ private void rgbThreshold(Mat input, double[] red, double[] green, double[] blue,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2RGB);
+ Core.inRange(out, new Scalar(red[0], green[0], blue[0]),
+ new Scalar(red[1], green[1], blue[1]), out);
+ }
+
+ /**
+ * 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);
+ }
+
+
+ /**
+ * 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/team708/robot/commands/visionProcessor/GripPipelineGear.java b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineGear.java
new file mode 100644
index 0000000..4d61151
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineGear.java
@@ -0,0 +1,187 @@
+//package org.usfirst.frc.team708.robot.commands.visionProcessor;
+//
+//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.*;
+//
+///**
+//* GripPipelineLift class.
+//*
+//* An OpenCV pipeline generated by GRIP.
+//*
+//* @author GRIP
+//*/
+//public class GripPipelineGear implements VisionPipeline {
+//
+// //Outputs
+// private Mat hslThresholdOutput = new Mat();
+// private ArrayList findContoursOutput = 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 HSL_Threshold0:
+// Mat hslThresholdInput = source0;
+// double[] hslThresholdHue = {87.41007194244604, 180.0};
+// double[] hslThresholdSaturation = {179.10669303814598, 255.0};
+// double[] hslThresholdLuminance = {91.72661870503596, 255.0};
+// hslThreshold(hslThresholdInput, hslThresholdHue, hslThresholdSaturation, hslThresholdLuminance, hslThresholdOutput);
+//
+// // Step Find_Contours0:
+// Mat findContoursInput = hslThresholdOutput;
+// boolean findContoursExternalOnly = false;
+// findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+//
+// // Step Filter_Contours0:
+// ArrayList filterContoursContours = findContoursOutput;
+// double filterContoursMinArea = 200.0;
+// double filterContoursMinPerimeter = 50.0;
+// double filterContoursMinWidth = 10.0;
+// double filterContoursMaxWidth = 60.0;
+// double filterContoursMinHeight = 11.0;
+// double filterContoursMaxHeight = 80.0;
+// double[] filterContoursSolidity = {0, 100};
+// double filterContoursMaxVertices = 1000000;
+// double filterContoursMinVertices = 0;
+// double filterContoursMinRatio = 0;
+// double filterContoursMaxRatio = 1000;
+// 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 HSL_Threshold.
+// * @return Mat output from HSL_Threshold.
+// */
+// public Mat hslThresholdOutput() {
+// return hslThresholdOutput;
+// }
+//
+// /**
+// * 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 Filter_Contours.
+// * @return ArrayList output from Filter_Contours.
+// */
+// public ArrayList filterContoursOutput() {
+// return filterContoursOutput;
+// }
+//
+//
+// /**
+// * 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);
+// }
+//
+// /**
+// * 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);
+// }
+//
+//
+// /**
+// * 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/team708/robot/commands/visionProcessor/GripPipelineLift.java b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineLift.java
new file mode 100644
index 0000000..e86c369
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineLift.java
@@ -0,0 +1,187 @@
+package org.usfirst.frc.team708.robot.commands.visionProcessor;
+
+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.*;
+
+/**
+* GripPipelineLift class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class GripPipelineLift implements VisionPipeline {
+
+ //Outputs
+ private Mat rgbThresholdOutput = new Mat();
+ private ArrayList findContoursOutput = 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 RGB_Threshold0:
+ Mat rgbThresholdInput = source0;
+ double[] rgbThresholdRed = {0.0, 0.0};
+ double[] rgbThresholdGreen = {56, 255.0};
+ double[] rgbThresholdBlue = {0.0, 0.0};
+ rgbThreshold(rgbThresholdInput, rgbThresholdRed, rgbThresholdGreen, rgbThresholdBlue, rgbThresholdOutput);
+
+ // Step Find_Contours0:
+ Mat findContoursInput = rgbThresholdOutput;
+ boolean findContoursExternalOnly = false;
+ findContours(findContoursInput, findContoursExternalOnly, findContoursOutput);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = findContoursOutput;
+ double filterContoursMinArea = 5.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 = 1000000.0;
+ double filterContoursMinVertices = 0.0;
+ double filterContoursMinRatio = 0.0;
+ double filterContoursMaxRatio = 1000.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 RGB_Threshold.
+ * @return Mat output from RGB_Threshold.
+ */
+ public Mat rgbThresholdOutput() {
+ return rgbThresholdOutput;
+ }
+
+ /**
+ * 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 Filter_Contours.
+ * @return ArrayList output from Filter_Contours.
+ */
+ public ArrayList filterContoursOutput() {
+ return filterContoursOutput;
+ }
+
+
+ /**
+ * Segment an image based on color ranges.
+ * @param input The image on which to perform the RGB threshold.
+ * @param red The min and max red.
+ * @param green The min and max green.
+ * @param blue The min and max blue.
+ * @param output The image in which to store the output.
+ */
+ private void rgbThreshold(Mat input, double[] red, double[] green, double[] blue,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2RGB);
+ Core.inRange(out, new Scalar(red[0], green[0], blue[0]),
+ new Scalar(red[1], green[1], blue[1]), out);
+ }
+
+ /**
+ * 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);
+ }
+
+
+ /**
+ * 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/team708/robot/commands/visionProcessor/GripPipelineLiftGear.java b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineLiftGear.java
new file mode 100644
index 0000000..11cf8e2
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/commands/visionProcessor/GripPipelineLiftGear.java
@@ -0,0 +1,246 @@
+package org.usfirst.frc.team708.robot.commands.visionProcessor;
+
+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.*;
+
+/**
+* GripPipelineLiftGear class.
+*
+* An OpenCV pipeline generated by GRIP.
+*
+* @author GRIP
+*/
+public class GripPipelineLiftGear implements VisionPipeline {
+
+ //Outputs
+ private Mat resizeImageOutput = new Mat();
+ private Mat rgbThreshold0Output = new Mat();
+ private ArrayList findContours0Output = new ArrayList();
+ private Mat rgbThreshold1Output = new Mat();
+ private ArrayList findContours1Output = 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 RGB_Threshold0:
+ Mat rgbThreshold0Input = resizeImageOutput;
+ double[] rgbThreshold0Red = {0.0, 0.0};
+ double[] rgbThreshold0Green = {74.43502824858757, 141.36363636363637};
+ double[] rgbThreshold0Blue = {0.0, 0.0};
+ rgbThreshold(rgbThreshold0Input, rgbThreshold0Red, rgbThreshold0Green, rgbThreshold0Blue, rgbThreshold0Output);
+
+ // Step Find_Contours0:
+ Mat findContours0Input = rgbThreshold0Output;
+ boolean findContours0ExternalOnly = false;
+ findContours(findContours0Input, findContours0ExternalOnly, findContours0Output);
+
+ // Step RGB_Threshold1:
+ Mat rgbThreshold1Input = resizeImageOutput;
+ double[] rgbThreshold1Red = {255.0, 255.0}; //{156.58204205866073, 255.0};
+ double[] rgbThreshold1Green = {255.0, 255.0}; //{156.8172385168788, 255.0};
+ double[] rgbThreshold1Blue = {0.0, 150.0}; // {55.03597122302158, 143.37518287980558};
+ rgbThreshold(rgbThreshold1Input, rgbThreshold1Red, rgbThreshold1Green, rgbThreshold1Blue, rgbThreshold1Output);
+
+ // Step Find_Contours1:
+ Mat findContours1Input = rgbThreshold1Output;
+ boolean findContours1ExternalOnly = false;
+ findContours(findContours1Input, findContours1ExternalOnly, findContours1Output);
+
+ // Step Filter_Contours0:
+ ArrayList filterContoursContours = findContours1Output;
+ double filterContoursMinArea = 5.0;
+ double filterContoursMinPerimeter = 0.0;
+ double filterContoursMinWidth = 0.0;
+ double filterContoursMaxWidth = 1000.0;
+ double filterContoursMinHeight = 0.0;
+ double filterContoursMaxHeight = 998.0;
+ double[] filterContoursSolidity = {0.0, 100.0};
+ double filterContoursMaxVertices = 1000000.0;
+ double filterContoursMinVertices = 0.0;
+ double filterContoursMinRatio = 0.0;
+ double filterContoursMaxRatio = 1000.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 RGB_Threshold.
+ * @return Mat output from RGB_Threshold.
+ */
+ public Mat rgbThreshold0Output() {
+ return rgbThreshold0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContours0Output() {
+ return findContours0Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a RGB_Threshold.
+ * @return Mat output from RGB_Threshold.
+ */
+ public Mat rgbThreshold1Output() {
+ return rgbThreshold1Output;
+ }
+
+ /**
+ * This method is a generated getter for the output of a Find_Contours.
+ * @return ArrayList output from Find_Contours.
+ */
+ public ArrayList findContours1Output() {
+ return findContours1Output;
+ }
+
+ /**
+ * 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 color ranges.
+ * @param input The image on which to perform the RGB threshold.
+ * @param red The min and max red.
+ * @param green The min and max green.
+ * @param blue The min and max blue.
+ * @param output The image in which to store the output.
+ */
+ private void rgbThreshold(Mat input, double[] red, double[] green, double[] blue,
+ Mat out) {
+ Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2RGB);
+ Core.inRange(out, new Scalar(red[0], green[0], blue[0]),
+ new Scalar(red[1], green[1], blue[1]), out);
+ }
+
+ /**
+ * 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);
+ }
+
+
+ /**
+ * 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/team708/robot/subsystems/Climber.java b/src/org/usfirst/frc/team708/robot/subsystems/Climber.java
new file mode 100644
index 0000000..abf397b
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Climber.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.commands.Climber.ManualMoveClimber;
+
+import com.ctre.phoenix.motorcontrol.can.*;
+
+//import org.usfirst.frc.team708.robot.RobotMap;
+//import edu.wpi.first.wpilibj.DigitalInput;
+//import edu.wpi.first.wpilibj.Encoder;
+//import edu.wpi.first.wpilibj.Talon;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * @author James Makovics
+ * @author Cody Cooper
+ * @author James McPeak
+ */
+public class Climber extends Subsystem {
+ public static WPI_TalonSRX climberMotor; // Uses the Motor CanTalon
+
+ /**
+ * Constructor
+ */
+ public Climber() {
+ // Initializes the motor for the Climber
+ climberMotor = new WPI_TalonSRX (RobotMap.climberMotor);
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ public void manualMove(double speed) {
+ climberMotor.set(speed);
+ }
+
+ public void stop(){
+ climberMotor.set(Constants.MOTOR_OFF);
+ }
+
+ public void sendToDashboard() {
+ if (Constants.DEBUG) {
+ }
+ }
+}
+
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team708/robot/subsystems/Drivetrain.java
index 87e3794..716fa88 100644
--- a/src/org/usfirst/frc/team708/robot/subsystems/Drivetrain.java
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Drivetrain.java
@@ -2,6 +2,8 @@
import org.usfirst.frc.team708.robot.Constants;
import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.Robot;
+
import org.usfirst.frc.team708.robot.RobotMap;
import org.usfirst.frc.team708.robot.commands.drivetrain.JoystickDrive;
import org.usfirst.frc.team708.robot.commands.visionProcessor.SonarOverride;
@@ -12,16 +14,21 @@
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import com.ctre.CANTalon;
-import com.ctre.CANTalon.FeedbackDevice;
-import com.ctre.CanTalonJNI;
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+//import com.ctre.CANTalon.FeedbackDevice;
+//import com.ctre.CanTalonJNI;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Solenoid;
//import edu.wpi.first.wpilibj.interfaces.Gyro;
//import edu.wpi.first.wpilibj.GyroBase;
//import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Talon;
-import edu.wpi.first.wpilibj.TalonSRX;
+//import edu.wpi.first.wpilibj.TalonSRX;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -29,13 +36,16 @@
public class Drivetrain extends PIDSubsystem {
- private boolean usePID = true;
+
+ private ADXRS450_Gyro gyro;
+ private int count = 0;
// Variables specific for drivetrain PID loop
- private double moveSpeed = 0.0;
- private double pidOutput = 0.0;
+ private boolean usePID = false;
+ private double moveSpeed = 0.0;
+ private double pidOutput = 0.0;
- private CANTalon leftMaster, leftSlave, rightMaster, rightSlave; // Motor Controllers
+ private WPI_TalonSRX leftMaster, leftSlave, rightMaster, rightSlave; // Motor Controllers
private HatterDrive drivetrain; // FRC provided drivetrain class
@@ -43,51 +53,87 @@ public class Drivetrain extends PIDSubsystem {
private Encoder encoder2; // Encoder for the drivetrain
private double distancePerPulse;
- private BuiltInAccelerometer accelerometer; // Accelerometer that is built into the roboRIO
- private ADXRS450_Gyro gyro; // Gyro that is used for drift correction
+// private BuiltInAccelerometer accelerometer; // Accelerometer that is built into the roboRIO
- private IRSensor drivetrainIRSensor; // IR Sensor for <=25inches
- private UltrasonicSensor drivetrainUltrasonicSensor; // Sonar used for <=21feet
- private DigitalInput opticalSensor;
+// private IRSensor drivetrainIRSensor; // IR Sensor for <=25inches
+ private UltrasonicSensor drivetrainUltrasonicSensor; // Sonar used for <=21feet
+// private DigitalInput opticalSensor;
+// private DigitalInput gearSensor;
- public int sonarOverride = 0; //0 = default, 1 = high, 2 = low; Used for overriding sonar
- private boolean brake = true; // Whether the talons should be in coast or brake mode
- // (this could be important if a jerky robot causes things to topple
-
- /**
- * Constructor
- */
+ public int sonarOverride = 0; //0 = default, 1 = high, 2 = low; Used for overriding sonar
+ private boolean brake = true; // Whether the talons should be in coast or brake mode
+ private boolean nobrake = false; // Whether the talons should be in coast or brake mode
+
+ public static Solenoid pwr0;
+ public static Solenoid pwr1;
+ public static Solenoid pwr2;
+ public static Solenoid pwr3;
+ public static Solenoid gearLight;
+ public static Solenoid boilerLight;
+
public Drivetrain() {
// Passes variables from this class into the superclass constructor
super("Drivetrain", Constants.Kp, Constants.Ki, Constants.Kd);
+ gyro = new ADXRS450_Gyro(); // Initializes the gyro
+ gyro.reset(); // Resets the gyro so that it starts with a 0.0 value
+
+
// Initializes motor controllers with device IDs from RobotMap
- leftMaster = new CANTalon(RobotMap.drivetrainLeftMotorMaster);
- leftSlave = new CANTalon(RobotMap.drivetrainLeftMotorSlave);
- rightMaster = new CANTalon(RobotMap.drivetrainRightMotorMaster);
- rightSlave = new CANTalon(RobotMap.drivetrainRightMotorSlave);
+ leftMaster = new WPI_TalonSRX(RobotMap.drivetrainLeftMotorMaster);
+ leftSlave = new WPI_TalonSRX(RobotMap.drivetrainLeftMotorSlave);
+ rightMaster = new WPI_TalonSRX(RobotMap.drivetrainRightMotorMaster);
+ rightSlave = new WPI_TalonSRX(RobotMap.drivetrainRightMotorSlave);
drivetrain = new HatterDrive(leftMaster, rightMaster, Constants.DRIVE_USE_SQUARED_INPUT); // Initializes drivetrain class
setupMasterSlave(); // Sets up master and slave
- accelerometer = new BuiltInAccelerometer(); // Initializes the accelerometer from the roboRIO
- gyro = new ADXRS450_Gyro(); // Initializes the gyro
- gyro.reset(); // Resets the gyro so that it starts with a 0.0 value
+// accelerometer = new BuiltInAccelerometer(); // Initializes the accelerometer from the roboRIO
+
encoder = new Encoder(RobotMap.drivetrainEncoderARt, RobotMap.drivetrainEncoderBRt, Constants.DRIVETRAIN_USE_LEFT_ENCODER);
+// encoder = new Encoder(leftMaster.getPinStateQuadA(), leftMaster.getPinStateQuadB(), Constants.DRIVETRAIN_USE_LEFT_ENCODER);
+
encoder2 = new Encoder(RobotMap.drivetrainEncoderALeft, RobotMap.drivetrainEncoderBLeft, !Constants.DRIVETRAIN_USE_LEFT_ENCODER);
- // Initializes the encoder
+// encoder2 = new Encoder(rightMaster.getPinStateQuadA(), rightMaster.getPinStateQuadB(), !Constants.DRIVETRAIN_USE_LEFT_ENCODER);
+
distancePerPulse = (Constants.DRIVETRAIN_WHEEL_DIAMETER * Math.PI) /
- (Constants.DRIVETRAIN_ENCODER_PULSES_PER_REV);
- // Sets the distance per pulse of the encoder to read distance properly
- encoder.setDistancePerPulse(distancePerPulse);
- encoder.reset(); // Resets the encoder so that it starts with a 0.0 value
+ (Constants.DRIVETRAIN_ENCODER_PULSES_PER_REV);
+
+ leftMaster.set(ControlMode.PercentOutput,Constants.DRIVETRAIN_ENCODER_PULSES_PER_REV);
+ rightMaster.set(ControlMode.PercentOutput,Constants.DRIVETRAIN_ENCODER_PULSES_PER_REV);
+
+ encoder.setDistancePerPulse(distancePerPulse); // Sets the distance per pulse of the encoder to read distance properly
+ encoder.reset(); // Resets the encoder so that it starts with a 0.0 value
+
encoder2.setDistancePerPulse(distancePerPulse);
- encoder2.reset(); // Resets the encoder so that it starts with a 0.0 value
+ encoder2.reset();
-// drivetrainIRSensor = new IRSensor(RobotMap.DTIRSensor, IRSensor.GP2Y0A21YK0F);
- drivetrainUltrasonicSensor = new UltrasonicSensor(RobotMap.dtSonar, UltrasonicSensor.MB1010);
- }
+ leftMaster.setNeutralMode(NeutralMode.Brake);
+ leftSlave.setNeutralMode(NeutralMode.Brake);
+ rightMaster.setNeutralMode(NeutralMode.Brake);
+ rightSlave.setNeutralMode(NeutralMode.Brake);
+
+// drivetrainIRSensor = new IRSensor(RobotMap.gearIRSensor, IRSensor.GP2Y0A21YK0F);
+ drivetrainUltrasonicSensor = new UltrasonicSensor(RobotMap.dtSonar, UltrasonicSensor.MB1010);
+
+// gearSensor = new DigitalInput(RobotMap.gearSensorSwitch);
+
+ pwr0 = new Solenoid(RobotMap.PWR0);
+ pwr1 = new Solenoid(RobotMap.PWR1);
+ pwr2 = new Solenoid(RobotMap.PWR2);
+ pwr3 = new Solenoid(RobotMap.PWR3);
+ gearLight = new Solenoid(RobotMap.GEARLIGHT);
+ boilerLight = new Solenoid(RobotMap.BOILERLIGHT);
+
+ pwr0.set(true);
+ pwr1.set(true);
+ pwr2.set(true);
+ pwr3.set(true);
+
+// setGearLight(true);
+// setBoilerLight(true);
+}
/**
@@ -119,9 +165,7 @@ public void haloDrive(double move, double rotate, boolean usePID) {
{
getPIDController().setPID(Constants.Kp, Constants.Ki, Constants.Kd);
getPIDController().reset();
- gyro.reset();
enable();
- gyro.reset();
}
else
{
@@ -148,6 +192,14 @@ public void haloDrive(double move, double rotate) {
haloDrive(move, rotate, this.usePID);
}
+ public void setGearLight(boolean on) {
+ gearLight.set(on);
+ }
+
+ public void setBoilerLight(boolean on) {
+ boilerLight.set(on);
+ }
+
public boolean getUsePID() {
return usePID;
}
@@ -166,16 +218,23 @@ public void stop() {
* @return
*/
public double getAngle() {
- return gyro.getAngle();
+ return -gyro.getAngle(); //gyro is mounted upside down
}
/**
* Resets the gyro reading
*/
public void resetGyro() {
+// count++;
+// SmartDashboard.putNumber("resetgyro: ", count);
gyro.reset();
}
+// public boolean hasGear() {
+// return gearSensor.get();
+//
+// }
+
public double rotateByGyro(double targetAngle, double tolerance) {
double difference = getAngle() - targetAngle;
@@ -186,9 +245,9 @@ public double rotateByGyro(double targetAngle, double tolerance) {
return difference / targetAngle;
}
- public double getIRDistance() {
- return drivetrainIRSensor.getAverageDistance();
- }
+// public double getIRDistance() {
+// return drivetrainIRSensor.getAverageDistance();
+// }
public double getSonarDistance() {
return drivetrainUltrasonicSensor.getClippedAverageDistance();
@@ -200,17 +259,17 @@ public double getSonarDistance() {
* @param targetDistance
* @return
*/
- public double moveByIR(double targetDistance, double minSpeed, double maxSpeed, double tolerance) {
- double current_location = getIRDistance();
-
- double value = Math708.getClippedPercentError(current_location, targetDistance, minSpeed, maxSpeed);
-
- if (value <= 0.0 || ((Math.abs(current_location - targetDistance)) <= tolerance)) {
-
- return 0.0;
- }
- return value;
- }
+// public double moveByIR(double targetDistance, double minSpeed, double maxSpeed, double tolerance) {
+// double current_location = getIRDistance();
+//
+// double value = Math708.getClippedPercentError(current_location, targetDistance, minSpeed, maxSpeed);
+//
+// if (value <= 0.0 || ((Math.abs(current_location - targetDistance)) <= tolerance)) {
+//
+// return 0.0;
+// }
+// return value;
+// }
/**
* Returns the move speed of the robot needed to get to a certain Sonar distance reading.
@@ -234,8 +293,8 @@ public double moveByUltrasonic(double targetDistance, double minSpeed, double ma
* talon is doing
*/
public void setupMasterSlave() {
- leftSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
- rightSlave.changeControlMode(CANTalon.TalonControlMode.Follower);
+ leftSlave.follow(leftMaster);
+ rightSlave.follow(rightMaster);
leftSlave.set(leftMaster.getDeviceID());
rightSlave.set(rightMaster.getDeviceID());
@@ -246,21 +305,29 @@ public void setupMasterSlave() {
*/
public void toggleBrakeMode() {
brake = !brake;
- leftMaster.enableBrakeMode(brake);
- leftSlave.enableBrakeMode(brake);
- rightMaster.enableBrakeMode(brake);
- rightSlave.enableBrakeMode(brake);
+ leftMaster.setNeutralMode(NeutralMode.Brake);
+ leftSlave.setNeutralMode(NeutralMode.Brake);
+ rightMaster.setNeutralMode(NeutralMode.Brake);
+ rightSlave.setNeutralMode(NeutralMode.Brake);
}
+ public void setBrakeMode(Boolean brake) {
+ leftMaster.setNeutralMode(NeutralMode.Brake);
+ leftSlave.setNeutralMode(NeutralMode.Brake);
+ rightMaster.setNeutralMode(NeutralMode.Brake);
+ rightSlave.setNeutralMode(NeutralMode.Brake);
+ }
/**
* Sets encoder direction depending on which side of the drivetrain it is on
*/
public void setEncoderReading() {
encoder.setReverseDirection(Constants.DRIVETRAIN_USE_LEFT_ENCODER);
+ // leftMaster.setInverted(true);
}
public void setEncoderReading2() {
- encoder.setReverseDirection(!Constants.DRIVETRAIN_USE_LEFT_ENCODER);
+ encoder2.setReverseDirection(!Constants.DRIVETRAIN_USE_LEFT_ENCODER);
+// rightMaster.setInverted(true);
}
/**
@@ -269,9 +336,13 @@ public void setEncoderReading2() {
*/
public double getEncoderDistance() {
return encoder.getDistance();
+// return leftMaster.getEncPosition();
+
+
}
public double getEncoderDistance2() {
return encoder2.getDistance();
+// return rightMaster.getEncPosition();
}
/**
* Resets the encoder to 0.0
@@ -286,9 +357,9 @@ public void resetEncoder2() {
* Returns if the optical sensor detects the color white
* @return
*/
- public boolean isOpticalSensorWhite() {
- return opticalSensor.get();
- }
+// public boolean isOpticalSensorWhite() {
+// return opticalSensor.get();
+// }
/**
* Returns a process variable to the PIDSubsystem for correction
@@ -307,6 +378,15 @@ protected void usePIDOutput(double output) {
drivetrain.arcadeDrive(moveSpeed, -output);
}
+ public int getAlliance(){
+ return Robot.allianceColor;
+ }
+
+ public void setAlliance(int c)
+ {
+ Robot.allianceColor = c;
+ }
+
/**
* Sends data for this subsystem to the dashboard
*/
@@ -320,19 +400,20 @@ public void sendToDashboard() {
// SmartDashboard.putNumber("Gyro Rate", gyro.getRate()); // Gyro rate
// SmartDashboard.putNumber("PID Output", pidOutput); // PID Info
// SmartDashboard.putNumber("DT Encoder Raw", encoder.get()); // Encoder raw count
-// SmartDashboard.putBoolean("Brake", brake); // Brake or Coast
// SmartDashboard.putNumber("DT IR Distance", getIRDistance()); // IR distance reading
//
// SmartDashboard.putNumber("DT Rt Master", rightMaster.getTemperature());
// SmartDashboard.putNumber("DT Rt Slave", rightSlave.getTemperature());
// SmartDashboard.putNumber("DT Lft Master", leftMaster.getTemperature());
// SmartDashboard.putNumber("DT Lft Slave", leftSlave.getTemperature());
+// SmartDashboard.putNumber("Sonar Mode", sonarOverride);
+// SmartDashboard.putNumber("DT Encoder 1 Distance", encoder.getDistance()); // Encoder reading
+ SmartDashboard.putNumber("DT Encoder 2 Distance", encoder2.getDistance()); // Encoder reading
}
- SmartDashboard.putNumber("Gyro angle", ( (int)gyro.getAngle())); // Gyro angle
+ SmartDashboard.putBoolean("Brake", brake); // Brake or Coast
+ SmartDashboard.putNumber("AllianceColor", getAlliance());
+ SmartDashboard.putNumber("Gyro angle", ( (int)getAngle())); // Gyro angle
// SmartDashboard.putNumber("DT Sonar Distance", getSonarDistance()); // Sonar distance reading
-// SmartDashboard.putNumber("DT Encoder Distance", encoder.getDistance()); // Encoder reading
-// SmartDashboard.putNumber("DT Encoder 2 Distance", encoder2.getDistance()); // Encoder reading
-// SmartDashboard.putNumber("Sonar Mode", sonarOverride);
}
}
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Feeder.java b/src/org/usfirst/frc/team708/robot/subsystems/Feeder.java
new file mode 100644
index 0000000..b17b01c
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Feeder.java
@@ -0,0 +1,49 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+import org.usfirst.frc.team708.robot.OI;
+import org.usfirst.frc.team708.robot.commands.drivetrain.JoystickDrive;
+import org.usfirst.frc.team708.robot.commands.visionProcessor.SonarOverride;
+
+import com.ctre.CANTalon;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Leaders
+ *
+ */
+public class Feeder extends Subsystem {
+
+ private WPI_TalonSRX feedMotor;
+ /**
+ * Constructor
+ */
+ public Feeder() {
+ feedMotor = new WPI_TalonSRX(RobotMap.feederMotor); //initializes the loading motor
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ public void manualMove(double speed){
+ feedMotor.set(speed);
+ }
+
+ public void stop(){
+ feedMotor.set(Constants.MOTOR_OFF);
+ }
+
+ public void sendToDashboard() {
+// SmartDashboard.putNumber("Loader Motor Speed", feedMotor.getSpeed());
+
+ if (Constants.DEBUG) {
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Hanger.java b/src/org/usfirst/frc/team708/robot/subsystems/Hanger.java
deleted file mode 100644
index 63ef20a..0000000
--- a/src/org/usfirst/frc/team708/robot/subsystems/Hanger.java
+++ /dev/null
@@ -1,44 +0,0 @@
-package org.usfirst.frc.team708.robot.subsystems;
-
-
-import org.usfirst.frc.team708.robot.Constants;
-//import org.usfirst.frc.team708.robot.RobotMap;
-//import edu.wpi.first.wpilibj.DigitalInput;
-//import edu.wpi.first.wpilibj.Encoder;
-//import edu.wpi.first.wpilibj.Talon;
-import edu.wpi.first.wpilibj.command.Subsystem;
-//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- *
- */
-public class Hanger extends Subsystem {
-
-
-
- /**
- * Constructor
- */
- public Hanger() {
-
-
-
- }
-
- public void initDefaultCommand() {
- // Set the default command for a subsystem here.
-
- }
-
- /**
- * Sends data to the Smart Dashboard
- */
- public void sendToDashboard() {
-
-
- if (Constants.DEBUG) {
-
- }
- }
-}
-
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Intake.java b/src/org/usfirst/frc/team708/robot/subsystems/Intake.java
deleted file mode 100644
index 487e76e..0000000
--- a/src/org/usfirst/frc/team708/robot/subsystems/Intake.java
+++ /dev/null
@@ -1,49 +0,0 @@
-package org.usfirst.frc.team708.robot.subsystems;
-
-import org.usfirst.frc.team708.robot.Constants;
-//import org.usfirst.frc.team708.robot.RobotMap;
-//import edu.wpi.first.wpilibj.Relay;
-//import edu.wpi.first.wpilibj.Relay.Value;
-//import edu.wpi.first.wpilibj.Talon;
-import edu.wpi.first.wpilibj.command.Subsystem;
-//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-/**
- * Subsystem that intakes balls
- * @author James_Makovics
- * @author Michael_Steinberg
- * @author Thomas Zhao
- * @author Alex Tysak
- */
-
-public class Intake extends Subsystem {
-
- /**
- * Constructor
- */
- public Intake() {
-
- }
-
- public void initDefaultCommand() {
-
- }
-
- public void moveMotor(double speed) {
-
- }
-
- public void stop(){
-
- }
-
- /**
- * Sends data about the subsystem to the Smart Dashboard
- */
- public void sendToDashboard() {
- if (Constants.DEBUG) {
- }
- }
-
-
-}
-
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Intake_Ball.java b/src/org/usfirst/frc/team708/robot/subsystems/Intake_Ball.java
new file mode 100644
index 0000000..85f6708
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Intake_Ball.java
@@ -0,0 +1,44 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.RobotMap;
+
+import com.ctre.CANTalon;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Subsystem that intakes balls
+ * @author Madison
+ * @author Nick
+ */
+
+public class Intake_Ball extends Subsystem {
+
+ private WPI_TalonSRX intakeMotor;
+
+ public Intake_Ball() {
+ intakeMotor = new WPI_TalonSRX (RobotMap.intakeMotorBall);
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ //I believe this sets the speed of the motor
+ public void moveMotor(double speed) {
+ intakeMotor.set(speed);
+ }
+ //I believe this stops the motor
+ public void stop(){
+ intakeMotor.set(Constants.INTAKE_OFF);
+ }
+
+//Sends data about the subsystem to the Smart Dashboard
+ public void sendToDashboard() {
+ if (Constants.DEBUG) {
+ }
+ }
+}
+
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Intake_Gear.java b/src/org/usfirst/frc/team708/robot/subsystems/Intake_Gear.java
new file mode 100644
index 0000000..5359321
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Intake_Gear.java
@@ -0,0 +1,70 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.RobotMap;
+
+import com.ctre.CANTalon;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Subsystem that intakes balls
+ * @author Madison
+ * @author Nick
+ */
+
+public class Intake_Gear extends Subsystem {
+
+ private WPI_TalonSRX intakeMotor;
+ private DigitalInput gearSensor;
+
+ //I'm trying to link the right motor to the intake code here
+ public Intake_Gear() {
+ intakeMotor = new WPI_TalonSRX (RobotMap.intakeMotorGear);
+ gearSensor = new DigitalInput(RobotMap.gearSensorSwitch);
+
+// intakeMotor.reset();
+// intakeMotor.changeControlMode(TalonControlMode.Voltage);
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ //I believe this sets the speed of the motor
+ public void moveMotor(double speed) {
+ intakeMotor.set(speed);
+ }
+
+ public boolean hasGear() {
+
+ if (gearSensor.get()) {
+// Robot.led1.send_to_led(Constants.SET_HAS_GEAR_TARGETING);
+ return (true);
+ }
+ else {
+// Robot.led1.send_to_led(Robot.ledAllianceColor);
+ return (false);
+ }
+ }
+ public void stop(){
+ intakeMotor.set(Constants.INTAKE_OFF);
+
+ }
+
+
+ /**
+ * Sends data about the subsystem to the Smart Dashboard
+ */
+ public void sendToDashboard() {
+ if (Constants.DEBUG) {
+ SmartDashboard.putBoolean("has gear", hasGear());
+ }
+ }
+
+
+}
+
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/LED.java b/src/org/usfirst/frc/team708/robot/subsystems/LED.java
new file mode 100644
index 0000000..e1b0e7f
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/LED.java
@@ -0,0 +1,57 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+import org.usfirst.frc.team708.robot.AutoConstants;
+
+//import org.team708.robot.commands.visionProcessor.ProcessData;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.util.Math708;
+
+//import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+//import edu.wpi.first.wpilibj.networktables2.type.NumberArray;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.SerialPort.*;
+
+/**
+ *
+ */
+
+public class LED extends Subsystem {
+
+ public static SerialPort led_out;
+ public static Port port;
+
+ public static byte[] msg = new byte[10];
+
+ public static String messageback;
+ static public byte count = 0x00;
+
+ public LED() {
+
+// port = Port.kOnboard; //on board serial - bits were reversed 0x55 = 0xAA to arduino
+ port = Port.kMXP; //expansion board serial
+ led_out = new SerialPort(9600, port, 8, Parity.kNone, StopBits.kOne);
+ led_out.setWriteBufferMode(WriteBufferMode.kFlushOnAccess);
+ msg[0] = 0x00;
+ }
+
+ public void send_to_led(byte command){
+ msg[0] = command;
+ led_out.write(msg, 1);
+ led_out.flush();
+ }
+
+ public void sendToDashboard() {
+ if (Constants.DEBUG) {
+ SmartDashboard.putNumber("LED code sent", count);
+ }
+ }
+
+ public void initDefaultCommand() {
+ }
+}
+
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Loader.java b/src/org/usfirst/frc/team708/robot/subsystems/Loader.java
index d59199a..b05b9df 100644
--- a/src/org/usfirst/frc/team708/robot/subsystems/Loader.java
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Loader.java
@@ -1,50 +1,59 @@
-package org.usfirst.frc.team708.robot.subsystems;
-
-
-import org.usfirst.frc.team708.robot.Constants;
-import org.usfirst.frc.team708.robot.RobotMap;
-import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
-import org.usfirst.frc.team708.robot.OI;
-import org.usfirst.frc.team708.robot.commands.drivetrain.JoystickDrive;
-import org.usfirst.frc.team708.robot.commands.visionProcessor.SonarOverride;
-
-import com.ctre.CANTalon;
-import com.ctre.CANTalon.FeedbackDevice;
-import com.ctre.CANTalon.TalonControlMode;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- * Leaders
- *
- */
-public class Loader extends Subsystem {
-
- private CANTalon loadMotor;
- /**
- * Constructor
- */
- public Loader() {
- loadMotor = new CANTalon(RobotMap.loaderMotor); //initializes the loading motor
- }
-
- public void initDefaultCommand() {
- // setDefaultCommand(new ManualLoader());
- }
-
- public void manualMove(double speed){
- loadMotor.set(speed);
- }
-
- public void stop(){
- loadMotor.set(Constants.MOTOR_OFF);
- }
-
- public void sendToDashboard() {
- SmartDashboard.putNumber("Loader Motor Speed", loadMotor.getSpeed());
-
- if (Constants.DEBUG) {
- }
- }
-}
\ No newline at end of file
+//package org.usfirst.frc.team708.robot.subsystems;
+//
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.RobotMap;
+//import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
+//import org.usfirst.frc.team708.robot.OI;
+//import org.usfirst.frc.team708.robot.commands.drivetrain.JoystickDrive;
+//import org.usfirst.frc.team708.robot.commands.visionProcessor.SonarOverride;
+//
+//import com.ctre.CANTalon;
+//import com.ctre.CANTalon.FeedbackDevice;
+//import com.ctre.CANTalon.TalonControlMode;
+//import edu.wpi.first.wpilibj.command.Subsystem;
+//
+//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+//
+///**
+// * Leaders
+// *
+// */
+//public class Loader extends Subsystem {
+//
+// private CANTalon loadMotor;
+// private boolean spinforward;
+//
+// /**
+// * Constructor
+// */
+// public Loader() {
+// loadMotor = new CANTalon(RobotMap.loaderMotor); //initializes the loading motor
+// spinforward = false;
+// }
+//
+// public void initDefaultCommand() {
+// }
+//
+// public void manualMove(double speed){
+// loadMotor.set(speed);
+// }
+//
+// public void stop(){
+// loadMotor.set(Constants.MOTOR_OFF);
+// }
+//
+// public boolean toggleSpin() {
+// spinforward = !spinforward;
+// return(spinforward);
+// }
+//
+// public boolean spinForward() {
+// return(spinforward);
+// }
+//
+// public void sendToDashboard() {
+// if (Constants.DEBUG) {
+// }
+// }
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Pivot_Gear.java b/src/org/usfirst/frc/team708/robot/subsystems/Pivot_Gear.java
new file mode 100644
index 0000000..f8e53c8
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Pivot_Gear.java
@@ -0,0 +1,65 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.util.Math708;
+
+import com.ctre.CANTalon;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Subsystem that intakes balls
+ * @author Madison
+ * @author Nick
+ */
+
+public class Pivot_Gear extends Subsystem {
+
+ private WPI_TalonSRX pivotMotor;
+
+ //I'm trying to link the right motor to the intake code here
+ public Pivot_Gear() {
+ pivotMotor = new WPI_TalonSRX (RobotMap.pivotGearMotor);
+ // pivotMotor.Enabled();
+
+// pivotMotor.reverseSensor(true);
+// pivotMotor.enableBrakeMode(true);
+// pivotMotor.setFeedbackDevice(FeedbackDevice.QuadEncoder);
+// pivotMotor.changeControlMode(com.ctre.CANTalon.TalonControlMode.Position);
+// pivotMotor.configEncoderCodesPerRev(Constants.PIVOT_GEAR_ENCODER_COUNT);
+ }
+
+ public void initDefaultCommand() {
+ }
+
+ //I believe this sets the speed of the motor
+ public void moveMotor(double speed) {
+ pivotMotor.set(speed);
+ }
+}
+// public boolean isFwdSwitch() {
+ // return (pivotMotor.getPosition()>= Constants.PIVOT_GEAR_ENCODER_HIGH);
+// return (false);
+// }
+/*
+ public boolean isRevSwitch() {
+ return (pivotMotor.getPosition()<= Constants.PIVOT_GEAR_ENCODER_LOW);
+// return (false);
+ }
+
+ //I believe this stops the motor
+ public void stop(){
+ pivotMotor.set(Constants.INTAKE_OFF);
+ }
+
+ public void sendToDashboard() {
+ SmartDashboard.putNumber("Pivot encoder", pivotMotor.getPosition());
+ if (Constants.DEBUG) {
+ }
+ }
+}
+*/
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team708/robot/subsystems/Shooter.java
index 3ba95ec..f5d575f 100644
--- a/src/org/usfirst/frc/team708/robot/subsystems/Shooter.java
+++ b/src/org/usfirst/frc/team708/robot/subsystems/Shooter.java
@@ -2,55 +2,66 @@
import org.usfirst.frc.team708.robot.Constants;
import org.usfirst.frc.team708.robot.RobotMap;
+import org.usfirst.frc.team708.robot.commands.shooter.ManualShoot;
import org.usfirst.frc.team708.robot.commands.shooter.SpinShooter;
import org.usfirst.frc.team708.robot.OI;
import org.usfirst.frc.team708.robot.commands.drivetrain.JoystickDrive;
import org.usfirst.frc.team708.robot.commands.visionProcessor.SonarOverride;
import com.ctre.CANTalon;
-import com.ctre.CANTalon.FeedbackDevice;
-import com.ctre.CANTalon.TalonControlMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.Talon;
-import edu.wpi.first.wpilibj.TalonSRX;
+import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-/**
- *
- */
public class Shooter extends Subsystem {
// Put methods for controlling this subsystem here. Call these
// from Commands.
- private CANTalon shooter; // Motor Controllers
+ private WPI_TalonSRX shooter, shooterSlave; // Motor Controllers
+ private Servo hood;
+ private int hoodLocation;
+ private int spinSpeed;
/**
* Constructor
*/
- public Shooter() {
+// public Shooter() {
// Initializes the encoder
// Initializes the motor
+/*
+ shooter = new WPI_TalonSRX(RobotMap.shooterMotorMaster);
+ shooterSlave = new WPI_TalonSRX(RobotMap.shooterMotorSlave);
- shooter = new CANTalon(56);
- shooter.reset();
-// shooter.setFeedbackDevice(FeedbackDevice.QuadEncoder);
-// shooter.reverseSensor(false);
-// shooter.configEncoderCodesPerRev(500);
- shooter.changeControlMode(TalonControlMode.PercentVbus);
-// shooter.changeControlMode(TalonControlMode.Speed);
-// shooter.setAllowableClosedLoopErr(0);
-// shooter.configNominalOutputVoltage(0.0, 0.0);
-// shooter.configPeakOutputVoltage(12.0,-12.0);
- shooter.set(0);
+ shooterSlave.changeControlMode(WPI_TalonSRX.TalonControlMode.Follower);
+ shooterSlave.set(shooter.getDeviceID());
+
+// shooter.reset();
+ shooter.enable();
+
+ shooter.reverseSensor(true);
+ shooter.setFeedbackDevice(FeedbackDevice.QuadEncoder);
+// shooter.changeControlMode(TalonControlMode.PercentVbus);
+ shooter.changeControlMode(TalonControlMode.Speed);
+
+ shooter.configNominalOutputVoltage(Constants.NOMINAL_POS, Constants.NOMINAL_NEG);
+ shooter.configPeakOutputVoltage(Constants.SHOOTER_PEAK_POS, Constants.SHOOTER_PEAK_NEG);
+ shooter.configEncoderCodesPerRev(Constants.SHOOTER_ENCODER_PULSES);
+ shooter.setPID(Constants.SHOOTER_P, Constants.SHOOTER_I, Constants.SHOOTER_D, Constants.SHOOTER_F, Constants.SHOOTER_IZONE, Constants.SHOOTER_RAMPRATE, Constants.SHOOTER_PROFILE);
+
+ hood = new Servo(RobotMap.hoodAngle);
+ hoodLocation = Constants.HOOD_MIN;
+ spinSpeed = 0;
}
public void initDefaultCommand() {
@@ -58,20 +69,63 @@ public void initDefaultCommand() {
}
public void manualSpeed(double speed) {
+ shooter.changeControlMode(TalonControlMode.PercentVbus);
+ shooter.set(speed);
+}
+
+ public void manualRPM(double rpm) {
shooter.changeControlMode(TalonControlMode.Speed);
-// shooter.set(speed);
- shooter.set(1);
+ shooter.set(rpm);
}
+
+// public void setFgain(double F){
+// shooter.setF(F);
+// }
+
+ public void setSpinSpeed(int speed)
+ {
+ spinSpeed = speed;
+ }
+
+ public int getSpinSpeed()
+ {
+ return(spinSpeed);
+ }
+
public void stop(){
shooter.set(Constants.MOTOR_OFF);
}
+
+ public void moveHood(int angle) {
+ hoodLocation = angle;
+ hood.setRaw(angle);
+ SmartDashboard.putNumber("Move Hood", hoodLocation);
+ }
+
+ public void hoodAdjust(double angle) {
+
+ if ((angle > 0.0) && (hoodLocationConstants.HOOD_MIN)) hoodLocation-=Constants.HOOD_CALIBRATION;
+
+ SmartDashboard.putNumber("Servo Location", hoodLocation);
+ moveHood(hoodLocation);
+ }
/**
* Sends data to the Smart Dashboard
*/
public void sendToDashboard() {
- SmartDashboard.putNumber("Encoder Position", shooter.getEncPosition());
- SmartDashboard.putNumber("Encoder Speed", shooter.getSpeed());
- SmartDashboard.putNumber("Encoder Velocity", shooter.getEncVelocity());
+ if (Constants.DEBUG) {
+ }
+// SmartDashboard.putNumber("Encoder Position", shooter.getEncPosition());
+// SmartDashboard.putNumber("Encoder Speed", shooter.getSpeed());
+// SmartDashboard.putNumber("Encoder Velocity", shooter.getEncVelocity());
+
+ }
+
+ @Override
+ protected void initDefaultCommand() {
+ // TODO Auto-generated method stub
+
}
}
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/VisionBoiler.java b/src/org/usfirst/frc/team708/robot/subsystems/VisionBoiler.java
new file mode 100644
index 0000000..a721d3a
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/VisionBoiler.java
@@ -0,0 +1,501 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+import org.opencv.core.Rect;
+import edu.wpi.cscore.AxisCamera;
+import org.opencv.imgproc.Imgproc;
+import org.usfirst.frc.team708.robot.AutoConstants;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.visionProcessor.GripPipelineBoiler;
+import org.usfirst.frc.team708.robot.util.Math708;
+
+import edu.wpi.cscore.CvSource;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.vision.VisionThread;
+
+/**
+ *@authors Viet & Sue
+ *This subsystem is specific to the 2017 Game FIRST Steamworks Boiler Goal
+ */
+public class VisionBoiler extends Subsystem {
+
+ // Camera Variables
+ private double fovDegrees = AutoConstants.USB_BOILER_FOV_DEGREES; // Field of View of the Camera
+ private double bPipelineSize; // Number of Contours in the Pipline- 0 = target not in view
+ private int bImageWidth = AutoConstants.USB_BOILER_IMG_WIDTH; // Width of image
+ private int bImageHeight = AutoConstants.USB_BOILER_IMG_HEIGHT; // Height of image
+
+ // Image OpenCV Image Processing Variables
+ private VisionThread visionThreadBoiler; // vision processing thread - processes grip code
+ private final Object imgLockBoiler = new Object(); // vision boiler object
+
+// private UsbCamera usbCameraBoiler; // USB Camera
+ private AxisCamera axisCameraBoiler; // Axis Camera
+
+ private CvSource outputStreamBoiler; // Output stream to the Dashboard
+
+
+ // Targeting variables
+ private int brectX = 0; // the 4 values used which define the full rectangle around the target
+ private int brectY = 0;
+ private int brectWidth = 0;
+ private int brectHeight = 0;
+
+ private int bminX = 0; // the 4 values used to create maximum rectangle around the target (used when evaluating each of the contour images)
+ private int bminY = 0;
+ private int bmaxX = 0;
+ private int bmaxY = 0;
+
+ private boolean boilerHasTarget = false; // flag indicating whether the robot sees the target
+ private boolean boilerIsCentered = false; // flag indicating whether the robot sees the center of the target
+ private boolean boilerIsAtDistance = false; // flag indicating whether the robot is at the correct distance from the target
+ private boolean boilerIsAtHeight = false; // Determine if the robot is at height (eyy, that's the name of the boolean!)
+
+ private int boilerTargetHeight = AutoConstants.BOILER_TARGET_HEIGHT; //actual height of the boilers tape
+ private int boilerTargetWidth = AutoConstants.BOILER_TARGET_WIDTH; //actual width of the boilers tape
+
+ private double trueCenter = bImageWidth/2; // horizontal value of the center of the target
+
+// private double boilerDistanceToStop = 0.0; // distance to stop at in front of boiler target
+ private double boilerCurrentCenter = 0.0; // horizontal value of where robot is looking
+ private double boilerCurrentHeight = 0.0; // height value of where robot is looking
+ private double boilerStopAtHeight = 0.0; // distance to stop at based on height
+ private double boilerStopAtDistance = 0.0; // distance to stop at based on sonar
+
+
+ private double thresholdX = AutoConstants.X_THRESHOLD_CENTER_BOILER; // threshold for determining center of the target
+ private double thresholdDistance = AutoConstants.DISTANCE_TARGET_THRESHOLD; // threshold for determining threshold for stopping at the target
+ private double minThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MIN; // threshold for determining min value for whether the robot sees the target
+ private double maxThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MAX; // threshold for determining max value for whether the robot sees the target
+ private double thresholdHeight = AutoConstants.Y_HEIGHT_THRESHOLD; // threshold for deteriming threshold for stopping at the specified height in the target
+
+ // Sweep Variables
+ private boolean boilerInSweep = false; // flag indicating whether the robot is sweeping left and right looking for the target
+ private double boilerSweepDirection = 0.0; // value indicating the direction of the sweep -1 = right; 1 = left
+ private int boilerSweepCounter = 0; // value indicating when the sweep will change direction
+
+
+ // drive variables
+ private double boilerRotateDiff = 0; // for smartdashboard - how far away from center
+ private double boilerMoveDiff = 0; // for smartdashboard - how far away from target
+ double boilerRotate; // speed of the rotate being returned to the command
+ double boilerMove; // speed of the move forward being returned to the command
+
+
+ // Vision Processing
+ public VisionBoiler() {
+ super("Vision Processor");
+
+
+ // define the Cameras: --
+ axisCameraBoiler=CameraServer.getInstance().addAxisCamera(AutoConstants.AXIS_CAMERA_ID, AutoConstants.AXIS_IP_ADDRESS);
+
+// usbCameraBoiler=CameraServer.getInstance().startAutomaticCapture("cam0", 0);
+ axisCameraBoiler.setResolution(bImageWidth, bImageHeight);
+
+
+ // define the output stream on the smart dashboard
+ outputStreamBoiler = CameraServer.getInstance().putVideo("Target Boiler", bImageWidth, bImageHeight);
+
+
+ // Vision thread which processes the image contours
+ visionThreadBoiler = new VisionThread(axisCameraBoiler, new GripPipelineBoiler(), boilerPipeline -> {
+ bPipelineSize = boilerPipeline.filterContoursOutput().size();
+
+ // if the grip pipeline filter "filterContoursOutput" sees the target
+ // loop through each contour image
+ // grab the bounding rectangle values of each contour
+ // to create the biggest rectangle around the 2 vertical retroreflective tapes
+ // on either side of the lift peg
+ if (!boilerPipeline.filterContoursOutput().isEmpty()) {
+
+ for (int i = 0; i < boilerPipeline.filterContoursOutput().size(); i++) {
+ Rect r = Imgproc.boundingRect(boilerPipeline.filterContoursOutput().get(i));
+
+ // set the min/max values to match the values form the 1st image
+ if (i == 0) {
+ bminX = r.x;
+ bminY = r.y;
+ bmaxX = r.x + r.width;
+ bmaxY = r.y + r.height;
+ }
+
+ // compare each value to the min/max and replace if a better one is found
+ if (r.x < bminX) {
+ bminX = r.x;
+ }
+ if (r.y < bminY) {
+ bminY = r.y;
+ }
+ if (r.x + r.width > bmaxX) {
+ bmaxX = r.x + r.width;
+ }
+ if (r.y + r.height> bmaxY) {
+ bmaxY = r.y + r.height;
+ }
+ }
+
+// // this is a second method of looping through the contours in the filterContoursOutput Array of Mat Images
+// for (MatOfPoint contour : pipeline.filterContoursOutput()) {
+// Rect r = Imgproc.boundingRect(contour);
+// if (r.x < minX) {
+// minX = r.x;
+// }
+// }
+
+
+
+ synchronized (imgLockBoiler) {
+ boilerCurrentCenter = bminX + ((bmaxX - bminX) / 2);
+
+ // set values for the smartdashboard
+ brectX = bminX;
+ brectY = bminY;
+ brectWidth = bmaxX - bminX;
+ brectHeight = bmaxY - bminY;
+
+ // brectY - represents where the height the robot is looking
+ boilerCurrentHeight = brectY;
+
+
+
+ // display the current image on the driver station
+ if (Constants.BOILER_CAMERA_OUTPUT_DEBUG){
+ outputStreamBoiler.putFrame(boilerPipeline.rgbThresholdOutput());
+ }
+ }
+
+ }
+
+ // the target is not in the camera (ie, pipeline is empty)
+ else {
+ boilerHasTarget = false;
+ bminX = 0;
+ bminY = 0;
+ bmaxX = 0;
+ bmaxY = 0;
+ }
+
+ });
+ visionThreadBoiler.start();
+ }
+
+ /*
+ * GetClosestLocation
+ * Determine which shooting location is closer to the robot
+ * Will not use right now
+ */
+// public double getClosestLocation() {
+// if(Robot.drivetrain.getSonarDistance() >= AutoConstants.DISTANCE_TO_BOILER_LOCATION2/2) {
+// return AutoConstants.DISTANCE_TO_BOILER_LOCATION2;
+// }
+// else {
+// return AutoConstants.DISTANCE_TO_BOILER_LOCATION1;
+// }
+// }
+
+ /*
+ * ProcessData
+ * Method to interpret the camera data received above
+ */
+
+ /*
+ public void boilerProcessData() {
+ try {
+
+ // using the Y value which is calculated above for processing the how high the robot is seeing
+
+ // if robot sees the target (current X between its min and max)
+ if ((boilerCurrentCenter > minThresholdX) && (boilerCurrentCenter < maxThresholdX)) {
+ boilerHasTarget = true;
+ }
+ else {
+ boilerHasTarget = false;
+ }
+
+ } catch (TableKeyNotDefinedException e) {
+ e.printStackTrace();
+ }
+ }
+ */
+ /*
+ * isCentered
+ * Method to determine whether the robot sees the center of the target (within the threshold value)
+ */
+ public boolean boilerIsCentered() {
+
+ // if the robot sees the target
+ // determine whether the horizontal value the robot sees is within the threshold defining the center of the target
+ // set isCentered according to whether the robot is aligned with the center of the target
+ if (boilerHasTarget)
+ {
+
+ double difference = trueCenter - (boilerCurrentCenter);
+ if (Math.abs(difference) <= thresholdX) {
+ boilerIsCentered = true;
+ }
+ else if (Math.abs(difference) > thresholdX) {
+ boilerIsCentered = false;
+ }
+ boilerRotateDiff = difference;
+ }
+ else{
+ boilerIsCentered = false;
+ }
+ return boilerIsCentered;
+ }
+
+ /*
+ * getRotate
+ * Method to determine whether the robot is at the center of the target so it can drive towards target
+ */
+ public double boilerGetRotate() {
+ double difference=0;
+
+ // currently we are only running 1 cycle of the sweep and stopping
+ // if in the future additional sweeps are required, this is where the reset should occur
+// if (sweepCounter > 400){
+// sweepCounter = 0;
+// }
+
+ // if robot sees target and is centered - no need to rotate the robot
+ if (boilerHasTarget && boilerIsCentered)
+ {
+ boilerRotate = 0.0;
+ }
+
+ // if the robot sees the target but is not centered
+ // check to see whether the robot is within the threshold
+ // rotate based on the math function
+ else if (boilerHasTarget && !boilerIsCentered){
+ difference = trueCenter - (boilerCurrentCenter);
+
+// boilerRotate = Math708.getClippedPercentError(boilerCurrentCenter, trueCenter, AutoConstants.DRIVE_ROTATE_MIN, AutoConstants.DRIVE_ROTATE_MAX);
+ boilerRotate = AutoConstants.BOILER_ROTATE_SPEED;
+
+ if (Math.abs(difference) > thresholdX) {
+ if (boilerCurrentCenter < trueCenter){
+ boilerRotate = Math.abs(boilerRotate);
+ }
+ else {
+ boilerRotate = Math.abs(boilerRotate) * -1;
+ }
+ }
+ }
+
+ // if the robot does not have the target
+ // begin the sweep
+ // sweep is defined as rotating the robot right, left, right in predefined counts
+ // if in the sweep the robot does not find the target, it stops after 3 sweeps
+ // otherwise it will jump back into the hasTarget logic identified above
+ else if (!boilerHasTarget){
+// if (Math.abs(boilerSweepDirection) < .1){
+// boilerSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// boilerRotate = AutoConstants.SWEEP_ROTATE;
+// }
+// else if (boilerSweepDirection > AutoConstants.SWEEP1_MIN){
+// if ((boilerSweepCounter >= AutoConstants.SWEEP1_MIN && boilerSweepCounter <= AutoConstants.SWEEP1_MAX)
+// || (boilerSweepCounter >= AutoConstants.SWEEP3_MIN && boilerSweepCounter <= AutoConstants.SWEEP3_MAX)){
+//
+// boilerRotate = AutoConstants.SWEEP_ROTATE;
+// if (boilerSweepCounter== AutoConstants.SWEEP1_MAX || boilerSweepCounter== AutoConstants.SWEEP3_MAX){
+// boilerSweepDirection = AutoConstants.SWEEP_DIRECTION_LEFT;
+// }
+// }
+// }
+// else {
+// if (boilerSweepCounter >= AutoConstants.SWEEP2_MIN && boilerSweepCounter <= AutoConstants.SWEEP2_MAX)
+// boilerRotate = -AutoConstants.SWEEP_ROTATE;
+// if (boilerSweepCounter== AutoConstants.SWEEP2_MAX){
+// boilerSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// }
+// }
+
+ boilerSweepCounter++;
+ boilerRotate = 0.0; //no sweep sit there till time out or find target rolling forward slowly
+ }
+ else
+ {
+ boilerRotate = 0.0;
+ SmartDashboard.putBoolean("boiler in dead end if", true);
+ }
+ boilerRotateDiff = difference; // what is this for????
+
+ return boilerRotate;
+ }
+
+ /*
+ * getMove
+ * Method to determine if the robot is close enough to target so it can stop
+ */
+
+ public double boilerGetMove() {
+ // if the robot sees the target
+ // Method to determine whether the robot is at the correct distance to the target so stop
+ if (boilerHasTarget)
+ {
+ double difference;
+ double boilerCurrentDistance;
+ double boilerStopAtValue;
+// double differenceY = boilerStopAtHeight - boilerCurrentHeight;
+ double differenceY = boilerCurrentHeight - boilerStopAtHeight;
+
+ //using the y value
+ difference = differenceY;
+ boilerCurrentDistance = boilerCurrentHeight;
+ boilerStopAtValue = boilerStopAtHeight;
+
+ boilerMove = Math708.getClippedPercentError(boilerCurrentDistance, boilerStopAtValue, AutoConstants.DRIVE_MOVE_MIN, AutoConstants.DRIVE_MOVE_MAX);
+// boilerMove = .3;
+ //if the target distance is farther than the current distance move backwards
+// if(difference >= 0){
+// boilerMove = boilerMove * -1;
+// }
+
+ //Check if target is at correct distance within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+ if (difference <= thresholdDistance) {
+ boilerMove = 0.0;
+ boilerIsAtDistance = true;
+ } else {
+ boilerIsAtDistance = false;
+ }
+ boilerMoveDiff = difference;
+ } else { // no target - where is it?
+// boilerMove = 0.0;
+ boilerMove = 0.2; //move forward slowly
+ }
+
+// return boilerMove;
+ return 0;
+ }
+
+
+
+
+
+ /**
+ * GETTERS and PUTTERS to return the private variables
+ * @return
+ */
+
+
+
+
+
+ /*
+ * isAtHeight
+ * Method to determine whether the robot is at the distance from the target based on the threshold value
+ */
+ public boolean boilerIsAtHeight() {
+// double difference = boilerStopAtHeight - boilerCurrentHeight;
+ double difference = boilerCurrentHeight - boilerStopAtHeight;
+
+ //Check if target is at correct level within threshold
+// if (Math.abs(difference) <= thresholdHeight) {
+ if (difference <= thresholdHeight) {
+ boilerIsAtHeight = true;
+ } else {
+ boilerIsAtHeight = false;
+ }
+ return boilerIsAtHeight;
+ }
+
+ public boolean boilerIsHasTarget() {
+ return boilerHasTarget;
+ }
+
+
+ public void putBoilerCurrentCenter(double cc) {
+ boilerCurrentCenter = cc;
+ }
+
+
+ public void putBoilerHasTarget(boolean ht) {
+ boilerHasTarget = ht;
+ }
+
+ public int getBoilerCounter() {
+ return boilerSweepCounter;
+ }
+
+
+
+ public void putBoilerCounter(int ct) {
+ boilerSweepCounter = ct;
+ }
+
+ public void putBoilerIsCentered(boolean ic) {
+ boilerIsCentered = ic;
+ }
+
+ public void putBoilerStopAtDistance (double sad) {
+ boilerStopAtDistance = sad;
+ // :(
+ }
+
+ public void putBoilerStopAtHeight (double sah) {
+ boilerStopAtHeight = sah;
+ }
+
+
+ public void putBoilerCurrentHeight (double ch) {
+ boilerCurrentHeight = ch;
+ // :(
+ }
+
+
+ public void putBoilerAtDistance(boolean ad) {
+ boilerIsAtDistance = ad;
+ }
+
+ public void putBoilerAtHeight(boolean ah) {
+ boilerIsAtHeight = ah;
+ }
+
+ public boolean boilerIsInSweep() {
+ if (boilerHasTarget) {
+ boilerInSweep = false;
+ boilerSweepCounter=1;
+ }
+ else {
+ boilerInSweep = true;
+ }
+ return boilerInSweep;
+ }
+
+ public void sendToDashboard() {
+ if (Constants.BOILER_DEBUG) {
+// SmartDashboard.putNumber("b-True Center", trueCenter);
+// SmartDashboard.putBoolean("b-Has Target", boilerIsHasTarget());
+// SmartDashboard.putBoolean("b-IsAtHeight", boilerIsAtHeight());
+// SmartDashboard.putNumber("b-Rotation", boilerRotate);
+// SmartDashboard.putNumber("b-Rotate Difference", boilerRotateDiff);
+// SmartDashboard.putNumber("b-Distance Move Difference", boilerMoveDiff);
+// SmartDashboard.putNumber("b-Sweep Counter", boilerSweepCounter);
+// SmartDashboard.putNumber("b-SweepDirection", boilerSweepDirection);
+// SmartDashboard.putBoolean("b-isCentered", boilerIsCentered());
+// SmartDashboard.putNumber("b-rectX", brectX);
+// SmartDashboard.putNumber("b-maxY", bmaxY);
+// SmartDashboard.putNumber("b-rectY", brectY);
+// SmartDashboard.putNumber("b-rectWidth", brectWidth);
+// SmartDashboard.putNumber("b-rectHeight", brectHeight);
+// SmartDashboard.putNumber("b-pipelineSize", bPipelineSize);
+// SmartDashboard.putNumber("b-stop at distance", boilerStopAtDistance);
+// SmartDashboard.putNumber("b-boiler current height (y)", boilerCurrentHeight);
+
+ SmartDashboard.putNumber("b-Center of Target", boilerCurrentCenter);
+
+ }
+ }
+
+ public void initDefaultCommand() {
+ if (Constants.DEBUG) {
+ }
+ }
+}
+
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/VisionGear.java b/src/org/usfirst/frc/team708/robot/subsystems/VisionGear.java
new file mode 100644
index 0000000..3ab9828
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/VisionGear.java
@@ -0,0 +1,426 @@
+//package org.usfirst.frc.team708.robot.subsystems;
+//
+//import org.opencv.core.Rect;
+//import org.opencv.imgproc.Imgproc;
+//import org.usfirst.frc.team708.robot.AutoConstants;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.commands.visionProcessor.GripPipelineGear;
+//import org.usfirst.frc.team708.robot.util.Math708;
+//
+//import edu.wpi.cscore.AxisCamera;
+//import edu.wpi.cscore.CvSource;
+//import edu.wpi.cscore.UsbCamera;
+//import edu.wpi.first.wpilibj.CameraServer;
+//import edu.wpi.first.wpilibj.command.Subsystem;
+//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+//import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
+//import edu.wpi.first.wpilibj.vision.VisionThread;
+//
+///**
+// *@authors Viet & Sue
+// *This subsystem is specific to the 2017 Game FIRST Steamworks Gear
+// */
+//public class VisionGear extends Subsystem {
+//
+// // Camera Variables
+// private double fovDegrees = AutoConstants.USB_FOV_DEGREES; // Field of View of the Camera
+// private double pipelineSize; // Number of Contours in the Pipline- 0 = target not in view
+// private int imageWidth = AutoConstants.USB_IMG_WIDTH; // Width of image
+// private int imageHeight = AutoConstants.USB_IMG_HEIGHT; // Height of image
+//
+// // Image OpenCV Image Processing Variables
+// private VisionThread visionThread; // vision processing thread - processes grip code
+// private final Object imgLock = new Object(); // vision Gear object
+//
+// private AxisCamera axisCamera; // Axis Camera
+// private UsbCamera usbCamera; // USB Camera
+// private CvSource outputStream; // Output stream to the Dashboard
+//
+//
+// // Targeting variables
+// private int rectX = 0; // the 4 values used which define the full rectangle around the target
+// private int rectY = 0;
+// private int rectWidth = 0;
+// private int rectHeight = 0;
+//
+// private int minX = 0; // the 4 values used to create maximum rectangle around the target (used when evaluating each of the contour images)
+// private int minY = 0;
+// private int maxX = 0;
+// private int maxY = 0;
+//
+// private boolean hasTarget = false; // flag indicating whether the robot sees the target
+// private boolean isCentered = false; // flag indicating whether the robot sees the center of the target
+// private boolean isAtDistance = false; // flag indicating whether the robot is at the correct distance from the target
+//
+//
+// private int TargetHeight = AutoConstants.GEAR_TARGET_HEIGHT; //Target height
+// private int TargetWidth = AutoConstants.GEAR_TARGET_WIDTH; //Target width
+//
+// private double trueCenter = imageWidth/2; // horizontal value of the center of the target
+// private double distanceToStop = AutoConstants.DISTANCE_TO_GEAR; // distance to stop at in front of gear
+// private double currentCenter = 0.0; // horizontal value of where robot is looking
+// private double currentDistance = 0.0; // distance robot is from the target
+//
+// private double thresholdX = AutoConstants.X_THRESHOLD_CENTER; // threshold for determining center of the target
+// private double thresholdDistance = AutoConstants.DISTANCE_TARGET_THRESHOLD; // threshold for determining threshold for stopping at the gear
+////VIET NEED MIN/MAX FOR GEAR AND CENTER
+// private double minThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MIN; // threshold for determining min value for whether the robot sees the target
+// private double maxThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MAX; // threshold for determining max value for whether the robot sees the target
+//
+//
+// // Sweep Variables
+// private boolean inSweep = false; // flag indicating whether the robot is sweeping left and right looking for the target
+// private double sweepDirection = 0.0; // value indicating the direction of the sweep -1 = right; 1 = left
+// private int sweepCounter = 0; // value indicating when the sweep will change direction
+//
+//
+// // drive variables
+// private double RotateDiff = 0; // for smartdashboard - how far away from center
+// private double MoveDiff = 0; // for smartdashboard - how far away from target
+// double rotate; // speed of the rotate being returned to the command
+// double move; // speed of the move forward being returned to the command
+//
+//
+// // Vision Processing
+// public VisionGear() {
+// super("Vision Processor");
+//
+//
+// // define the Cameras:
+//// usbCamera=CameraServer.getInstance().startAutomaticCapture("cam3", 0);
+//// axisCamera=CameraServer.getInstance().addAxisCamera("cam1", "10.7.8.11");
+//// axisCamera.setResolution(imageWidth, imageHeight);
+//
+//
+// // define the output stream on the smart dashboard
+// outputStream = CameraServer.getInstance().putVideo("Target", imageWidth, imageHeight);
+//
+//
+// // Vision thread which processes the image contours
+// visionThread = new VisionThread(usbCamera, new GripPipelineGear(), pipeline -> {
+// pipelineSize = pipeline.filterContoursOutput().size();
+//
+// // if the grip pipeline filter "filterContoursOutput" sees the target
+// // loop through each contour image
+// // grab the bounding rectangle values of each contour
+// // to create the biggest rectangle around the gear
+// if (!pipeline.filterContoursOutput().isEmpty()) {
+//
+// for (int i = 0; i < pipeline.filterContoursOutput().size(); i++) {
+// Rect r = Imgproc.boundingRect(pipeline.filterContoursOutput().get(i));
+//
+// // set the min/max values to match the values form the 1st image
+// if (i == 0) {
+// minX = r.x;
+// minY = r.y;
+// maxX = r.x + r.width;
+// maxY = r.y + r.height;
+// }
+//
+// // compare each value to the min/max and replace if a better one is found
+// if (r.x < minX) {
+// minX = r.x;
+// }
+// if (r.y < minY) {
+// minY = r.y;
+// }
+// if (r.x + r.width > maxX) {
+// maxX = r.x + r.width;
+// }
+// if (r.y + r.height> maxY) {
+// maxY = r.y + r.height;
+// }
+// }
+//
+//// // this is a second method of looping through the contours in the filterContoursOutput Array of Mat Images
+//// for (MatOfPoint contour : pipeline.filterContoursOutput()) {
+//// Rect r = Imgproc.boundingRect(contour);
+//// if (r.x < minX) {
+//// minX = r.x;
+//// }
+//// }
+//
+//
+//
+// synchronized (imgLock) {
+// currentCenter = minX + ((maxX - minX) / 2);
+//
+// // set values for the smartdashboard
+// rectX = minX;
+// rectY = minY;
+// rectWidth = maxX - minX;
+// rectHeight = maxY - minY;
+//
+// // note - this formula was pulled from 1640's github code - need to find the specific reference
+// // from 1640
+// //Equation to determine the distance from a target (d) given the width in pixels of a vision target in the camera image (w):
+// // d = (TARGET_WIDTH*CAMERA_IMAGE_WIDTH)/(2*tan(FOV_ANGLE/2)*w)
+// // i.e. d and w are inversely related.
+// currentDistance = TargetWidth * imageWidth / (2*(Math.tan(Math.toRadians(fovDegrees))/2)*rectWidth);
+//
+// // display the current image on the driver station
+//
+// if (Constants.DEBUG){
+// outputStream.putFrame(pipeline.hslThresholdOutput());
+// }
+// }
+//
+// }
+//
+// // the target is not in the camera (ie, pipeline is empty)
+// else {
+// hasTarget = false;
+// minX = 0;
+// minY = 0;
+// maxX = 0;
+// maxY = 0;
+// }
+//
+// });
+// visionThread.start();
+// }
+//
+//
+// /*
+// * ProcessData
+// * Method to interpret the camera data received above
+// */
+// public void processData() {
+// try {
+//
+// // use the sonar to get the distance from the target (backup plan if camera distance not available)
+//// currentDistance=Robot.drivetrain.getSonarDistance();
+//
+// // if robot sees the target (current X between its min and max)
+// if ((currentCenter > minThresholdX) && (currentCenter < maxThresholdX)) {
+// hasTarget = true;
+// }
+// else {
+// hasTarget = false;
+// }
+//
+// } catch (TableKeyNotDefinedException e) {
+// e.printStackTrace();
+// }
+// }
+//
+// /*
+// * isCentered
+// * Method to determine whether the robot sees the center of the target (within the threshold value)
+// */
+// public boolean isCentered() {
+//
+// // if the robot sees the target
+// // determine whether the horizontal value the robot sees is within the threshold defining the center of the target
+// // set isCentered according to whether the robot is aligned with the center of the target
+// if (hasTarget)
+// {
+//
+// double difference = trueCenter - (currentCenter);
+// if (Math.abs(difference) <= thresholdX) {
+// isCentered = true;
+// }
+// else if (Math.abs(difference) > thresholdX) {
+// isCentered = false;
+// }
+// RotateDiff = difference;
+// }
+// else{
+// isCentered = false;
+// }
+// return isCentered;
+// }
+//
+// /*
+// * getRotate
+// * Method to determine whether the robot is at the center of the target so it can drive towards target
+// */
+// public double getRotate() {
+// double difference=0;
+//
+// // currently we are only running 1 cycle of the sweep and stopping
+// // if in the future additional sweeps are required, this is where the reset should occur
+//// if (sweepCounter > 400){
+//// sweepCounter = 0;
+//// }
+//
+// // if robot sees target and is centered - no need to rotate the robot
+// if (hasTarget && isCentered)
+// {
+// rotate = 0.0;
+// }
+//
+// // if the robot sees the target but is not centered
+// // check to see whether the robot is within the threshold
+// // rotate based on the math function
+// else if (hasTarget && !isCentered){
+// difference = trueCenter - (currentCenter);
+//
+// rotate = Math708.getSignClippedPercentError(currentCenter, trueCenter, AutoConstants.DRIVE_ROTATE_MIN, AutoConstants.DRIVE_ROTATE_MAX);
+//
+//
+// if (Math.abs(difference) > thresholdX) {
+// if (currentCenter < trueCenter){
+// rotate = Math.abs(rotate);
+// }
+// else {
+// rotate = Math.abs(rotate) * -1;
+// }
+// }
+// }
+//
+// // if the robot does not have the target
+// // begin the sweep
+// // sweep is defined as rotating the robot right, left, right in predefined counts
+// // if in the sweep the robot does not find the target, it stops after 3 sweeps
+// // otherwise it will jump back into the hasTarget logic identified above
+// else if (!hasTarget){
+// if (Math.abs(sweepDirection) < .1){
+// sweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// }
+// else if (sweepDirection > AutoConstants.SWEEP1_MIN){
+// if ((sweepCounter >= AutoConstants.SWEEP1_MIN && sweepCounter <= AutoConstants.SWEEP1_MAX)
+// || (sweepCounter >= AutoConstants.SWEEP3_MIN && sweepCounter <= AutoConstants.SWEEP3_MAX)){
+//
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// if (sweepCounter== AutoConstants.SWEEP1_MAX || sweepCounter== AutoConstants.SWEEP3_MAX){
+// sweepDirection = AutoConstants.SWEEP_DIRECTION_LEFT;
+// }
+// }
+// }
+// else {
+// if (sweepCounter >= AutoConstants.SWEEP2_MIN && sweepCounter <= AutoConstants.SWEEP2_MAX)
+// rotate = AutoConstants.SWEEP_ROTATE;
+// if (sweepCounter== AutoConstants.SWEEP2_MAX){
+// sweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// }
+// }
+//
+// sweepCounter++;
+// }
+// RotateDiff = difference;
+// return rotate;
+// }
+//
+// /*
+// * getMove
+// * Method to determine if the robot is close enough to target so it can stop
+// */
+//
+// public double getMove() {
+//
+// // if the robot sees the target
+// // Method to determine whether the robot is at the correct distance to the target so stop
+// if (hasTarget)
+// {
+// double difference = distanceToStop - currentDistance;
+// move = Math708.getSignClippedPercentError(currentDistance, distanceToStop, AutoConstants.DRIVE_MOVE_MIN, AutoConstants.DRIVE_MOVE_MAX);
+//
+// //Check if target is at correct distance within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// move = 0.0;
+// isAtDistance = true;
+// } else {
+// isAtDistance = false;
+// }
+// MoveDiff = difference;
+// } else {
+// move = 0.0;
+// }
+//
+// return move;
+// }
+// /*
+// * isAtDistance
+// * Method to determine whether the robot is at the distance from the target based on the threshold value
+// */
+// public boolean isAtDistance() {
+// double difference = distanceToStop - currentDistance;
+// //Check if target is at correct level within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// isAtDistance = true;
+// } else {
+// isAtDistance = false;
+// }
+// return isAtDistance;
+// }
+//
+// /**
+// * GETTERS and PUTTERS to return the private variables
+// * @return
+// */
+// public boolean isHasTarget() {
+// return hasTarget;
+// }
+//
+//
+// public void putCurrentCenter(double cc) {
+// currentCenter = cc;
+// }
+//
+//
+// public void putHasTarget(boolean ht) {
+// hasTarget = ht;
+// }
+//
+//
+// public int getCounter() {
+// return sweepCounter;
+// }
+//
+//
+// public void putCounter(int ct) {
+// sweepCounter = ct;
+// }
+//
+// public void putIsCentered(boolean ic) {
+// isCentered = ic;
+// }
+//
+//
+// public void putAtDistance(boolean ay) {
+// isAtDistance = ay;
+// }
+//
+//
+// public boolean isInSweep() {
+// if (hasTarget) {
+// inSweep = false;
+// sweepCounter=1;
+// }
+// else {
+// inSweep = true;
+// }
+// return inSweep;
+// }
+//
+// public void sendToDashboard() {
+// if (Constants.DEBUG) {
+// SmartDashboard.putBoolean("Has Target", isHasTarget());
+// SmartDashboard.putBoolean("Is At Distance", isAtDistance());
+// SmartDashboard.putNumber("Current Distance", currentDistance);
+// SmartDashboard.putNumber("Center of Target", currentCenter);
+// SmartDashboard.putNumber("Rotation", rotate);
+// SmartDashboard.putNumber("Rotate Difference", RotateDiff);
+// SmartDashboard.putNumber("Distance Difference", MoveDiff);
+// SmartDashboard.putNumber("Sweep Counter", sweepCounter);
+// SmartDashboard.putNumber("SweepDirection", sweepDirection);
+// SmartDashboard.putBoolean("isCentered", isCentered());
+// SmartDashboard.putNumber("rectX", rectX);
+// SmartDashboard.putNumber("rectY", rectY);
+// SmartDashboard.putNumber("rectWidth", rectWidth);
+// SmartDashboard.putNumber("rectHeight", rectHeight);
+// SmartDashboard.putNumber("Distance To Target", currentDistance);
+// SmartDashboard.putNumber("pipelineSize", pipelineSize);
+// }
+// }
+//
+// public void initDefaultCommand() {
+// if (Constants.DEBUG) {
+// }
+// }
+//}
+//
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/VisionLift.java b/src/org/usfirst/frc/team708/robot/subsystems/VisionLift.java
new file mode 100644
index 0000000..ef8beeb
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/VisionLift.java
@@ -0,0 +1,458 @@
+package org.usfirst.frc.team708.robot.subsystems;
+
+import org.opencv.core.Rect;
+import org.opencv.imgproc.Imgproc;
+import org.usfirst.frc.team708.robot.AutoConstants;
+
+import org.usfirst.frc.team708.robot.Constants;
+import org.usfirst.frc.team708.robot.Robot;
+import org.usfirst.frc.team708.robot.commands.visionProcessor.GripPipelineLift;
+import org.usfirst.frc.team708.robot.util.Math708;
+
+import edu.wpi.cscore.AxisCamera;
+import edu.wpi.cscore.CvSource;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+//import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
+import edu.wpi.first.wpilibj.vision.VisionThread;
+
+/**
+ *@authors Viet & Sue
+ *This subsystem is specific to the 2017 Game FIRST Steamworks Lift Peg
+ */
+
+public class VisionLift extends Subsystem {
+
+ // Camera Variables
+ private double fovDegrees = AutoConstants.USB_LIFT_FOV_DEGREES; // Field of View of the Camera
+ private double liftPipelineSize; // Number of Contours in the Pipline- 0 = target not in view
+ private int liftImageWidth = AutoConstants.USB_LIFT_IMG_WIDTH; // Width of image from camera stream
+ private int liftImageHeight = AutoConstants.USB_LIFT_IMG_HEIGHT; // Height of image from camera stream
+
+ // Image OpenCV Image Processing Variables
+ private VisionThread visionThreadLift; // vision processing thread - processes grip code for lift
+ private final Object imgLockLift = new Object(); // vision Lift object
+
+// private AxisCamera axisCamera; // Axis Camera
+ private UsbCamera usbCameraLiftGear; // USB Camera
+ private CvSource outputStreamLift; // Output stream to the Dashboard
+
+
+ // Targeting variables
+ private int lrectX = 0; // the 4 values used which define the full rectangle around the target
+ private int lrectY = 0;
+ private int grectX = 0;
+ private int grectY = 0;
+ private int lrectWidth = 0;
+ private int lrectHeight = 0;
+ private int grectWidth = 0;
+ private int grectHeight = 0;
+
+ private int lminX = 0; // the 4 values used to create maximum rectangle around the target (used when evaluating each of the contour images)
+ private int lminY = 0;
+ private int lmaxX = 0;
+ private int lmaxY = 0;
+ private int gminX = 0; // the 4 values used to create maximum rectangle around the target (used when evaluating each of the contour images)
+ private int gminY = 0;
+ private int gmaxX = 0;
+ private int gmaxY = 0;
+
+ private boolean liftHasTarget = false; // flag indicating whether the robot sees the target
+ private boolean liftIsCentered = false; // flag indicating whether the robot sees the center of the target
+ private boolean liftIsAtDistance = false; // flag indicating whether the robot is at the correct distance from the target
+
+ private int liftTargetHeight = AutoConstants.LIFT_TARGET_HEIGHT; //height of actual target reflective tape
+ private int liftTargetWidth = AutoConstants.LIFT_TARGET_WIDTH; //width of actual target reflective tape
+
+ private double trueCenter = liftImageWidth/2; // horizontal value of the center of the camera image
+ private double liftDistanceToStop = AutoConstants.DISTANCE_TO_LIFT_TARGET; // distance to stop at in front of lift target
+ private double liftCurrentCenter = 0.0; // horizontal value of where robot is looking
+ private double liftCurrentDistance = 0.0; // distance robot is from the target
+
+ private double thresholdX = AutoConstants.X_THRESHOLD_CENTER; // threshold for determining center of the target
+ private double thresholdDistance = AutoConstants.DISTANCE_TARGET_THRESHOLD; // threshold for determining threshold for stopping at the lift peg
+ private double minThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MIN; // threshold for determining min value for whether the robot sees the target
+ private double maxThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MAX; // threshold for determining max value for whether the robot sees the target
+
+
+ // Sweep Variables
+ private boolean liftInSweep = false; // flag indicating whether the robot is sweeping left and right looking for the target
+ private double liftSweepDirection = 0.0; // value indicating the direction of the sweep -1 = right; 1 = left
+ private int liftSweepCounter = 0; // value indicating when the sweep will change direction
+
+
+ // drive variables
+ private double liftRotateDiff = 0; // for smartdashboard - how far away from center
+ private double liftMoveDiff = 0; // for smartdashboard - how far away from target
+
+ double rotate; // speed of the rotate being returned to the command
+ double move; // speed of the move forward being returned to the command
+
+ // Vision Processing
+ public VisionLift() {
+ super("Vision Processor");
+
+
+// define the Cameras:
+// on little bot - cam2, 1
+// Pipeline outputs: hsl and 0 means lift, rgb and 1 means gear
+// axisCamera=CameraServer.getInstance().addAxisCamera("cam1", "10.7.8.11");
+
+ usbCameraLiftGear=CameraServer.getInstance().startAutomaticCapture(AutoConstants.USB_CAMERA_ID, AutoConstants.USB_VIDEO_PORT);
+ usbCameraLiftGear.setResolution(liftImageWidth, liftImageHeight);
+
+ usbCameraLiftGear.setBrightness(0); //40 for lift
+ usbCameraLiftGear.setExposureManual(0); //was 25 for lift
+ usbCameraLiftGear.setWhiteBalanceManual(10000); //10000 for lift 2800 for gear
+// usbCameraLiftGear.setFPS(20)
+ setLiftCamera();
+
+ // define the output stream on the smart dashboard
+ outputStreamLift = CameraServer.getInstance().putVideo("Target_Lift", liftImageWidth, liftImageHeight);
+
+ // Vision thread which processes the image contours
+ visionThreadLift = new VisionThread(usbCameraLiftGear, new GripPipelineLift(), lgPipeline -> {
+
+ liftPipelineSize = lgPipeline.filterContoursOutput().size();
+
+ // if the grip pipeline filter "filterContours0Output" sees the target
+ // loop through each contour image
+ // grab the bounding rectangle values of each contour
+ // to create the biggest rectangle around the 2 vertical retro-reflective tapes
+ // on either side of the lift peg
+ if (!lgPipeline.filterContoursOutput().isEmpty()) {
+
+ for (int i = 0; i < lgPipeline.filterContoursOutput().size(); i++) {
+ Rect lift = Imgproc.boundingRect(lgPipeline.filterContoursOutput().get(i));
+
+ // set the min/max values to match the values form the 1st image
+ if (i == 0) {
+ lminX = lift.x;
+ lminY = lift.y;
+ lmaxX = lift.x + lift.width;
+ lmaxY = lift.y + lift.height;
+ }
+
+ // compare each value to the min/max and replace if a better one is found
+ if (lift.x < lminX) {
+ lminX = lift.x;
+ }
+ if (lift.y < lminY) {
+ lminY = lift.y;
+ }
+ if (lift.x + lift.width > lmaxX) {
+ lmaxX = lift.x + lift.width;
+ }
+ if (lift.y + lift.height> lmaxY) {
+ lmaxY = lift.y + lift.height;
+ }
+ }
+
+
+
+// // this is a second method of looping through the contours in the filterContours0Output Array of Mat Images
+// for (MatOfPoint contour : pipeline.filterContours0Output()) {
+// Rect r = Imgproc.boundingRect(contour);
+// if (r.x < minX) {
+// minX = r.x;
+// }
+// }
+
+
+
+ synchronized (imgLockLift) {
+ liftCurrentCenter = lminX + ((lmaxX - lminX) / 2);
+
+ // set values for the smartdashboard
+ lrectX = lminX;
+ lrectY = lminY;
+ lrectWidth = lmaxX - lminX;
+ lrectHeight = lmaxY - lminY;
+
+ // note - this formula was pulled from 1640's github code - need to find the specific reference
+ // from 1640
+ //Equation to determine the distance from a target (d) given the width in pixels of a vision target in the camera image (w):
+ // d = (TARGET_WIDTH*CAMERA_IMAGE_WIDTH)/(2*tan(FOV_ANGLE/2)*w)
+ // i.e. d and w are inversely related.
+ liftCurrentDistance = liftTargetWidth * liftImageWidth / (2*(Math.tan(Math.toRadians(fovDegrees))/2)*lrectWidth);
+
+ // display the current image on the driver station
+ if (Constants.LIFT_CAMERA_OUTPUT_DEBUG){
+ // outputStreamLift.putFrame(lgPipeline.resizeImageOutput());
+ outputStreamLift.putFrame(lgPipeline.rgbThresholdOutput());
+ }
+ }
+ }
+
+ // the target is not in the camera (ie, pipeline is empty)
+ else {
+ liftHasTarget = false;
+ lminX = 0;
+ lminY = 0;
+ lmaxX = 0;
+ lmaxY = 0;
+ }
+
+ });
+
+ visionThreadLift.start();
+ }
+
+ /*
+ * liftProcessData
+ * Method to interpret the camera data received above
+ */
+ /*
+ public void liftProcessData() {
+ try {
+
+ // use the sonar to get the distance from the target (backup plan if camera distance not available)
+ // currentDistance=Robot.drivetrain.getSonarDistance();
+
+ // if robot sees the target (current X between its min and max)
+ if ((liftCurrentCenter > minThresholdX) && (liftCurrentCenter < maxThresholdX)) {
+ liftHasTarget = true;
+ }
+ else {
+ liftHasTarget = false;
+ }
+
+ } catch (TableKeyNotDefinedException e) {
+ e.printStackTrace();
+ }
+ }
+ */
+ /*
+ * isCentered
+ * Method to determine whether the robot sees the center of the target (within the threshold value)
+ */
+ public boolean liftIsCentered() {
+
+ // if the robot sees the target
+ // determine whether the horizontal value the robot sees is within the threshold defining the center of the target
+ // set isCentered according to whether the robot is aligned with the center of the target
+ if (liftHasTarget)
+ {
+
+ double difference = trueCenter - (liftCurrentCenter);
+ if (Math.abs(difference) <= thresholdX) {
+ SmartDashboard.putBoolean("Lift is centered", true);
+ liftIsCentered = true;
+ }
+ else if (Math.abs(difference) > thresholdX) {
+ liftIsCentered = false;
+ }
+ liftRotateDiff = difference;
+ }
+ else{
+ liftIsCentered = false;
+ }
+
+ return liftIsCentered;
+ }
+
+ /*
+ * getRotate
+ * Method to determine whether the robot is at the center of the target so it can drive towards target
+ */
+ public double liftGetRotate() {
+ double difference=0;
+
+ // currently we are only running 1 cycle of the sweep and stopping
+ // if in the future additional sweeps are required, this is where the reset should occur
+ // if (sweepCounter > 400){
+ // sweepCounter = 0;
+ // }
+
+ // if robot sees target and is centered - no need to rotate the robot
+ if (liftHasTarget && liftIsCentered)
+ {
+ rotate = 0.0;
+ }
+
+ // if the robot sees the target but is not centered
+ // check to see whether the robot is within the threshold
+ // rotate based on the math function
+ else if (liftHasTarget && !liftIsCentered){
+ difference = trueCenter - (liftCurrentCenter);
+
+ rotate = Math708.getClippedPercentError(liftCurrentCenter, trueCenter, AutoConstants.DRIVE_ROTATE_MIN, AutoConstants.DRIVE_ROTATE_MAX);
+// rotate = .6;
+
+ if (Math.abs(difference) > thresholdX) {
+ if (liftCurrentCenter < trueCenter){
+ rotate = Math.abs(rotate);
+ }
+ else {
+ rotate = Math.abs(rotate) * -1;
+ }
+ }
+ }
+
+ // if the robot does not have the target
+ // begin the sweep
+ // sweep is defined as rotating the robot right, left, right in predefined counts
+ // if in the sweep the robot does not find the target, it stops after 3 sweeps
+ // otherwise it will jump back into the hasTarget logic identified above
+ else if (!liftHasTarget){
+ if (Math.abs(liftSweepDirection) < .1){
+ liftSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+ rotate = AutoConstants.SWEEP_ROTATE;
+ }
+ else if (liftSweepDirection > AutoConstants.SWEEP1_MIN){
+ if ((liftSweepCounter >= AutoConstants.SWEEP1_MIN && liftSweepCounter <= AutoConstants.SWEEP1_MAX)
+ || (liftSweepCounter >= AutoConstants.SWEEP3_MIN && liftSweepCounter <= AutoConstants.SWEEP3_MAX)){
+
+ rotate = AutoConstants.SWEEP_ROTATE;
+ if (liftSweepCounter== AutoConstants.SWEEP1_MAX || liftSweepCounter== AutoConstants.SWEEP3_MAX){
+ liftSweepDirection = AutoConstants.SWEEP_DIRECTION_LEFT;
+ }
+ }
+ }
+ else {
+ if (liftSweepCounter >= AutoConstants.SWEEP2_MIN && liftSweepCounter <= AutoConstants.SWEEP2_MAX)
+ rotate = -AutoConstants.SWEEP_ROTATE;
+ if (liftSweepCounter== AutoConstants.SWEEP2_MAX){
+ liftSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+ }
+ }
+
+ liftSweepCounter++;
+ }
+ liftRotateDiff = difference;
+ return rotate;
+ }
+
+ /*
+ * getMove
+ * Method to determine if the robot is close enough to target so it can stop
+ */
+
+ public double liftGetMove() {
+
+ // if the robot sees the target
+ // Method to determine whether the robot is at the correct distance to the target so stop
+ if (liftHasTarget)
+ {
+// double difference = liftDistanceToStop - liftCurrentDistance;
+ double difference = liftCurrentDistance - liftDistanceToStop;
+
+ liftMoveDiff = difference;
+
+ //Check if target is at correct distance within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+ if (difference <= thresholdDistance) {
+ move = 0.0;
+ liftIsAtDistance = true;
+ } else {
+ move = Math708.getClippedPercentError(liftCurrentDistance, liftDistanceToStop, AutoConstants.DRIVE_MOVE_MIN, AutoConstants.DRIVE_MOVE_MAX);
+// move = .6;
+ liftIsAtDistance = false;
+ }
+
+ } else {
+ move = 0.0;
+ }
+
+ return move;
+ }
+
+
+
+ /**
+ * GETTERS and PUTTERS to return the private variables
+ * @return
+ */
+
+
+ public boolean liftIsAtDistance() {
+// double difference = liftDistanceToStop - liftCurrentDistance;
+ double difference = liftCurrentDistance - liftDistanceToStop;
+ //Check if target is at correct level within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+ if (difference <= thresholdDistance) {
+ liftIsAtDistance = true;
+ SmartDashboard.putBoolean("Lift is at distance", true);
+
+ } else {
+ liftIsAtDistance = false;
+ }
+ return liftIsAtDistance;
+ }
+
+ public boolean liftIsHasTarget() {
+ return liftHasTarget;
+ }
+
+ public void putLiftHasTarget(boolean lht) {
+ liftHasTarget = lht;
+ }
+
+ public void putLiftCurrentCenter(double lcc) {
+ liftCurrentCenter = lcc;
+ }
+
+
+ public int getLiftCounter() {
+ return liftSweepCounter;
+ }
+
+ public void putLiftCounter(int lct) {
+ liftSweepCounter = lct;
+ }
+
+ public void putLiftIsCentered(boolean lic) {
+ liftIsCentered = lic;
+ }
+
+ public void putLiftAtDistance(boolean lad) {
+ liftIsAtDistance = lad;
+ }
+
+ public boolean liftIsInSweep() {
+ if (liftHasTarget) {
+ liftInSweep = false;
+ liftSweepCounter=1;
+ }
+ else {
+ liftInSweep = true;
+ }
+ return liftInSweep;
+ }
+
+ public void setLiftCamera() {
+ usbCameraLiftGear.setBrightness(40); //40 for lift
+ usbCameraLiftGear.setExposureManual(0); //25 for lift
+ usbCameraLiftGear.setWhiteBalanceManual(10000); //10000
+ usbCameraLiftGear.setFPS(20);
+ }
+
+ public void sendToDashboard() {
+ if (Constants.LIFT_DEBUG) {
+
+// SmartDashboard.putBoolean("L-Has Target", liftIsHasTarget());
+// SmartDashboard.putBoolean("L-Is At Distance", liftIsAtDistance());
+ SmartDashboard.putNumber("L-Center of Target", liftCurrentCenter);
+// SmartDashboard.putNumber("L-Rotation", rotate);
+// SmartDashboard.putNumber("L-Rotate Difference", liftRotateDiff);
+// SmartDashboard.putNumber("L-Distance Difference", liftMoveDiff);
+// SmartDashboard.putNumber("L-Sweep Counter", liftSweepCounter);
+// SmartDashboard.putNumber("L-SweepDirection", liftSweepDirection);
+// SmartDashboard.putBoolean("L-isCentered", liftIsCentered());
+// SmartDashboard.putNumber("L-rectX", lrectX);
+// SmartDashboard.putNumber("L-rectY", lrectY);
+// SmartDashboard.putNumber("L-rectWidth", lrectWidth);
+// SmartDashboard.putNumber("L-rectHeight", lrectHeight);
+ SmartDashboard.putNumber("L-Distance To Target", liftCurrentDistance);
+// SmartDashboard.putNumber("L-pipelineSize", liftPipelineSize);
+ }
+ }
+
+ public void initDefaultCommand() {
+ if (Constants.DEBUG) {
+ }
+ }
+
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/VisionLiftGear.java b/src/org/usfirst/frc/team708/robot/subsystems/VisionLiftGear.java
new file mode 100644
index 0000000..fca249f
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/VisionLiftGear.java
@@ -0,0 +1,760 @@
+//package org.usfirst.frc.team708.robot.subsystems;
+//
+//import org.opencv.core.Rect;
+//import org.opencv.imgproc.Imgproc;
+//import org.usfirst.frc.team708.robot.AutoConstants;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.commands.visionProcessor.GripPipelineLiftGear;
+//import org.usfirst.frc.team708.robot.util.Math708;
+//
+//import edu.wpi.cscore.AxisCamera;
+//import edu.wpi.cscore.CvSource;
+//import edu.wpi.cscore.UsbCamera;
+//import edu.wpi.first.wpilibj.CameraServer;
+//import edu.wpi.first.wpilibj.command.Subsystem;
+//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+//import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
+//import edu.wpi.first.wpilibj.vision.VisionThread;
+//
+///**
+// *@authors Viet & Sue
+// *This subsystem is specific to the 2017 Game FIRST Steamworks Lift Peg
+// */
+//
+//public class VisionLiftGear extends Subsystem {
+//
+// // Camera Variables
+// private double fovDegrees = AutoConstants.USB_LIFT_FOV_DEGREES; // Field of View of the Camera
+// private double liftPipelineSize; // Number of Contours in the Pipline- 0 = target not in view
+// private double gearPipelineSize;
+// private int liftGearImageWidth = AutoConstants.USB_LIFT_IMG_WIDTH; // Width of image from camera stream
+// private int liftGearImageHeight = AutoConstants.USB_LIFT_IMG_HEIGHT; // Height of image from camera stream
+//
+// // Image OpenCV Image Processing Variables
+// private VisionThread visionThreadLiftGear; // vision processing thread - processes grip code for lift
+// private VisionThread visionThreadGear; // vision processing thread - processes grip code for gear
+// private final Object imgLockLift = new Object(); // vision Lift object
+// private final Object imgLockGear = new Object(); // vision Lift object
+//
+//// private AxisCamera axisCamera; // Axis Camera
+// private UsbCamera usbCameraLiftGear; // USB Camera
+// private CvSource outputStreamLift; // Output stream to the Dashboard
+// private CvSource outputStreamGear; // Output stream to the Dashboard
+//
+//
+// // Targeting variables
+// private int lrectX = 0; // the 4 values used which define the full rectangle around the target
+// private int lrectY = 0;
+// private int grectX = 0;
+// private int grectY = 0;
+// private int lrectWidth = 0;
+// private int lrectHeight = 0;
+// private int grectWidth = 0;
+// private int grectHeight = 0;
+//
+// private int lminX = 0; // the 4 values used to create maximum rectangle around the target (used when evaluating each of the contour images)
+// private int lminY = 0;
+// private int lmaxX = 0;
+// private int lmaxY = 0;
+// private int gminX = 0; // the 4 values used to create maximum rectangle around the target (used when evaluating each of the contour images)
+// private int gminY = 0;
+// private int gmaxX = 0;
+// private int gmaxY = 0;
+//
+// private boolean liftHasTarget = false; // flag indicating whether the robot sees the target
+// private boolean liftIsCentered = false; // flag indicating whether the robot sees the center of the target
+// private boolean liftIsAtDistance = false; // flag indicating whether the robot is at the correct distance from the target
+// private boolean gearHasTarget = false;
+// private boolean gearIsCentered = false;
+// private boolean gearIsAtDistance = false;
+//
+// private int liftTargetHeight = AutoConstants.LIFT_TARGET_HEIGHT; //height of actual target reflective tape
+// private int liftTargetWidth = AutoConstants.LIFT_TARGET_WIDTH; //width of actual target reflective tape
+// private int gearTargetHeight = AutoConstants.GEAR_TARGET_HEIGHT; //height of actual target reflective tape
+// private int gearTargetWidth = AutoConstants.GEAR_TARGET_WIDTH; //width of actual target reflective tape
+//
+// private double trueCenter = liftGearImageWidth/2; // horizontal value of the center of the camera image
+// private double liftDistanceToStop = AutoConstants.DISTANCE_TO_LIFT_TARGET; // distance to stop at in front of lift target
+// private double liftCurrentCenter = 0.0; // horizontal value of where robot is looking
+// private double liftCurrentDistance = 0.0; // distance robot is from the target
+// private double gearDistanceToStop = AutoConstants.DISTANCE_TO_GEAR; // distance to stop at in front of gear
+// private double gearCurrentCenter = 0.0; // horizontal value of where robot is looking
+// private double gearCurrentDistance = 0.0; // distance robot is from the target
+//
+// private double thresholdX = AutoConstants.X_THRESHOLD_CENTER; // threshold for determining center of the target
+// private double thresholdDistance = AutoConstants.DISTANCE_TARGET_THRESHOLD; // threshold for determining threshold for stopping at the lift peg
+// private double minThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MIN; // threshold for determining min value for whether the robot sees the target
+// private double maxThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MAX; // threshold for determining max value for whether the robot sees the target
+//
+//
+// // Sweep Variables
+// private boolean liftInSweep = false; // flag indicating whether the robot is sweeping left and right looking for the target
+// private double liftSweepDirection = 0.0; // value indicating the direction of the sweep -1 = right; 1 = left
+// private int liftSweepCounter = 0; // value indicating when the sweep will change direction
+// private boolean gearInSweep = false; // flag indicating whether the robot is sweeping left and right looking for the target
+// private double gearSweepDirection = 0.0; // value indicating the direction of the sweep -1 = right; 1 = left
+// private int gearSweepCounter = 0; // value indicating when the sweep will change direction
+//
+//
+// // drive variables
+// private double liftRotateDiff = 0; // for smartdashboard - how far away from center
+// private double liftMoveDiff = 0; // for smartdashboard - how far away from target
+// private double gearRotateDiff = 0; // for smartdashboard - how far away from center
+// private double gearMoveDiff = 0; // for smartdashboard - how far away from target
+// double rotate; // speed of the rotate being returned to the command
+// double move; // speed of the move forward being returned to the command
+//
+// // Vision Processing
+// public VisionLiftGear() {
+// super("Vision Processor");
+//
+//
+// // define the Cameras:
+// // on little bot - cam2, 1
+// // Pipeline outputs: hsl and 0 means lift, rgb and 1 means gear
+// usbCameraLiftGear=CameraServer.getInstance().startAutomaticCapture(AutoConstants.USB_CAMERA_ID, AutoConstants.USB_VIDEO_PORT);
+//// axisCamera=CameraServer.getInstance().addAxisCamera("cam1", "10.7.8.11");
+// usbCameraLiftGear.setResolution(liftGearImageWidth, liftGearImageHeight);
+//
+//// usbCameraLiftGear.setBrightness(40); //40 for lift 70 for gear
+//// usbCameraLiftGear.setExposureManual(25); //25 for lift 48 for gear
+//// usbCameraLiftGear.setWhiteBalanceManual(10000); //10000 for lift 2800 for gear
+//// usbCameraLiftGear.setFPS(20);
+//
+//
+//
+// // define the output stream on the smart dashboard
+// outputStreamLift = CameraServer.getInstance().putVideo("Target_Lift", liftGearImageWidth, liftGearImageHeight);
+// outputStreamGear = CameraServer.getInstance().putVideo("Target_Gear", liftGearImageWidth, liftGearImageHeight);
+//
+// // Vision thread which processes the image contours
+// visionThreadLiftGear = new VisionThread(usbCameraLiftGear, new GripPipelineLiftGear(), lgPipeline -> {
+// liftPipelineSize = lgPipeline.findContours0Output().size();
+// gearPipelineSize = lgPipeline.filterContoursOutput().size();
+//
+// // if the grip pipeline filter "filterContours0Output" sees the target
+// // loop through each contour image
+// // grab the bounding rectangle values of each contour
+// // to create the biggest rectangle around the 2 vertical retroreflective tapes
+// // on either side of the lift peg
+// if (!lgPipeline.findContours0Output().isEmpty()) {
+//
+// for (int i = 0; i < lgPipeline.findContours0Output().size(); i++) {
+// Rect lift = Imgproc.boundingRect(lgPipeline.findContours0Output().get(i));
+//
+// // set the min/max values to match the values form the 1st image
+// if (i == 0) {
+// lminX = lift.x;
+// lminY = lift.y;
+// lmaxX = lift.x + lift.width;
+// lmaxY = lift.y + lift.height;
+// }
+//
+// // compare each value to the min/max and replace if a better one is found
+// if (lift.x < lminX) {
+// lminX = lift.x;
+// }
+// if (lift.y < lminY) {
+// lminY = lift.y;
+// }
+// if (lift.x + lift.width > lmaxX) {
+// lmaxX = lift.x + lift.width;
+// }
+// if (lift.y + lift.height> lmaxY) {
+// lmaxY = lift.y + lift.height;
+// }
+// }
+//
+//
+//
+//// // this is a second method of looping through the contours in the filterContours0Output Array of Mat Images
+//// for (MatOfPoint contour : pipeline.filterContours0Output()) {
+//// Rect r = Imgproc.boundingRect(contour);
+//// if (r.x < minX) {
+//// minX = r.x;
+//// }
+//// }
+//
+//
+//
+// synchronized (imgLockLift) {
+// liftCurrentCenter = lminX + ((lmaxX - lminX) / 2);
+//
+// // set values for the smartdashboard
+// lrectX = lminX;
+// lrectY = lminY;
+// lrectWidth = lmaxX - lminX;
+// lrectHeight = lmaxY - lminY;
+//
+// // note - this formula was pulled from 1640's github code - need to find the specific reference
+// // from 1640
+// //Equation to determine the distance from a target (d) given the width in pixels of a vision target in the camera image (w):
+// // d = (TARGET_WIDTH*CAMERA_IMAGE_WIDTH)/(2*tan(FOV_ANGLE/2)*w)
+// // i.e. d and w are inversely related.
+// liftCurrentDistance = liftTargetWidth * liftGearImageWidth / (2*(Math.tan(Math.toRadians(fovDegrees))/2)*lrectWidth);
+//
+// // display the current image on the driver station
+//
+//
+//// outputStreamLift.putFrame(lgPipeline.hslThresholdOutput());
+// outputStreamLift.putFrame(lgPipeline.resizeImageOutput());
+// }
+// }
+//
+// // the target is not in the camera (ie, pipeline is empty)
+// else {
+// liftHasTarget = false;
+// lminX = 0;
+// lminY = 0;
+// lmaxX = 0;
+// lmaxY = 0;
+// }
+//
+// if (!lgPipeline.filterContoursOutput().isEmpty()) {
+//
+// for (int i = 0; i < lgPipeline.filterContoursOutput().size(); i++) {
+// Rect gear = Imgproc.boundingRect(lgPipeline.filterContoursOutput().get(i));
+//
+// // set the min/max values to match the values form the 1st image
+// if (i == 0) {
+// gminX = gear.x;
+// gminY = gear.y;
+// gmaxX = gear.x + gear.width;
+// gmaxY = gear.y + gear.height;
+// }
+//
+// // compare each value to the min/max and replace if a better one is found
+// if (gear.x < lminX) {
+// gminX = gear.x;
+// }
+// if (gear.y < gminY) {
+// gminY = gear.y;
+// }
+// if (gear.x + gear.width > gmaxX) {
+// gmaxX = gear.x + gear.width;
+// }
+// if (gear.y + gear.height> gmaxY) {
+// gmaxY = gear.y + gear.height;
+// }
+// }
+//
+//// // this is a second method of looping through the contours in the filterContours0Output Array of Mat Images
+//// for (MatOfPoint contour : pipeline.filterContours1Output()) {
+//// Rect r = Imgproc.boundingRect(contour);
+//// if (r.x < minX) {
+//// minX = r.x;
+//// }
+//// }
+//
+//
+//
+// synchronized (imgLockGear) {
+// gearCurrentCenter = gminX + ((gmaxX - gminX) / 2);
+//
+// // set values for the smartdashboard
+// grectX = gminX;
+// grectY = gminY;
+// grectWidth = gmaxX - gminX;
+// grectHeight = gmaxY - gminY;
+//
+// // note - this formula was pulled from 1640's github code - need to find the specific reference
+// // from 1640
+// //Equation to determine the distance from a target (d) given the width in pixels of a vision target in the camera image (w):
+// // d = (TARGET_WIDTH*CAMERA_IMAGE_WIDTH)/(2*tan(FOV_ANGLE/2)*w)
+// // i.e. d and w are inversely related. 10 * 320 /
+// gearCurrentDistance = gearTargetWidth * liftGearImageWidth / (2*(Math.tan(Math.toRadians(fovDegrees))/2)*grectWidth);
+//
+// // display the current image on the driver station
+//
+//
+//// outputStreamGear.putFrame(lgPipeline.rgbThresholdOutput());
+//// outputStreamLift.putFrame(lgPipeline.resizeImageOutput());
+// }
+// }
+//
+// // the target is not in the camera (ie, pipeline is empty)
+// else {
+// gearHasTarget = false;
+// gminX = 0;
+// gminY = 0;
+// gmaxX = 0;
+// gmaxY = 0;
+// }
+//
+// });
+// visionThreadLiftGear.start();
+// }
+//
+// /*
+// * liftProcessData
+// * Method to interpret the camera data received above
+// */
+// public void liftProcessData() {
+// try {
+//
+// // use the sonar to get the distance from the target (backup plan if camera distance not available)
+// // currentDistance=Robot.drivetrain.getSonarDistance();
+//
+// // if robot sees the target (current X between its min and max)
+// if ((liftCurrentCenter > minThresholdX) && (liftCurrentCenter < maxThresholdX)) {
+// liftHasTarget = true;
+// }
+// else {
+// liftHasTarget = false;
+// }
+//
+// } catch (TableKeyNotDefinedException e) {
+// e.printStackTrace();
+// }
+// }
+//
+// /*
+// * isCentered
+// * Method to determine whether the robot sees the center of the target (within the threshold value)
+// */
+// public boolean liftIsCentered() {
+//
+// // if the robot sees the target
+// // determine whether the horizontal value the robot sees is within the threshold defining the center of the target
+// // set isCentered according to whether the robot is aligned with the center of the target
+// if (liftHasTarget)
+// {
+//
+// double difference = trueCenter - (liftCurrentCenter);
+// if (Math.abs(difference) <= thresholdX) {
+// liftIsCentered = true;
+// }
+// else if (Math.abs(difference) > thresholdX) {
+// liftIsCentered = false;
+// }
+// liftRotateDiff = difference;
+// }
+// else{
+// liftIsCentered = false;
+// }
+//
+// return liftIsCentered;
+// }
+//
+// /*
+// * getRotate
+// * Method to determine whether the robot is at the center of the target so it can drive towards target
+// */
+// public double liftGetRotate() {
+// double difference=0;
+//
+// // currently we are only running 1 cycle of the sweep and stopping
+// // if in the future additional sweeps are required, this is where the reset should occur
+// // if (sweepCounter > 400){
+// // sweepCounter = 0;
+// // }
+//
+// // if robot sees target and is centered - no need to rotate the robot
+// if (liftHasTarget && liftIsCentered)
+// {
+// rotate = 0.0;
+// }
+//
+// // if the robot sees the target but is not centered
+// // check to see whether the robot is within the threshold
+// // rotate based on the math function
+// else if (liftHasTarget && !liftIsCentered){
+// difference = trueCenter - (liftCurrentCenter);
+//
+// rotate = Math708.getClippedPercentError(liftCurrentCenter, trueCenter, AutoConstants.DRIVE_ROTATE_MIN, AutoConstants.DRIVE_ROTATE_MAX);
+//
+//
+// if (Math.abs(difference) > thresholdX) {
+// if (liftCurrentCenter < trueCenter){
+// rotate = Math.abs(rotate);
+// }
+// else {
+// rotate = Math.abs(rotate) * -1;
+// }
+// }
+// }
+//
+// // if the robot does not have the target
+// // begin the sweep
+// // sweep is defined as rotating the robot right, left, right in predefined counts
+// // if in the sweep the robot does not find the target, it stops after 3 sweeps
+// // otherwise it will jump back into the hasTarget logic identified above
+// else if (!liftHasTarget){
+// if (Math.abs(liftSweepDirection) < .1){
+// liftSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// }
+// else if (liftSweepDirection > AutoConstants.SWEEP1_MIN){
+// if ((liftSweepCounter >= AutoConstants.SWEEP1_MIN && liftSweepCounter <= AutoConstants.SWEEP1_MAX)
+// || (liftSweepCounter >= AutoConstants.SWEEP3_MIN && liftSweepCounter <= AutoConstants.SWEEP3_MAX)){
+//
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// if (liftSweepCounter== AutoConstants.SWEEP1_MAX || liftSweepCounter== AutoConstants.SWEEP3_MAX){
+// liftSweepDirection = AutoConstants.SWEEP_DIRECTION_LEFT;
+// }
+// }
+// }
+// else {
+// if (liftSweepCounter >= AutoConstants.SWEEP2_MIN && liftSweepCounter <= AutoConstants.SWEEP2_MAX)
+// rotate = AutoConstants.SWEEP_ROTATE;
+// if (liftSweepCounter== AutoConstants.SWEEP2_MAX){
+// liftSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// }
+// }
+//
+// liftSweepCounter++;
+// }
+// liftRotateDiff = difference;
+// return rotate;
+// }
+//
+// /*
+// * getMove
+// * Method to determine if the robot is close enough to target so it can stop
+// */
+//
+// public double liftGetMove() {
+//
+// // if the robot sees the target
+// // Method to determine whether the robot is at the correct distance to the target so stop
+// if (liftHasTarget)
+// {
+// double difference = liftDistanceToStop - liftCurrentDistance;
+// move = Math708.getClippedPercentError(liftCurrentDistance, liftDistanceToStop, AutoConstants.DRIVE_MOVE_MIN, AutoConstants.DRIVE_MOVE_MAX);
+//
+// //Check if target is at correct distance within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// move = 0.0;
+// liftIsAtDistance = true;
+// } else {
+// liftIsAtDistance = false;
+// }
+// liftMoveDiff = difference;
+// } else {
+// move = 0.0;
+// }
+//
+// return move;
+// }
+//
+// /*
+// * liftProcessData
+// * Method to interpret the camera data received above
+// */
+// public void gearProcessData() {
+// try {
+//
+// // use the sonar to get the distance from the target (backup plan if camera distance not available)
+// // currentDistance=Robot.drivetrain.getSonarDistance();
+//
+// // if robot sees the target (current X between its min and max)
+// if ((gearCurrentCenter > minThresholdX) && (gearCurrentCenter < maxThresholdX)) {
+// gearHasTarget = true;
+// }
+// else {
+// gearHasTarget = false;
+// }
+//
+// } catch (TableKeyNotDefinedException e) {
+// e.printStackTrace();
+// }
+// }
+//
+// /*
+// * isCentered
+// * Method to determine whether the robot sees the center of the target (within the threshold value)
+// */
+// public boolean gearIsCentered() {
+//
+// // if the robot sees the target
+// // determine whether the horizontal value the robot sees is within the threshold defining the center of the target
+// // set isCentered according to whether the robot is aligned with the center of the target
+// if (gearHasTarget)
+// {
+//
+// double difference = trueCenter - (gearCurrentCenter);
+// if (Math.abs(difference) <= thresholdX) {
+// gearIsCentered = true;
+// }
+// else if (Math.abs(difference) > thresholdX) {
+// gearIsCentered = false;
+// }
+// gearRotateDiff = difference;
+// }
+// else{
+// gearIsCentered = false;
+// }
+//
+// return gearIsCentered;
+// }
+//
+// /*
+// * getRotate
+// * Method to determine whether the robot is at the center of the target so it can drive towards target
+// */
+// public double gearGetRotate() {
+// double difference=0;
+//
+// // currently we are only running 1 cycle of the sweep and stopping
+// // if in the future additional sweeps are required, this is where the reset should occur
+// // if (sweepCounter > 400){
+// // sweepCounter = 0;
+// // }
+//
+// // if robot sees target and is centered - no need to rotate the robot
+// if (gearHasTarget && gearIsCentered)
+// {
+// rotate = 0.0;
+// }
+//
+// // if the robot sees the target but is not centered
+// // check to see whether the robot is within the threshold
+// // rotate based on the math function
+// else if (gearHasTarget && !gearIsCentered){
+// difference = trueCenter - (gearCurrentCenter);
+//
+// rotate = Math708.getClippedPercentError(gearCurrentCenter, trueCenter, AutoConstants.DRIVE_ROTATE_MIN, AutoConstants.DRIVE_ROTATE_MAX);
+//
+//
+// if (Math.abs(difference) > thresholdX) {
+// if (gearCurrentCenter < trueCenter){
+// rotate = Math.abs(rotate);
+// }
+// else {
+// rotate = Math.abs(rotate) * -1;
+// }
+// }
+// }
+//
+// // if the robot does not have the target
+// // begin the sweep
+// // sweep is defined as rotating the robot right, left, right in predefined counts
+// // if in the sweep the robot does not find the target, it stops after 3 sweeps
+// // otherwise it will jump back into the hasTarget logic identified above
+// else if (!gearHasTarget){
+// if (Math.abs(gearSweepDirection) < .1){
+// gearSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// }
+// else if (gearSweepDirection > AutoConstants.SWEEP1_MIN){
+// if ((gearSweepCounter >= AutoConstants.SWEEP1_MIN && liftSweepCounter <= AutoConstants.SWEEP1_MAX)
+// || (gearSweepCounter >= AutoConstants.SWEEP3_MIN && liftSweepCounter <= AutoConstants.SWEEP3_MAX)){
+//
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// if (gearSweepCounter== AutoConstants.SWEEP1_MAX || liftSweepCounter== AutoConstants.SWEEP3_MAX){
+// gearSweepDirection = AutoConstants.SWEEP_DIRECTION_LEFT;
+// }
+// }
+// }
+// else {
+// if (gearSweepCounter >= AutoConstants.SWEEP2_MIN && liftSweepCounter <= AutoConstants.SWEEP2_MAX)
+// rotate = AutoConstants.SWEEP_ROTATE;
+// if (gearSweepCounter== AutoConstants.SWEEP2_MAX){
+// gearSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// }
+// }
+//
+// gearSweepCounter++;
+// }
+// gearRotateDiff = difference;
+// return rotate;
+// }
+//
+// /*
+// * getMove
+// * Method to determine if the robot is close enough to target so it can stop
+// */
+//
+// public double gearGetMove() {
+//
+// // if the robot sees the target
+// // Method to determine whether the robot is at the correct distance to the target so stop
+// if (gearHasTarget)
+// {
+// double difference = gearDistanceToStop - gearCurrentDistance;
+// move = Math708.getClippedPercentError(gearCurrentDistance, gearDistanceToStop, AutoConstants.DRIVE_MOVE_MIN, AutoConstants.DRIVE_MOVE_MAX);
+//
+// //Check if target is at correct distance within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// move = 0.0;
+// gearIsAtDistance = true;
+// } else {
+// gearIsAtDistance = false;
+// }
+// gearMoveDiff = difference;
+// } else {
+// move = 0.0;
+// }
+//
+// return move;
+// }
+//
+// /**
+// * GETTERS and PUTTERS to return the private variables
+// * @return
+// */
+//
+//
+// public boolean liftIsAtDistance() {
+// double difference = liftDistanceToStop - liftCurrentDistance;
+// //Check if target is at correct level within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// liftIsAtDistance = true;
+// } else {
+// liftIsAtDistance = false;
+// }
+// return liftIsAtDistance;
+// }
+//
+// public boolean gearIsAtDistance() {
+// double difference = gearDistanceToStop - gearCurrentDistance;
+// //Check if target is at correct level within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// gearIsAtDistance = true;
+// } else {
+// gearIsAtDistance = false;
+// }
+// return gearIsAtDistance;
+// }
+//
+// public boolean liftIsHasTarget() {
+// return liftHasTarget;
+// }
+//
+// public boolean gearIsHasTarget() {
+// return gearHasTarget;
+// }
+//
+// public void putLiftHasTarget(boolean lht) {
+// liftHasTarget = lht;
+// }
+//
+// public void putGearHasTarget(boolean ght) {
+// gearHasTarget = ght;
+// }
+//
+// public void putLiftCurrentCenter(double lcc) {
+// liftCurrentCenter = lcc;
+// }
+//
+// public void putGearCurrentCenter(double gcc) {
+// gearCurrentCenter = gcc;
+// }
+//
+// public int getLiftCounter() {
+// return liftSweepCounter;
+// }
+//
+// public int getGearCounter() {
+// return gearSweepCounter;
+// }
+//
+// public void putLiftCounter(int lct) {
+// liftSweepCounter = lct;
+// }
+//
+// public void putGearCounter(int gct) {
+// gearSweepCounter = gct;
+// }
+//
+// public void putLiftIsCentered(boolean lic) {
+// liftIsCentered = lic;
+// }
+//
+// public void putGearIsCentered(boolean gic) {
+// gearIsCentered = gic;
+// }
+//
+// public void putLiftAtDistance(boolean lad) {
+// liftIsAtDistance = lad;
+// }
+//
+// public void putGearAtDistance(boolean gad) {
+// gearIsAtDistance = gad;
+// }
+//
+// public boolean liftIsInSweep() {
+// if (liftHasTarget) {
+// liftInSweep = false;
+// liftSweepCounter=1;
+// }
+// else {
+// liftInSweep = true;
+// }
+// return liftInSweep;
+// }
+//
+// public boolean gearIsInSweep() {
+// if (gearHasTarget) {
+// gearInSweep = false;
+// gearSweepCounter=1;
+// }
+// else {
+// gearInSweep = true;
+// }
+// return gearInSweep;
+// }
+//
+//
+// public void setLiftCamera() {
+// usbCameraLiftGear.setBrightness(40); //40 for lift 70 for gear
+// usbCameraLiftGear.setExposureManual(25); //25 for lift 48 for gear
+// usbCameraLiftGear.setWhiteBalanceManual(10000); //10000 for lift 2800 for gear
+// usbCameraLiftGear.setFPS(20);
+// }
+//
+// public void setGearCamera() {
+// usbCameraLiftGear.setBrightness(70); //40 for lift 70 for gear
+// usbCameraLiftGear.setExposureManual(48); //25 for lift 48 for gear
+// usbCameraLiftGear.setWhiteBalanceManual(2800); //10000 for lift 2800 for gear
+// usbCameraLiftGear.setFPS(20);
+// }
+//
+// public void sendToDashboard() {
+// if (Constants.LIFT_DEBUG) {
+//
+// SmartDashboard.putBoolean("L-Has Target", liftIsHasTarget());
+// SmartDashboard.putBoolean("L-Is At Distance", liftIsAtDistance());
+// SmartDashboard.putNumber("L-Center of Target", liftCurrentCenter);
+// SmartDashboard.putNumber("L-Rotation", rotate);
+// SmartDashboard.putNumber("L-Rotate Difference", liftRotateDiff);
+// SmartDashboard.putNumber("L-Distance Difference", liftMoveDiff);
+// SmartDashboard.putNumber("L-Sweep Counter", liftSweepCounter);
+// SmartDashboard.putNumber("L-SweepDirection", liftSweepDirection);
+// SmartDashboard.putBoolean("L-isCentered", liftIsCentered());
+// SmartDashboard.putNumber("L-rectX", lrectX);
+// SmartDashboard.putNumber("L-rectY", lrectY);
+// SmartDashboard.putNumber("L-rectWidth", lrectWidth);
+// SmartDashboard.putNumber("L-rectHeight", lrectHeight);
+// SmartDashboard.putNumber("L-Distance To Target", liftCurrentDistance);
+// SmartDashboard.putNumber("L-pipelineSize", liftPipelineSize);
+// }
+// if (Constants.GEAR_DEBUG) {
+//
+// SmartDashboard.putBoolean("G-Has Target", gearIsHasTarget());
+// SmartDashboard.putBoolean("G-Is At Distance", gearIsAtDistance());
+// SmartDashboard.putNumber("G-Center of Target", gearCurrentCenter);
+// SmartDashboard.putNumber("G-Rotation", rotate);
+// SmartDashboard.putNumber("G-Move", move);
+// SmartDashboard.putNumber("G-Rotate Difference", gearRotateDiff);
+// SmartDashboard.putNumber("G-Distance Difference", gearMoveDiff);
+// SmartDashboard.putNumber("G-Sweep Counter", gearSweepCounter);
+// SmartDashboard.putNumber("G-SweepDirection", gearSweepDirection);
+// SmartDashboard.putBoolean("G-isCentered", gearIsCentered());
+// SmartDashboard.putNumber("G-rectX", grectX);
+// SmartDashboard.putNumber("G-rectY", grectY);
+// SmartDashboard.putNumber("G-rectWidth", grectWidth);
+// SmartDashboard.putNumber("G-rectHeight", grectHeight);
+// SmartDashboard.putNumber("G-Distance To Target", gearCurrentDistance);
+// SmartDashboard.putNumber("G-pipelineSize", gearPipelineSize);
+// }
+// }
+//
+// public void initDefaultCommand() {
+// if (Constants.DEBUG) {
+// }
+// }
+//
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/VisionLift_orig.java b/src/org/usfirst/frc/team708/robot/subsystems/VisionLift_orig.java
new file mode 100644
index 0000000..4e7ca3e
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/subsystems/VisionLift_orig.java
@@ -0,0 +1,425 @@
+//package org.usfirst.frc.team708.robot.subsystems;
+//
+//import org.opencv.core.Rect;
+//import org.opencv.imgproc.Imgproc;
+//import org.usfirst.frc.team708.robot.AutoConstants;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.Robot;
+//import org.usfirst.frc.team708.robot.commands.visionProcessor.GripPipelineLift;
+//import org.usfirst.frc.team708.robot.util.Math708;
+//
+//import edu.wpi.cscore.AxisCamera;
+//import edu.wpi.cscore.CvSource;
+//import edu.wpi.cscore.UsbCamera;
+//import edu.wpi.first.wpilibj.CameraServer;
+//import edu.wpi.first.wpilibj.command.Subsystem;
+//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+//import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
+//import edu.wpi.first.wpilibj.vision.VisionThread;
+//
+///**
+// *@authors Viet & Sue
+// *This subsystem is specific to the 2017 Game FIRST Steamworks Lift Peg
+// */
+//
+//public class VisionLift extends Subsystem {
+//
+// // Camera Variables
+// private double fovDegrees = AutoConstants.USB_LIFT_FOV_DEGREES; // Field of View of the Camera
+// private double liftPipelineSize; // Number of Contours in the Pipline- 0 = target not in view
+// private int liftImageWidth = AutoConstants.USB_LIFT_IMG_WIDTH; // Width of image from camera stream
+// private int liftImageHeight = AutoConstants.USB_LIFT_IMG_HEIGHT; // Height of image from camera stream
+//
+// // Image OpenCV Image Processing Variables
+// private VisionThread visionThreadLift; // vision processing thread - processes grip code
+// private final Object imgLockLift = new Object(); // vision Lift object
+//
+//// private AxisCamera axisCamera; // Axis Camera
+// private UsbCamera usbCameraLift; // USB Camera
+// private CvSource outputStreamLift; // Output stream to the Dashboard
+//
+//
+// // Targeting variables
+// private int lrectX = 0; // the 4 values used which define the full rectangle around the target
+// private int lrectY = 0;
+// private int lrectWidth = 0;
+// private int lrectHeight = 0;
+//
+// private int lminX = 0; // the 4 values used to create maximum rectangle around the target (used when evaluating each of the contour images)
+// private int lminY = 0;
+// private int lmaxX = 0;
+// private int lmaxY = 0;
+//
+// private boolean liftHasTarget = false; // flag indicating whether the robot sees the target
+// private boolean liftIsCentered = false; // flag indicating whether the robot sees the center of the target
+// private boolean liftIsAtDistance = false; // flag indicating whether the robot is at the correct distance from the target
+//
+//
+// private int liftTargetHeight = AutoConstants.LIFT_TARGET_HEIGHT; //height of actual target reflective tape
+// private int liftTargetWidth = AutoConstants.LIFT_TARGET_WIDTH; //width of actual target reflective tape
+//
+// private double trueCenter = liftImageWidth/2; // horizontal value of the center of the camera image
+// private double liftDistanceToStop = AutoConstants.DISTANCE_TO_LIFT_TARGET; // distance to stop at in front of lift target
+// private double liftCurrentCenter = 0.0; // horizontal value of where robot is looking
+// private double liftCurrentDistance = 0.0; // distance robot is from the target
+//
+// private double thresholdX = AutoConstants.X_THRESHOLD_CENTER; // threshold for determining center of the target
+// private double thresholdDistance = AutoConstants.DISTANCE_TARGET_THRESHOLD; // threshold for determining threshold for stopping at the lift peg
+// private double minThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MIN; // threshold for determining min value for whether the robot sees the target
+// private double maxThresholdX = AutoConstants.X_THRESHOLD_HAS_TARGET_MAX; // threshold for determining max value for whether the robot sees the target
+//
+//
+// // Sweep Variables
+// private boolean liftInSweep = false; // flag indicating whether the robot is sweeping left and right looking for the target
+// private double liftSweepDirection = 0.0; // value indicating the direction of the sweep -1 = right; 1 = left
+// private int liftSweepCounter = 0; // value indicating when the sweep will change direction
+//
+//
+// // drive variables
+// private double liftRotateDiff = 0; // for smartdashboard - how far away from center
+// private double liftMoveDiff = 0; // for smartdashboard - how far away from target
+// double rotate; // speed of the rotate being returned to the command
+// double move; // speed of the move forward being returned to the command
+//
+// // Vision Processing
+// public VisionLift() {
+// super("Vision Processor");
+//
+//
+// // define the Cameras:
+// // on little bot - cam2, 1
+// usbCameraLift=CameraServer.getInstance().startAutomaticCapture("cam3", 0);
+//// axisCamera=CameraServer.getInstance().addAxisCamera("cam1", "10.7.8.11");
+// usbCameraLift.setResolution(liftImageWidth, liftImageHeight);
+//
+//
+// // define the output stream on the smart dashboard
+// outputStreamLift = CameraServer.getInstance().putVideo("Target_Lift", liftImageWidth, liftImageHeight);
+//
+// // Vision thread which processes the image contours
+// visionThreadLift = new VisionThread(usbCameraLift, new GripPipelineLift(), lPipeline -> {
+// liftPipelineSize = lPipeline.filterContoursOutput().size();
+//
+// // if the grip pipeline filter "filterContoursOutput" sees the target
+// // loop through each contour image
+// // grab the bounding rectangle values of each contour
+// // to create the biggest rectangle around the 2 vertical retroreflective tapes
+// // on either side of the lift peg
+// if (!lPipeline.filterContoursOutput().isEmpty()) {
+//
+// for (int i = 0; i < lPipeline.filterContoursOutput().size(); i++) {
+// Rect r = Imgproc.boundingRect(lPipeline.filterContoursOutput().get(i));
+//
+// // set the min/max values to match the values form the 1st image
+// if (i == 0) {
+// lminX = r.x;
+// lminY = r.y;
+// lmaxX = r.x + r.width;
+// lmaxY = r.y + r.height;
+// }
+//
+// // compare each value to the min/max and replace if a better one is found
+// if (r.x < lminX) {
+// lminX = r.x;
+// }
+// if (r.y < lminY) {
+// lminY = r.y;
+// }
+// if (r.x + r.width > lmaxX) {
+// lmaxX = r.x + r.width;
+// }
+// if (r.y + r.height> lmaxY) {
+// lmaxY = r.y + r.height;
+// }
+// }
+//
+//// // this is a second method of looping through the contours in the filterContoursOutput Array of Mat Images
+//// for (MatOfPoint contour : pipeline.filterContoursOutput()) {
+//// Rect r = Imgproc.boundingRect(contour);
+//// if (r.x < minX) {
+//// minX = r.x;
+//// }
+//// }
+//
+//
+//
+// synchronized (imgLockLift) {
+// liftCurrentCenter = lminX + ((lmaxX - lminX) / 2);
+//
+// // set values for the smartdashboard
+// lrectX = lminX;
+// lrectY = lminY;
+// lrectWidth = lmaxX - lminX;
+// lrectHeight = lmaxY - lminY;
+//
+// // note - this formula was pulled from 1640's github code - need to find the specific reference
+// // from 1640
+// //Equation to determine the distance from a target (d) given the width in pixels of a vision target in the camera image (w):
+// // d = (TARGET_WIDTH*CAMERA_IMAGE_WIDTH)/(2*tan(FOV_ANGLE/2)*w)
+// // i.e. d and w are inversely related.
+// liftCurrentDistance = liftTargetWidth * liftImageWidth / (2*(Math.tan(Math.toRadians(fovDegrees))/2)*lrectWidth);
+//
+// // display the current image on the driver station
+//
+//
+// outputStreamLift.putFrame(lPipeline.hslThresholdOutput());
+//
+// }
+// }
+//
+//
+//
+// // the target is not in the camera (ie, pipeline is empty)
+// else {
+// liftHasTarget = false;
+// lminX = 0;
+// lminY = 0;
+// lmaxX = 0;
+// lmaxY = 0;
+// }
+//
+// });
+// visionThreadLift.start();
+// }
+//
+//
+// /*
+// * liftProcessData
+// * Method to interpret the camera data received above
+// */
+// public void liftProcessData() {
+// try {
+//
+// // use the sonar to get the distance from the target (backup plan if camera distance not available)
+// // currentDistance=Robot.drivetrain.getSonarDistance();
+//
+// // if robot sees the target (current X between its min and max)
+// if ((liftCurrentCenter > minThresholdX) && (liftCurrentCenter < maxThresholdX)) {
+// liftHasTarget = true;
+// }
+// else {
+// liftHasTarget = false;
+// }
+//
+// } catch (TableKeyNotDefinedException e) {
+// e.printStackTrace();
+// }
+// }
+//
+// /*
+// * isCentered
+// * Method to determine whether the robot sees the center of the target (within the threshold value)
+// */
+// public boolean liftIsCentered() {
+//
+// // if the robot sees the target
+// // determine whether the horizontal value the robot sees is within the threshold defining the center of the target
+// // set isCentered according to whether the robot is aligned with the center of the target
+// if (liftHasTarget)
+// {
+//
+// double difference = trueCenter - (liftCurrentCenter);
+// if (Math.abs(difference) <= thresholdX) {
+// liftIsCentered = true;
+// }
+// else if (Math.abs(difference) > thresholdX) {
+// liftIsCentered = false;
+// }
+// liftRotateDiff = difference;
+// }
+// else{
+// liftIsCentered = false;
+// }
+//
+// return liftIsCentered;
+// }
+//
+//
+// /*
+// * getRotate
+// * Method to determine whether the robot is at the center of the target so it can drive towards target
+// */
+// public double liftGetRotate() {
+// double difference=0;
+//
+// // currently we are only running 1 cycle of the sweep and stopping
+// // if in the future additional sweeps are required, this is where the reset should occur
+// // if (sweepCounter > 400){
+// // sweepCounter = 0;
+// // }
+//
+// // if robot sees target and is centered - no need to rotate the robot
+// if (liftHasTarget && liftIsCentered)
+// {
+// rotate = 0.0;
+// }
+//
+// // if the robot sees the target but is not centered
+// // check to see whether the robot is within the threshold
+// // rotate based on the math function
+// else if (liftHasTarget && !liftIsCentered){
+// difference = trueCenter - (liftCurrentCenter);
+//
+// rotate = Math708.getClippedPercentError(liftCurrentCenter, trueCenter, AutoConstants.DRIVE_ROTATE_MIN, AutoConstants.DRIVE_ROTATE_MAX);
+//
+//
+// if (Math.abs(difference) > thresholdX) {
+// if (liftCurrentCenter < trueCenter){
+// rotate = Math.abs(rotate);
+// }
+// else {
+// rotate = Math.abs(rotate) * -1;
+// }
+// }
+// }
+//
+// // if the robot does not have the target
+// // begin the sweep
+// // sweep is defined as rotating the robot right, left, right in predefined counts
+// // if in the sweep the robot does not find the target, it stops after 3 sweeps
+// // otherwise it will jump back into the hasTarget logic identified above
+// else if (!liftHasTarget){
+// if (Math.abs(liftSweepDirection) < .1){
+// liftSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// }
+// else if (liftSweepDirection > AutoConstants.SWEEP1_MIN){
+// if ((liftSweepCounter >= AutoConstants.SWEEP1_MIN && liftSweepCounter <= AutoConstants.SWEEP1_MAX)
+// || (liftSweepCounter >= AutoConstants.SWEEP3_MIN && liftSweepCounter <= AutoConstants.SWEEP3_MAX)){
+//
+// rotate = -AutoConstants.SWEEP_ROTATE;
+// if (liftSweepCounter== AutoConstants.SWEEP1_MAX || liftSweepCounter== AutoConstants.SWEEP3_MAX){
+// liftSweepDirection = AutoConstants.SWEEP_DIRECTION_LEFT;
+// }
+// }
+// }
+// else {
+// if (liftSweepCounter >= AutoConstants.SWEEP2_MIN && liftSweepCounter <= AutoConstants.SWEEP2_MAX)
+// rotate = AutoConstants.SWEEP_ROTATE;
+// if (liftSweepCounter== AutoConstants.SWEEP2_MAX){
+// liftSweepDirection = AutoConstants.SWEEP_DIRECTION_RIGHT;
+// }
+// }
+//
+// liftSweepCounter++;
+// }
+// liftRotateDiff = difference;
+// return rotate;
+// }
+//
+// /*
+// * getMove
+// * Method to determine if the robot is close enough to target so it can stop
+// */
+//
+// public double liftGetMove() {
+//
+// // if the robot sees the target
+// // Method to determine whether the robot is at the correct distance to the target so stop
+// if (liftHasTarget)
+// {
+// double difference = liftDistanceToStop - liftCurrentDistance;
+// move = Math708.getClippedPercentError(liftCurrentDistance, liftDistanceToStop, AutoConstants.DRIVE_MOVE_MIN, AutoConstants.DRIVE_MOVE_MAX);
+//
+// //Check if target is at correct distance within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// move = 0.0;
+// liftIsAtDistance = true;
+// } else {
+// liftIsAtDistance = false;
+// }
+// liftMoveDiff = difference;
+// } else {
+// move = 0.0;
+// }
+//
+// return move;
+// }
+//
+// /**
+// * GETTERS and PUTTERS to return the private variables
+// * @return
+// */
+//
+//
+// public boolean liftIsAtDistance() {
+// double difference = liftDistanceToStop - liftCurrentDistance;
+// //Check if target is at correct level within threshold
+// if (Math.abs(difference) <= thresholdDistance) {
+// liftIsAtDistance = true;
+// } else {
+// liftIsAtDistance = false;
+// }
+// return liftIsAtDistance;
+// }
+//
+// public boolean liftIsHasTarget() {
+// return liftHasTarget;
+// }
+//
+// public void putLiftHasTarget(boolean ht) {
+// liftHasTarget = ht;
+// }
+//
+// public void putLiftCurrentCenter(double cc) {
+// liftCurrentCenter = cc;
+// }
+//
+// public int getLiftCounter() {
+// return liftSweepCounter;
+// }
+//
+//
+// public void putLiftCounter(int ct) {
+// liftSweepCounter = ct;
+// }
+//
+// public void putLiftIsCentered(boolean ic) {
+// liftIsCentered = ic;
+// }
+//
+//
+// public void putLiftAtDistance(boolean ay) {
+// liftIsAtDistance = ay;
+// }
+//
+//
+// public boolean liftIsInSweep() {
+// if (liftHasTarget) {
+// liftInSweep = false;
+// liftSweepCounter=1;
+// }
+// else {
+// liftInSweep = true;
+// }
+// return liftInSweep;
+// }
+//
+// public void sendToDashboard() {
+// if (Constants.LIFT_DEBUG) {
+//
+// SmartDashboard.putBoolean("L-Has Target", liftIsHasTarget());
+// SmartDashboard.putBoolean("L-Is At Distance", liftIsAtDistance());
+// SmartDashboard.putNumber("L-Center of Target", liftCurrentCenter);
+// SmartDashboard.putNumber("L-Rotation", rotate);
+// SmartDashboard.putNumber("L-Rotate Difference", liftRotateDiff);
+// SmartDashboard.putNumber("L-Distance Difference", liftMoveDiff);
+// SmartDashboard.putNumber("L-Sweep Counter", liftSweepCounter);
+// SmartDashboard.putNumber("L-SweepDirection", liftSweepDirection);
+// SmartDashboard.putBoolean("L-isCentered", liftIsCentered());
+// SmartDashboard.putNumber("L-rectX", lrectX);
+// SmartDashboard.putNumber("L-rectY", lrectY);
+// SmartDashboard.putNumber("L-rectWidth", lrectWidth);
+// SmartDashboard.putNumber("L-rectHeight", lrectHeight);
+// SmartDashboard.putNumber("L-Distance To Target", liftCurrentDistance);
+// SmartDashboard.putNumber("L-pipelineSize", liftPipelineSize);
+// }
+// }
+//
+// public void initDefaultCommand() {
+// if (Constants.LIFT_DEBUG) {
+// }
+// }
+//
+//}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/subsystems/VisionProcessor.java b/src/org/usfirst/frc/team708/robot/subsystems/VisionProcessor.java
index 15a4afb..4b5cb7d 100644
--- a/src/org/usfirst/frc/team708/robot/subsystems/VisionProcessor.java
+++ b/src/org/usfirst/frc/team708/robot/subsystems/VisionProcessor.java
@@ -1,200 +1,203 @@
-package org.usfirst.frc.team708.robot.subsystems;
-
-import org.usfirst.frc.team708.robot.AutoConstants;
-
-//import org.team708.robot.commands.visionProcessor.ProcessData;
-
-import org.usfirst.frc.team708.robot.Constants;
-import org.usfirst.frc.team708.robot.util.Math708;
-
-//import edu.wpi.first.wpilibj.Preferences;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.networktables.NetworkTable;
-//import edu.wpi.first.wpilibj.networktables2.type.NumberArray;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
-
-/**
- *
- */
-public class VisionProcessor extends Subsystem {
-
- private NetworkTable roboRealmInfo;
-// NumberArray deprecated private NumberArray targetCrosshair;
-
- private boolean hasTarget;
- private boolean wasCentered;
- private boolean isAtY = false;
-
- private final double imageWidth = 320;
-// private final double targetWidth = 18; //width of target in inches
-
- private double centerX = 0.0;
- private double targetY = AutoConstants.Y_TARGET;
- private double currentX = 0.0;
- private double currentY = 0.0;
-
- private double thresholdX = AutoConstants.X_THRESHOLD;
- private double thresholdY = AutoConstants.Y_THRESHOLD;
-
- // High goal aspect ratio (11ft6in/3ft1in) in inches (3.729 repeating)
-// private final double targetAspectRatio = 3.73;
-
- // Distance related measurements from the network table
- // private double distanceToTarget = 0.0;
-// private int differencePx = 0;
-// private final double targetDiameterIn = 24;
-
- // Data sent from the network table
-// private double currentAspectRatio = 0.0;
-// private double upper_left_x = 0;
-// private double upper_left_y = 0;
-// private double upper_right_x = 0;
-// private double upper_right_y = 0;
-// private double lower_left_x = 0;
-// private double lower_left_y = 0;
-// private double radius = 0;
-// private double blob_count = 0;
-
-// private double lowerLengthX;
-// private double setProportion;
-
- //daisy says to set this to 43.5 deg
-// private final double cameraFOVRads = Math.toRadians(47);
-// private double cameraFOVRads = Math.toRadians(43.5);
- double rotate;
-
- public VisionProcessor() {
- super("Vision Processor");
- roboRealmInfo = NetworkTable.getTable("vision");
-
-// Number array deprecated targetCrosshair = new NumberArray();
- centerX = imageWidth / 2;
- }
-
- public void processData() {
- try {
- currentX= roboRealmInfo.getNumber("cx", 0);
- currentY= roboRealmInfo.getNumber("cy", 0);
-// upper_left_x = (double) roboRealmInfo.getNumber("p1x");
-// upper_left_y = (double) roboRealmInfo.getNumber("p1y");
-// upper_right_x = (double)roboRealmInfo.getNumber("p2x");
-// upper_right_y = (double)roboRealmInfo.getNumber("p2y");
-// lower_left_x = (double) roboRealmInfo.getNumber("p3x");
-// lower_left_y = (double) roboRealmInfo.getNumber("p3y");
-
- if (currentX > 0) {
- hasTarget = true;
- } else {
- hasTarget = false;
- }
-
-
- } catch (TableKeyNotDefinedException e) {
- e.printStackTrace();
- }
- }
-
- public double getRotate() {
-
- if (hasTarget)
- {
-
- double difference = centerX - (currentX);
- rotate = Math708.getSignClippedPercentError(currentX, centerX, 0.3, 0.5);
-
- if (Math.abs(difference) <= thresholdX) {
- rotate = 0.0;
- wasCentered = true;
- }
- else if (Math.abs(difference) > thresholdX) {
- wasCentered = false;
- }
-
-
- /*
- rotate = difference / centerX;
-
-
- if (rotate > 0.0) {
- //reverses the sign to turn left, when target is left
- rotate = -Constants.VISION_ROTATE_MOTOR_SPEED;
- }
- else {
- rotate = Constants.VISION_ROTATE_MOTOR_SPEED;
- }
- */
- }
-
- else {
- //rotates if not target (default is right) if loses/doesn't have target
- rotate = -0.4;
-
- }
-
- return rotate;
- }
-
- //Returns how to move to get to target distance (targetAmount = target ratio)
-
- public double getMove() {
- double move;
-
- if (hasTarget)
- {
- double difference = targetY - currentY;
- move = Math708.getSignClippedPercentError(currentY, targetY, 0.4, 0.6);
- //Check if target is at correct level within threshold
- if (difference <= thresholdY) {
- move = 0.0;
- isAtY = true;
- } else {
- isAtY = false;
- }
-
- } else {
- move = 0.0;
- }
-
- return move;
- }
-
- /**
- * Returns if the robot sees a container
- * @return
- */
- public boolean isHasTarget() {
- return hasTarget;
- }
-
- public boolean isAtY() {
- double difference = targetY - currentY;
- //Check if target is at correct level within threshold
- if (Math.abs(difference) <= thresholdY) {
- isAtY = true;
- } else {
- isAtY = false;
- }
- return isAtY;
- }
-
- public boolean wasCentered() {
- return wasCentered;
- }
-
- public void sendToDashboard() {
-// SmartDashboard.putBoolean("See Target", isHasTarget());
-//// SmartDashboard.putBoolean("Was Centered", wasCentered());
-// SmartDashboard.putBoolean("Is At Y", isAtY());
-// SmartDashboard.putNumber("Current Y", currentY);
-// SmartDashboard.putNumber("Center of Target", currentX);
-//// SmartDashboard.putNumber("Rotation", rotate);
- SmartDashboard.putString("visionProcessor", "called");
- }
-
- public void initDefaultCommand() {
- if (Constants.DEBUG) {
- }
- }
-}
-
+//package org.usfirst.frc.team708.robot.subsystems;
+//
+//import org.usfirst.frc.team708.robot.AutoConstants;
+//
+////import org.team708.robot.commands.visionProcessor.ProcessData;
+//
+//import org.usfirst.frc.team708.robot.Constants;
+//import org.usfirst.frc.team708.robot.util.Math708;
+//
+////import edu.wpi.first.wpilibj.Preferences;
+//import edu.wpi.first.wpilibj.command.Subsystem;
+//import edu.wpi.first.wpilibj.networktables.NetworkTable;
+////import edu.wpi.first.wpilibj.networktables2.type.NumberArray;
+//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+//import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
+//
+///**
+// *
+// */
+//public class VisionProcessor extends Subsystem {
+//
+// private NetworkTable roboRealmInfo;
+//// NumberArray deprecated private NumberArray targetCrosshair;
+//
+// private boolean seesTarget;
+// private boolean isCentered;
+// private boolean isAtY = false;
+//
+// private final double imageWidth = 320;
+// private final double imageHeight = 240;
+// private final double targetWidth = 10.25; //width of lever in inches
+//
+//// Gear 11" in diameter
+//// Boiler Target is 1'3" wide 9" high -- 7' off ground to center
+//// lever is 10.25" wide 5" high -- 12.75" off ground to center
+//
+// private double centerX = 0.0;
+// private double targetY = 100; //location of target when on hook
+// private double currentX = 0.0;
+// private double currentY = 0.0;
+//
+// private double thresholdX = 20;
+// private double thresholdY = 20;
+//
+//// lift aspect ratio (12.75" / 23") .55435
+//// lift aspect ratio (4" / 23") (.174)
+// private final double targetAspectRatio = .55435;
+//
+//// Distance related measurements from the network table
+//// private double distanceToTarget = 0.0;
+//// private int differencePx = 0;
+//// private final double targetDiameterIn = 24;
+//
+//// Data sent from the network table
+//// private double currentAspectRatio = 0.0;
+//// private double upper_left_x = 0;
+//// private double upper_left_y = 0;
+//// private double upper_right_x = 0;
+//// private double upper_right_y = 0;
+//// private double lower_left_x = 0;
+//// private double lower_left_y = 0;
+//// private double radius = 0;
+//// private double blob_count = 0;
+//
+//// private double lowerLengthX;
+//// private double setProportion;
+//
+////daisy says to set this to 43.5 deg
+//// private final double cameraFOVRads = Math.toRadians(47);
+//// private double cameraFOVRads = Math.toRadians(43.5);
+//
+// double rotate;
+//
+// public VisionProcessor() {
+// super("Vision Processor");
+// roboRealmInfo = NetworkTable.getTable("vision");
+//
+// centerX = imageWidth / 2;
+// }
+//
+// public void processData() {
+// try {
+// currentX= roboRealmInfo.getNumber("lever_x", 0);
+// currentY= roboRealmInfo.getNumber("lever_y", 0);
+//
+//// upper_left_x = (double) roboRealmInfo.getNumber("p1x");
+//// upper_left_y = (double) roboRealmInfo.getNumber("p1y");
+//// upper_right_x = (double)roboRealmInfo.getNumber("p2x");
+//// upper_right_y = (double)roboRealmInfo.getNumber("p2y");
+//// lower_left_x = (double) roboRealmInfo.getNumber("p3x");
+//// lower_left_y = (double) roboRealmInfo.getNumber("p3y");
+//
+// if (currentX > 0) {
+// seesTarget = true;
+// } else {
+// seesTarget = false;
+// }
+//
+//
+// } catch (TableKeyNotDefinedException e) {
+// e.printStackTrace();
+// }
+// }
+//
+// public double getRotate() {
+//
+// if (seesTarget)
+// {
+//
+// double difference = centerX - (currentX);
+// rotate = Math708.getClippedPercentError(currentX, centerX, 0.25, 0.35);
+//
+// if (Math.abs(difference) <= thresholdX) {
+// rotate = 0.0;
+// isCentered = true;
+// }
+// else if (Math.abs(difference) > thresholdX) {
+// isCentered = false;
+// }
+//
+// rotate = difference / centerX;
+//
+// if (rotate > 0.0) {
+// rotate = -rotate; //reverses the sign to turn left, when target is left
+// }
+// }
+//
+// else {
+// //rotates if not target (default is right) if loses/doesn't have target
+// //let's d0 a sweep
+//
+//
+// }
+//
+// return rotate;
+// }
+//
+// //Returns how to move to get to target distance (targetAmount = target ratio)
+//
+// public double getMove() {
+//
+// double move = 0.0;
+// double difference = targetY - currentY;
+//
+// if (seesTarget)
+// {
+// move = Math708.getClippedPercentError(currentY, targetY, 0.25, 0.35);
+// //Check if target is at correct level within threshold
+// if (difference <= thresholdY) {
+// move = 0.0;
+// isAtY = true;
+// } else {
+// isAtY = false;
+// }
+//
+// } else {
+// move = 0.0;
+// }
+//
+// return move;
+// }
+//
+// /**
+// * Returns if the robot sees a container
+// * @return
+// */
+// public boolean isHasTarget() {
+// return seesTarget;
+// }
+//
+// public boolean isAtY() {
+// double difference = targetY - currentY;
+// //Check if target is at correct level within threshold
+// if (Math.abs(difference) <= thresholdY) {
+// isAtY = true;
+// } else {
+// isAtY = false;
+// }
+// return isAtY;
+// }
+//
+// public boolean isCentered() {
+// return isCentered;
+// }
+//
+// public void sendToDashboard() {
+// if (Constants.DEBUG) {
+// SmartDashboard.putBoolean("See Target", isHasTarget());
+// SmartDashboard.putBoolean("Is Centered", isCentered());
+// SmartDashboard.putBoolean("Is At Y", isAtY());
+// SmartDashboard.putNumber("Current Y", currentY);
+// SmartDashboard.putNumber("Center of Target", currentX);
+// SmartDashboard.putNumber("Rotation", rotate);
+//// SmartDashboard.putString("visionProcessor", "called");
+// }
+// }
+//
+// public void initDefaultCommand() {
+// if (Constants.DEBUG) {
+// }
+// }
+//}
+//
diff --git a/src/org/usfirst/frc/team708/robot/util/Gamepad.java b/src/org/usfirst/frc/team708/robot/util/Gamepad.java
index abf36aa..37d6321 100644
--- a/src/org/usfirst/frc/team708/robot/util/Gamepad.java
+++ b/src/org/usfirst/frc/team708/robot/util/Gamepad.java
@@ -57,6 +57,7 @@ public Gamepad(int port){
* @param axis
* @return
*/
+
public double getAxis(int axis){
double val = getRawAxis(axis);
if(Math.abs(val) <= axis_deadband){
diff --git a/src/org/usfirst/frc/team708/robot/util/HatterDrive.java b/src/org/usfirst/frc/team708/robot/util/HatterDrive.java
index 2ef6438..7332e71 100644
--- a/src/org/usfirst/frc/team708/robot/util/HatterDrive.java
+++ b/src/org/usfirst/frc/team708/robot/util/HatterDrive.java
@@ -1,9 +1,9 @@
package org.usfirst.frc.team708.robot.util;
-import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-public class HatterDrive extends RobotDrive {
+public class HatterDrive extends DifferentialDrive {
private double turnSensitivity = 1.5; // How sensitive turning is for the
// drivetrain
@@ -13,12 +13,13 @@ public class HatterDrive extends RobotDrive {
private final boolean USE_SAFETY = false;
+ /*
public HatterDrive(int leftMotorChannel, int rightMotorChannel, boolean squaredInputs) {
super(leftMotorChannel, rightMotorChannel);
this.setSafetyEnabled(USE_SAFETY);
this.squaredInputs = squaredInputs;
}
-
+*/
public HatterDrive(SpeedController leftMotor, SpeedController rightMotor, boolean squaredInputs) {
super(leftMotor, rightMotor);
this.setSafetyEnabled(USE_SAFETY);
@@ -66,7 +67,7 @@ public void cheesyDrive(double move, double rotate, boolean quickTurn) {
lPower = correctDriveStall(lPower);
rPower = correctDriveStall(rPower);
- setLeftRightMotorOutputs(lPower, rPower);
+ // setLeftRightMotorOutputs(lPower, rPower); HATTERS DRIVE DOES NOT USE THIS James Makovics
}
/**
diff --git a/src/org/usfirst/frc/team708/robot/util/Math708.java b/src/org/usfirst/frc/team708/robot/util/Math708.java
index 49d8e13..93ae07f 100644
--- a/src/org/usfirst/frc/team708/robot/util/Math708.java
+++ b/src/org/usfirst/frc/team708/robot/util/Math708.java
@@ -113,7 +113,7 @@ public static boolean squareWave(Timer timer,double onTimeSec,boolean prevValue)
*/
public static double getPercentError(double currentValue, double goalValue)
{
- return (currentValue - goalValue) / goalValue;
+ return Math.abs(currentValue - goalValue) / goalValue;
}
/**
@@ -127,20 +127,10 @@ public static double getPercentError(double currentValue, double goalValue)
* @return
*/
public static double getClippedPercentError(double currentValue, double goalValue, double minimumValue, double maximumValue) {
- return makeWithin(getPercentError(currentValue, goalValue), minimumValue, maximumValue);
- }
-
- public static double getSignClippedPercentError(double currentValue, double goalValue, double minimumValue, double maximumValue) {
- double sign = Math.signum(getPercentError(currentValue, goalValue));
- if (sign < 0) {
- return makeWithin(getPercentError(currentValue, goalValue), minimumValue * sign, maximumValue * sign);
- }
- else if (sign > 0) {
- return makeWithin(getPercentError(currentValue, goalValue), minimumValue * sign, maximumValue * sign);
- }
- else {
- return 0;
- }
+ double percentError = getPercentError(currentValue, goalValue);
+ double sign = Math.signum(percentError);
+
+ return sign * makeWithin(Math.abs(getPercentError(currentValue, goalValue)), minimumValue, maximumValue);
}
public double accelerateToSpeed(double maxSpeed, double accel){
@@ -178,4 +168,4 @@ public static double convergeOnSpeed(double speed){
public static boolean isWithinThreshold(double currentValue, double goalValue, double threshold) {
return Math.abs(goalValue - currentValue) <= threshold;
}
-}
+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team708/robot/util/triggers/AxisDown.java b/src/org/usfirst/frc/team708/robot/util/triggers/AxisDown.java
new file mode 100644
index 0000000..1d4808f
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/util/triggers/AxisDown.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team708.robot.util.triggers;
+
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.buttons.Trigger;
+
+/**
+ * Makes moving an axis down on a joystick result in a button input
+ */
+public class AxisDown extends Trigger {
+
+ private Gamepad gamepad;
+ private int axis;
+
+ public AxisDown(Gamepad targetGamepad, int targetAxis) {
+ gamepad = targetGamepad;
+ axis = targetAxis;
+ }
+
+ public boolean get() {
+ return (gamepad.getAxis(axis) <= -.50);
+ }
+}
diff --git a/src/org/usfirst/frc/team708/robot/util/triggers/AxisUp.java b/src/org/usfirst/frc/team708/robot/util/triggers/AxisUp.java
new file mode 100644
index 0000000..e75c8c4
--- /dev/null
+++ b/src/org/usfirst/frc/team708/robot/util/triggers/AxisUp.java
@@ -0,0 +1,23 @@
+package org.usfirst.frc.team708.robot.util.triggers;
+
+import org.usfirst.frc.team708.robot.util.Gamepad;
+
+import edu.wpi.first.wpilibj.buttons.Trigger;
+
+/**
+ * Makes moving an axis up on a joystick result in a button input
+ */
+public class AxisUp extends Trigger {
+
+ private Gamepad gamepad;
+ private int axis;
+
+ public AxisUp(Gamepad targetGamepad, int targetAxis) {
+ gamepad = targetGamepad;
+ axis = targetAxis;
+ }
+
+ public boolean get() {
+ return (gamepad.getAxis(axis) >= .50);
+ }
+}
diff --git a/sysProps.xml b/sysProps.xml
index 5aba6bb..a7a7951 100644
Binary files a/sysProps.xml and b/sysProps.xml differ