gistfile1.txt

package org.usfirst.frc.team8080.robot;

import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {
	RobotDrive robot = new RobotDrive(0,1,2,3);
	Joystick stick = new Joystick(0);
	Spark leftShooter = new Spark(5);
	Spark rightShooter = new Spark(4);
	Spark ballCollector = new Spark(6);
	Victor ropeClimberLeft = new Victor(7);
	Victor ropeClimberRight = new Victor(8);
	
	
	Thread visionThread;
	
	boolean seeColor = false;
	@Override
	public void teleopPeriodic() {
		/*Allowing the robot to drive using the stick as input
		 * 
		 * We make the x-axis negative to inverse the stick (So it feels good)
		 * */
		 
		robot.arcadeDrive(stick.getY(),-stick.getX());
		
		
		
		if(stick.getRawButton(1)){ //if the trigger is pressed, shoot
			leftShooter.set(1);
			rightShooter.set(-1);
		}else{ //if the trigger is not pressed stop shooting balls
			leftShooter.set(0);
			rightShooter.set(0);	
		}
		
		if(stick.getRawButton(12)){ //if button 12 is pressed collected balls
			ballCollector.set(-1);
		}else{ // if button 12 is not pressed stop collecting balls.
			ballCollector.set(0);
		}
		
		if(stick.getRawButton(11)){ //if button 11 is pressed, climb the rope
			ropeClimberLeft.set(1);
			ropeClimberRight.set(1);
		}else{ //if button 11 is not pressed, stop climbing the rope
			ropeClimberLeft.set(0);
			ropeClimberRight.set(0);	
		}
		
	}

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	@Override
	public void robotInit() {
		
		//this thread updates the camera and does color calculations
		visionThread = new Thread(() -> {
			// Get the UsbCamera from CameraServer
			UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
			// Set the resolution
			camera.setResolution(640, 480);

			// Get a CvSink. This will capture Mats from the camera
			CvSink cvSink = CameraServer.getInstance().getVideo();
			// Setup a CvSource. This will send images back to the Dashboard
			CvSource outputStream = CameraServer.getInstance().putVideo("Debug", 640, 480);

			// Mats are very memory expensive. Lets reuse this Mat.
			Mat mat = new Mat();

			// This cannot be 'true'. The program will never exit if it is. This
			// lets the robot stop this thread when restarting robot code or
			// deploying.
			while (!Thread.interrupted()) {
				
				// Tell the CvSink to grab a frame from the camera and put it
				// in the source mat.  If there is an error notify the output.
				if (cvSink.grabFrame(mat) == 0) {
					// Send the output the error.
					outputStream.notifyError(cvSink.getError());
					// skip the rest of the current iteration
					continue;
				}
				
				
				double[] color = mat.get(480/2, 640/2);
				double[] hsv = rgb2hsv(color[2], color[1], color[0]);
				
				
				Imgproc.putText(mat, "hue: " + hsv[0], new Point(0,50), Core.FONT_HERSHEY_PLAIN, 3, new Scalar(0,255,0));
				
				Imgproc.putText(mat, "seeColor: " + seeColor, new Point(0,100), Core.FONT_HERSHEY_PLAIN, 3, new Scalar(0,0,255));
				
				Imgproc.line(mat, new Point(590,0), new Point(590,500), new Scalar(0,255,0));
				if(hsv[0] <=  220 && hsv[0] >= 190){ //This checks if the color is BLUE
					seeColor = true;
				}else{
					seeColor = false;
				}
				
				// Put a rectangle on the image
				
				Imgproc.circle(mat, new Point(640/2, 480/2), 20, new Scalar(color[0], color[1], color[2]), -1);
				

				// Give the output stream a new image to display
				//Core.transpose(mat, mat);
				//Core.flip(mat, mat, 1);
				
				
				outputStream.putFrame(mat);
			}
		});
		
		
		visionThread.setDaemon(true);
		visionThread.start();
	}
	
	//Helper function to convert rgb values to hsv values. 
	public double[] rgb2hsv (double r,double g,double b) {
		 double computedH = 0;
		 double computedS = 0;
		 double computedV = 0;
		 
		
		 r=r/255; g=g/255; b=b/255;
		 double minRGB = Math.min(r,Math.min(g,b));
		 double maxRGB = Math.max(r,Math.max(g,b));

		 // Black-gray-white
		 if (minRGB==maxRGB) {
		  computedV = minRGB;
		  return new double[]{0,0,computedV};
		 }

		 // Colors other than black-gray-white:
		 double d = (r==minRGB) ? g-b : ((b==minRGB) ? r-g : b-r);
		 double h = (r==minRGB) ? 3 : ((b==minRGB) ? 1 : 5);
		 computedH = 60*(h - d/(maxRGB - minRGB));
		 computedS = (maxRGB - minRGB)/maxRGB;
		 computedV = maxRGB;
		 return new double[] {computedH,computedS,computedV};
		}

	/**
	 * This function is run once each time the robot enters autonomous mode
	 */
	@Override
	public void autonomousInit() {
		
		
	}

	/**
	 * This function is called periodically during autonomous
	 */
	
	boolean seenBlue = false;
	@Override
	public void autonomousPeriodic() {
		if(seeColor){
			seenBlue = true;
		}
		
		if(!seenBlue){
			robot.arcadeDrive(0,.5);
		}
		
		if(seenBlue){
			leftShooter.set(1);
			rightShooter.set(-1);
		}
	}

	/**
	 * This function is called once each time the robot enters tele-operated
	 * mode
	 */
	@Override
	public void teleopInit() {
	}

	/**
	 * This function is called periodically during operator control
	 */
	

	/**
	 * This function is called periodically during test mode
	 */
	@Override
	public void testPeriodic() {
		LiveWindow.run();
	}
}