GRIP vision

AutonomousProcedure.java

package org.usfirst.frc.team1573.robot;

import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class AutonomousProcedure {
	// Autonomous variables
	private DoubleSolenoid grip;
	private DoubleSolenoid lifter;
	private RobotDrive drive;
	private double imgWidth = 1;

	private int state = 0; // 0: done; 1: Looking for box ; 2: Looking for marker

	private long boxInitStarted = 0;
	private float boxRotationPasses = 1;
	private float boxMovementPasses = 1;
	private float boxLastRotationSign = 1;
	private float boxLastMovementSign = 1;
	private long boxPickingStarted = 0;
	private boolean boxFound = false;

	private long markInitStarted = 0;
	private float markRotationPasses = 1;
	private float markMovementPasses = 1;
	private float markLastRotationSign = 1;
	private float markLastMovementSign = 1;
	private long markPlacingStarted = 0;
	private boolean markFound = false;

	public AutonomousProcedure(DoubleSolenoid grip, DoubleSolenoid lifter, RobotDrive drive) {
		this.grip = grip;
		this.lifter = lifter;
		this.drive = drive;
	}

	private void findBoxInit() {
		boxRotationPasses = 1;
		boxMovementPasses = 1;

		imgWidth = SmartDashboard.getNumber("imgWidth", 1);

		boxLastRotationSign = getBoxX() > 0 ? 1 : -1;
		boxLastMovementSign = getBoxArea() > 0 ? 1 : -1;
		boxInitStarted = System.currentTimeMillis();
		
		boxFound = SmartDashboard.getBoolean("BoxFound", false);
		state = 1;
	}

	private double getBoxX() {
		return 2 * (SmartDashboard.getNumber("BoxX", 0) + SmartDashboard.getNumber("BoxWidth", 0) / 2) / imgWidth - 1;
	}

	private double getBoxArea() {
		return Math.max(Math.min((SmartDashboard.getNumber("BoxArea", 0) - 15500) / 1500, 1), -1);
	}

	private double getMarkX() {
		return 2 * (SmartDashboard.getNumber("MarkX", 0) + SmartDashboard.getNumber("MarkWidth", 0) / 2) / imgWidth - 1;
	}

	private double getMarkY() {
		return Math.max(Math.min((SmartDashboard.getNumber("MarkY", 0) - 150) / 35, 1), -1);
	}

	private void findBoxPeriodic() {
		// Scripted procedures
		if (System.currentTimeMillis() - boxInitStarted < 1000) { // Give time for valve to open
			lifter.set(Value.kForward);
			grip.set(Value.kForward);
			drive.arcadeDrive(0, 0);
			return;
		} else if (System.currentTimeMillis() - boxPickingStarted < 1000) {
			centerBox(); // Finish centering
			return;
		} else if (System.currentTimeMillis() - boxPickingStarted < 1500) {
			drive.arcadeDrive(0, 0); // Stabilize
			return;
		} else if (System.currentTimeMillis() - boxPickingStarted < 5500) {
			lifter.set(Value.kReverse); // Lower arm
			return;
		} else if (System.currentTimeMillis() - boxPickingStarted < 6000) {
			grip.set(Value.kReverse); // Grip box
			return;
		} else if (System.currentTimeMillis() - boxPickingStarted < 12000) {
			lifter.set(Value.kForward); // Lift box
			return;
		} else if (System.currentTimeMillis() - boxPickingStarted < 12100) {
			findMarkInit();
			return;
		}
		
		// Check if box is found, and search if not
		if (!boxFound && SmartDashboard.getBoolean("BoxFound", false)) {
			boxFound = true;
			findBoxInit();
			boxInitStarted -= 1000; // Skip delay
			return;
		} else if (!SmartDashboard.getBoolean("BoxFound", false)) {
			boxFound = false;
			drive.arcadeDrive(0, 0.8);
			return;
		}

		if (centerBox()) {
			boxPickingStarted = System.currentTimeMillis();
		}
	}

	private void findMarkInit() {
		markRotationPasses = 1;
		markMovementPasses = 1;

		imgWidth = SmartDashboard.getNumber("imgWidth", 1);

		markLastRotationSign = getMarkX() > 0 ? 1 : -1;
		markLastMovementSign = getMarkY() > 0 ? 1 : -1;
		markInitStarted = System.currentTimeMillis();
		
		markFound = SmartDashboard.getBoolean("MarkFound", false);
		state = 2;
	}

	private void findMarkPeriodic() {
		// Scripted procedures
		if (System.currentTimeMillis() - markInitStarted < 1000) {
			drive.arcadeDrive(0, 0); // Stabilize
			return;
		} else if (System.currentTimeMillis() - markPlacingStarted < 1000) {
			centerMark(); // Finish centering
		} else if (System.currentTimeMillis() - markPlacingStarted < 1500) {
			drive.arcadeDrive(0, 0); // Stabilize
			return;
		} else if (System.currentTimeMillis() - markPlacingStarted < 4000) {
			lifter.set(Value.kReverse); // Lower arm
			return;
		} else if (System.currentTimeMillis() - markPlacingStarted < 5000) {
			grip.set(Value.kForward); // Ungrip box
			return;
		} else if (System.currentTimeMillis() - markPlacingStarted < 5200) {
			drive.arcadeDrive(1, 0); // Go back a little
			return;
		} else if (System.currentTimeMillis() - markPlacingStarted < 8200) {
			drive.arcadeDrive(0, 0);
			lifter.set(Value.kForward); // Lift arm
			return;
		} else if (System.currentTimeMillis() - markPlacingStarted < 9000) {
			state = 0;
			return;
		}

		// Check if mark is found, and search if not
		if (!markFound && SmartDashboard.getBoolean("MarkFound", false)) {
			markFound = true;
			findMarkInit();
			markInitStarted -= 1000; // Skip delay
			return;
		} else if (!SmartDashboard.getBoolean("MarkFound", false)) {
			markFound = false;
			drive.arcadeDrive(0, 0.7);
			return;
		}
		
		if (centerMark())
			markPlacingStarted = System.currentTimeMillis();

		
	}
	
	private boolean centerBox() {
		boolean output = false;
		
		double x = getBoxX();
		double area = getBoxArea();
		if ((x > 0 ? 1 : -1) != boxLastRotationSign) {
			boxRotationPasses += 0.1;
		}

		if ((area > 0 ? 1 : -1) != boxLastMovementSign) {
			boxMovementPasses += 0.1;
		}

		if (x > 0.1)
			drive.arcadeDrive(0, -0.6 / boxRotationPasses);
		else if (x < -0.1)
			drive.arcadeDrive(0, 0.6 / boxRotationPasses);
		else {
			if (area > 0.5)
				drive.arcadeDrive(0.6 / boxMovementPasses, 0);
			else if (area < -0.5)
				drive.arcadeDrive(-0.6 / boxMovementPasses, 0);
			else {
				output = true;
				drive.arcadeDrive(0, 0);
			}
		}

		boxLastRotationSign = x > 0 ? 1 : -1;
		boxLastMovementSign = area > 0 ? 1 : -1;
		return output;
	}
	
	private boolean centerMark() {
		boolean output = false;
		
		double x = getMarkX();
		double y = getMarkY();
		if ((x > 0 ? 1 : -1) != markLastRotationSign) {
			markRotationPasses += 0.1;
		}

		if ((y > 0 ? 1 : -1) != markLastMovementSign) {
			markMovementPasses += 0.1;
		}

		if (x > 0.15)
			drive.arcadeDrive(0, Math.min(-0.7 / markRotationPasses, -0.5));
		else if (x < -0.15)
			drive.arcadeDrive(0, Math.max(0.7 / markRotationPasses, 0.5));
		else {
			if (y > 0.6)
				drive.arcadeDrive(0.6 / markMovementPasses, 0);
			else if (y < -0.6)
				drive.arcadeDrive(-0.6 / markMovementPasses, 0);
			else {
				output = true;
				drive.arcadeDrive(0, 0);
			}
		}

		markLastRotationSign = x > 0 ? 1 : -1;
		markLastMovementSign = y > 0 ? 1 : -1;
		return output;
	}

	public void init() {
		findBoxInit();
	}

	public void periodic() {
		SmartDashboard.putNumber("BoxXProcessed", getBoxX());
		SmartDashboard.putNumber("BoxAreaProcessed", getBoxArea());
		SmartDashboard.putNumber("MarkXProcessed", getMarkX());
		SmartDashboard.putNumber("MarkYProcessed", getMarkY());
		
		if (state == 1) { // Finding box
			findBoxPeriodic();
		} else if (state == 2) { // Finding mark
			findMarkPeriodic();
		}
		
		
	}
}

