DO NOT MERGE YET!!!

Steamworks/.classpath

@@ -8,6 +8,5 @@
 	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.7"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/navx_frc.jar"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/TalonSRXLibJava.jar"/>
-	<classpathentry kind="var" path="USERLIBS_DIR/niVisionWPI.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

Steamworks/src/org/usfirst/frc/team4786/robot/Robot.java

@@ -1,13 +1,13 @@
 package org.usfirst.frc.team4786.robot;
 
 import org.usfirst.frc.team4786.robot.commands.DoNothing;
-import org.usfirst.frc.team4786.robot.commands.DriveArcSpeed;
 import org.usfirst.frc.team4786.robot.commands.DriveToLeftGearPeg;
 import org.usfirst.frc.team4786.robot.commands.DriveToPosition;
 import org.usfirst.frc.team4786.robot.commands.DriveToRightGearPeg;
 import org.usfirst.frc.team4786.robot.commands.OpenLoopDrive;
+import org.usfirst.frc.team4786.robot.commands.ProcessVisionContinuously;
 import org.usfirst.frc.team4786.robot.commands.SwitchFrontSide;
-import org.usfirst.frc.team4786.robot.commands.TurnToAngle;
+import org.usfirst.frc.team4786.robot.commands.VisionAlignWithPeg;
 import org.usfirst.frc.team4786.robot.subsystems.Arduino;
 import org.usfirst.frc.team4786.robot.subsystems.Climber;
 import org.usfirst.frc.team4786.robot.subsystems.DrawBridge;