Robot.java

package org.usfirst.frc.team1573.robot;

import java.io.IOException;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;

/**
 * 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 drive = new RobotDrive(0, 1);
	Compressor comp = new Compressor();
	Joystick stick = new Joystick(0);
	DoubleSolenoid grip = new DoubleSolenoid(0, 1);
	DoubleSolenoid lifter = new DoubleSolenoid(2, 3);

    CameraServer server;
    
    AutonomousProcedure auto = new AutonomousProcedure(grip, lifter, drive);
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        System.out.println("TableBot ready to rock!");
        
        // DISABLE AUTOFOCUS
        
			try {
				Runtime.getRuntime().exec("sudo v4l2-ctl -c focus_auto=0 > /home/lvuser/cmd.log 2>&1");
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		
        
        server = CameraServer.getInstance();
        server.setQuality(50);
        //the camera name (ex "cam0") can be found through the roborio web interface
        server.startAutomaticCapture("cam1");
        //comp.stop();
    }
    
	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the getString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the switch structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
    public void autonomousInit() {
    	drive.setSafetyEnabled(false);
    	auto.init();
    }
    
    public void disabledInit() {
    	
    }
    
    public void disabledPeriodic() {
    	double imgWidth = SmartDashboard.getNumber("imgWidth", 1);
    	double x = 2 * (SmartDashboard.getNumber("BoxX", 0) + SmartDashboard.getNumber("BoxWidth", 0)/2) / imgWidth - 1;
    	double area = Math.max(Math.min((SmartDashboard.getNumber("BoxArea", 0) - 15500) / 1500, 1), -1);
		SmartDashboard.putNumber("BoxXProcessed", x);
		SmartDashboard.putNumber("BoxAreaProcessed", area);
    }
    
    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
    	auto.periodic();
    }
    
    public void teleopInit() {
    	drive.setSafetyEnabled(true);
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
    	drive.arcadeDrive(stick.getRawAxis(1), stick.getRawAxis(0) * -1);
    	lifter.set(stick.getRawButton(1) ? Value.kForward : Value.kReverse);
    	grip.set(stick.getRawButton(2) ? Value.kForward : Value.kReverse);
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
    
    }
    
}