@@ -52,6 +52,7 @@ public class Robot extends IterativeRobot {
 	public static CvSink cvSink;
 	public static CvSource outputStream;
 	public static CvSource regStream;
+	public static CvSource visionStream;
 	public static UsbCamera gearPlacementCamera;
 	public static UsbCamera ballPlacementCamera;
 
@@ -74,15 +75,20 @@ public class Robot extends IterativeRobot {
 
 	@Override
 	public void robotInit() {
-
+    	System.out.println("ERROR: RUSSIAN HACKING ATTEMPT DETECTED");
 		gearPlacementCamera = CameraServer.getInstance().startAutomaticCapture("Gear", 0);
 		ballPlacementCamera = CameraServer.getInstance().startAutomaticCapture("Ball Camera", 1);
 		gearPlacementCamera.setFPS(15);		
 		ballPlacementCamera.setFPS(15);
 
 		gearPlacementCamera.setResolution(RobotMap.cameraFOVWidth,RobotMap.cameraFOVHeight);
 		ballPlacementCamera.setResolution(RobotMap.cameraFOVWidth, RobotMap.cameraFOVHeight);
-		cvSink = CameraServer.getInstance().getVideo();
+		gearPlacementCamera.setExposureManual(1);
+
+		cvSink = CameraServer.getInstance().getVideo("Gear");
+		//outputStream = CameraServer.getInstance().putVideo("Gear Camera", RobotMap.cameraFOVWidth, RobotMap.cameraFOVHeight);
+		//= new CvSource("GearCamera", VideoMode.PixelFormat.kRGB565, RobotMap.cameraFOVWidth, RobotMap.cameraFOVHeight, 15);
+
 
 		frontSide = "Gear";
 
@@ -92,16 +98,14 @@ public void robotInit() {
 
 
 		sendableChooser = new SendableChooser<Command>();
-		sendableChooser.addDefault("Drive to Center Gear Peg", new DriveToPosition(7));
+		sendableChooser.addDefault("Drive to Center Gear Peg", new DriveToPosition(4.83));
 		sendableChooser.addObject("Do Nothing!", new DoNothing());
 		sendableChooser.addObject("Drive to Baseline", new DriveToPosition(10));
 		sendableChooser.addObject("Drive to Left Gear Peg", new DriveToLeftGearPeg());
 		sendableChooser.addObject("Drive to Right Gear Peg", new DriveToRightGearPeg());
-		//sendableChooser.addObject("GetToGearTest", new GearFromOffset());
+		//sendableChooser.addObject("Vision align with peg", new VisionAlignWithPeg());
 		SmartDashboard.putData("Autonomous Selector", sendableChooser);
 
-		/*Command initial = new SwitchFrontSide();
-		initial.start();*/
 	}
 	@Override
 	public void disabledInit() {
@@ -131,6 +135,7 @@ public void disabledPeriodic() {
 
 	@Override
 	public void autonomousInit() {
+
 		alliance = DriverStation.getInstance().getAlliance();
 
 		//send correct alliance data to arduino
@@ -147,8 +152,11 @@ public void autonomousInit() {
 		SmartDashboard.putString("Alliance", allianceColorVal);
 
 
-	//camera.setExposureManual(RobotMap.exposure);
-    autonomousCommand = (Command) sendableChooser.getSelected();
+		//camera.setExposureManual(RobotMap.exposure);
+	    autonomousCommand = (Command) sendableChooser.getSelected();
+	    //Command visionProcess = new ProcessVisionContinuously();
+	    //visionProcess.start();
+
 		if (autonomousCommand != null)
 			autonomousCommand.start();
 	}
@@ -166,16 +174,17 @@ public void autonomousInit() {
 	@Override
 	public void autonomousPeriodic() {
 		Scheduler.getInstance().run();
+
 		SmartDashboard.putNumber("Left Encoder Positon", driveTrain.getLeftEncoderPosition());
 		SmartDashboard.putNumber("Right Encoder Positon", driveTrain.getRightEncoderPosition());
-		SmartDashboard.putNumber("Servo Angle", drawBridge.getServoAngle());
 	}
 
 	@Override
 	public void teleopInit() {
 
-
-		//gearPlacementCamera.setExposureAuto();
+    	System.out.println("ERROR: RUSSIAN HACKING ATTEMPT DETECTED");
+
+		gearPlacementCamera.setExposureAuto();
 
 		// This makes sure that the autonomous stops running when
 		// teleop starts running. If you want the autonomous to
@@ -185,6 +194,9 @@ public void teleopInit() {
 		if (autonomousCommand != null)
 			autonomousCommand.cancel();
 
+		Command switchFront = new SwitchFrontSide();
+		switchFront.start();
+
 		teleopCommand = new OpenLoopDrive();
 
 		if(teleopCommand != null)
@@ -205,7 +217,7 @@ public void teleopPeriodic() {
 		SmartDashboard.putBoolean("Peg Present", gear.pegLimitSwitchPressed());
 		SmartDashboard.putNumber("New State", SwitchState.newState);  //These two lines are dependent of the SwitchState method
 		SmartDashboard.putNumber("Old State", SwitchState.oldState);  // get rid of them if we don't have this method
-
+		SmartDashboard.putBoolean("Reversed", driveTrain.isReversed());
 	}
 
 	@Override

Steamworks/src/org/usfirst/frc/team4786/robot/RobotMap.java

@@ -11,8 +11,7 @@ public class RobotMap {
 
 	//CANTalon ports, use instead of random numbers
 
-	/*public static final int frontLeftPort = 14;
-	public static final int frontRightPort = 13;*/
+
 	public static final int frontLeftPort = 13;
 	public static final int frontRightPort = 14;
 
@@ -29,12 +28,12 @@ public class RobotMap {
 	public static final double MESHBOT_ROBOT_LENGTH = 2.34375; //In feet
 
 	//General PID Constants
-	public static final int ERROR_CONSTANT_LEFT = 40; //In native units
+	public static final int ERROR_CONSTANT_LEFT = 20; //In native units
 	public static final int ERROR_CONSTANT_RIGHT = 20;
 	public static final int ALLOWABLE_TURN_ERROR = 1; //In degrees
-	public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 1024;
+	public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 1024; //Professor Elemental
 	public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_RIGHT = 1024;
-	/*public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 360;
+	/*public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_LEFT = 360; //Meshbot
 	public static final int DRIVETRAIN_ENCODER_CODES_PER_REV_RIGHT = 360;*/
 	public static final double CLOSED_LOOP_RAMP_RATE = 0.015625;
 	public static final int IZONE = 0;
@@ -107,6 +106,7 @@ public class RobotMap {
 	public static final int ledArduinoPort = 8;
 
 	//Vision constants
+	public static final double finalDistanceFromPeg = .1;
 	//for filtering
 	//new values: R:0, G: 196. B: 120
 	public static final int highRedValue = 50;
@@ -131,6 +131,9 @@ public class RobotMap {
 	//for calculating angles
 	public static final double distanceBetweenCentersOfTargets = 8.25/12;
 
+	//for PID
+	public static final double ALLOWABLE_CENTER_ERROR = 10; //In pixels
+
     /* For example to map the left and right motors, you could define the
      following variables to use with your drivetrain subsystem.
      If you are using multiple modules, make sure to define both the port

Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToLeftGearPeg.java

@@ -14,11 +14,11 @@ public DriveToLeftGearPeg() {
     	addSequential(new DriveToPosition(1));*/
 
     	//initial drive straight in feet
-    	addSequential(new DriveToPosition(2));
+    	addSequential(new DriveToPosition(1));
     	//turning degrees via .22 power ratio on inside of turn for .75 seconds
     	addSequential(new DriveArcSpeed(1, 1 * 0.22, 0.75));
     	//final drive straight to target
-    	addSequential(new DriveToPosition(3));
+    	addSequential(new DriveToPosition(1));
 
         // To run multiple commands at the same time,
         // use addParallel()

Steamworks/src/org/usfirst/frc/team4786/robot/commands/DriveToRightGearPeg.java

@@ -14,11 +14,11 @@ public DriveToRightGearPeg() {
 
 
     	//initial drive straight in feet
-    	addSequential(new DriveToPosition(2));
+    	addSequential(new DriveToPosition(1));
     	//turning degrees via .22 power ratio on inside of turn for .75 seconds
     	addSequential(new DriveArcSpeed(1 * 0.22, 1 , 0.75));
     	//final drive straight to target
-    	addSequential(new DriveToPosition(3));
+    	addSequential(new DriveToPosition(1));
 
         // Add Commands here:
         // e.g. addSequential(new Command1());

Steamworks/src/org/usfirst/frc/team4786/robot/commands/ProcessVisionContinuously.java

@@ -0,0 +1,54 @@
+package org.usfirst.frc.team4786.robot.commands;
+
+import org.opencv.core.Mat;
+import org.usfirst.frc.team4786.robot.Robot;
+import org.usfirst.frc.team4786.robot.subsystems.MatRapper;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ *
+ */
+public class ProcessVisionContinuously extends Command {
+
+    public ProcessVisionContinuously() {
+        requires(Robot.visionImage);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	Robot.gearPlacementCamera.setExposureManual(1);
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+
+    	Mat frame = new Mat();
+
+    	//Robot.outputStream.putFrame(frame);
+    	Robot.cvSink.grabFrame(frame);
+    	MatRapper rapperNamedMat = new MatRapper(frame);
+    	Robot.visionImage.processImage(rapperNamedMat);
+    	Robot.visionImage.analysis();
+    	SmartDashboard.putBoolean("Rectangles detected?", Robot.visionImage.getTwoTargets());
+    	SmartDashboard.putNumber("Right", Robot.visionImage.getDistanceRight());
+    	SmartDashboard.putNumber("Left", Robot.visionImage.getDistanceLeft());
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	//it only runs in autonomous right now
+    	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() {
+    }
+}

Steamworks/src/org/usfirst/frc/team4786/robot/commands/VisionAlignWithPeg.java

@@ -0,0 +1,50 @@
+package org.usfirst.frc.team4786.robot.commands;
+
+import org.usfirst.frc.team4786.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class VisionAlignWithPeg extends Command {
+	boolean check = false;
+
+    public VisionAlignWithPeg() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+    	requires(Robot.driveTrain);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    	//Robot.gearPlacementCamera.setExposureManual(1);
+    	Robot.driveTrain.processContinuouslyInit();
+    	check = false;
+
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+    	Robot.driveTrain.processContinuouslyExecute();
+    	check = !Robot.visionImage.getTwoTargets();
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+    	//return Robot.visionImage.isCloseEnoughToPeg();
+    	return false;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    	Robot.driveTrain.processContinuouslyEnd();
+    	System.out.println("ERROR: RUSSIAN HACKING ATTEMPT DETECTED");
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    	end();
+    }
+}