Implementing Limelight for Vision Tracking

PowerUp2018/.gitignore

@@ -1 +1,3 @@
 /bin/
+/build/
+/dist/

PowerUp2018/src/org/usfirst/frc/team6351/robot/Robot.java

@@ -10,6 +10,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import org.usfirst.frc.team6351.robot.commands.Auto_DriveStraight;
+import org.usfirst.frc.team6351.robot.commands.Auto_DriveToCrosshairs;
+import org.usfirst.frc.team6351.robot.commands.Auto_TurnToCrosshairs;
 import org.usfirst.frc.team6351.robot.commands.FlightStickDrive;
 import org.usfirst.frc.team6351.robot.commands.GTADrive;
 import org.usfirst.frc.team6351.robot.commands.GyroTurnToAngle;
@@ -38,6 +40,7 @@ public class Robot extends TimedRobot {
 	public static double targetX;
 	public static double targetY;
 	public static double targetArea;
+	public static double targetsVisible;
 
 	public static String fms_gameData;
 	boolean fms_dataFound = false;
@@ -53,6 +56,8 @@ public void robotInit() {
 		// chooser.addObject("My Auto", new MyAutoCommand());
 		m_autonomousChooser.addObject("Drive Straight", new Auto_DriveStraight(0.5,2));
 		m_autonomousChooser.addDefault("Turn 90 Degrees", new GyroTurnToAngle(90));
+		m_autonomousChooser.addDefault("Turn To Target", new Auto_TurnToCrosshairs());
+		m_autonomousChooser.addDefault("Drive To Target", new Auto_DriveToCrosshairs());
 		SmartDashboard.putData("Auto mode", m_autonomousChooser);
 
 //	    driveMode.addObject("Flight Stick Control", new FlightStickDrive());
@@ -75,6 +80,7 @@ public void disabledInit() {
 	@Override
 	public void disabledPeriodic() {
 		getFMSData();
+		getLimeLight();	
 		Scheduler.getInstance().run();
 	}
 
@@ -153,6 +159,7 @@ public void getLimeLight() {
 		targetX = limelight.getEntry("tx").getDouble(0);
 		targetY = limelight.getEntry("ty").getDouble(0);
 		targetArea = limelight.getEntry("ta").getDouble(0);
+		targetsVisible = limelight.getEntry("tv").getDouble(0);
 	}
 
 	public void getFMSData() {

PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_DriveToCrosshairs.java

@@ -0,0 +1,98 @@
+package org.usfirst.frc.team6351.robot.commands;
+
+import org.usfirst.frc.team6351.robot.Robot;
+import org.usfirst.frc.team6351.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class Auto_DriveToCrosshairs extends Command {
+
+	double targetX;
+	double targetY;
+	double targetArea;
+	double targetsVisible;
+   	double leftMotors;
+   	double rightMotors;
+
+   	double xChange;
+   	double yChange;
+
+    public Auto_DriveToCrosshairs() {
+        // 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() {
+    	targetX = Robot.targetX;
+    	targetY = Robot.targetY;
+    	targetArea = Robot.targetArea;
+    	targetsVisible = Robot.targetsVisible;
+
+       	leftMotors = 0;
+       	rightMotors = 0;
+
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+       	targetX = Robot.targetX;
+       	targetY = Robot.targetY;
+       	targetsVisible = Robot.targetsVisible;
+
+    	double error = Math.abs(targetX);
+    	double kP;
+    	if (error < 3.0) {
+    		kP = Math.pow((0.10*error), 2)+0.2;
+    	} else {
+    		kP = 1;
+    	}
+
+       	if (targetsVisible == 1) {
+       		if (targetY < -2) {
+       			//Too Close. Go Backwards
+        		yChange =+ RobotMap.Drive_Scaling_Auto*(-1);
+       		} else if (targetY > 2) {
+       			//Too Far. Go Forwards
+       			yChange =+ RobotMap.Drive_Scaling_Auto;
+       		} else {
+       		}
+
+       		if (targetX < -5) {
+       			//Turn Right
+        		rightMotors =+ 0.2*(-1);
+
+
+       		} else if (targetX > 5) {
+       			//Turn Left
+       			leftMotors =+ 0.2;
+
+       		}
+       	} else {
+       		DriverStation.reportError("Vision Auto: No Targets Found", false);
+       	}
+
+       	Robot.driveTrain.setLeft(leftMotors);
+       	Robot.driveTrain.setRight(rightMotors);
+
+    }
+
+    // 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() {
+    }
+}

PowerUp2018/src/org/usfirst/frc/team6351/robot/commands/Auto_TurnToCrosshairs.java

@@ -0,0 +1,67 @@
+package org.usfirst.frc.team6351.robot.commands;
+
+import org.usfirst.frc.team6351.robot.Robot;
+import org.usfirst.frc.team6351.robot.RobotMap;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ *
+ */
+public class Auto_TurnToCrosshairs extends Command {
+
+	double targetX;
+	double targetsVisible;
+
+    public Auto_TurnToCrosshairs() {
+        // 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() {
+    	targetX = Robot.targetX;
+    	targetsVisible = Robot.targetsVisible;
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+       	targetX = Robot.targetX;
+       	targetsVisible = Robot.targetsVisible;
+
+       	if (targetsVisible == 1) {
+    	double error = Math.abs(targetX);
+    	double kP;
+    	if (error < 10.0) {
+    		kP = Math.pow((0.10*error), 2)+0.2;
+    	} else {
+    		kP = 1;
+    	}
+    	if (targetX < -3){
+    		Robot.driveTrain.setLeft(RobotMap.Drive_Scaling_Auto*(-1)*kP);
+    		Robot.driveTrain.setRight(RobotMap.Drive_Scaling_Auto*(-1)*kP);
+    	} else if (targetX > 3) {
+    		Robot.driveTrain.setLeft(RobotMap.Drive_Scaling_Auto*kP);
+    		Robot.driveTrain.setRight(RobotMap.Drive_Scaling_Auto*kP);
+    	}
+       	} else {
+       		DriverStation.reportError("Vision Auto: No Targets Found", false);
+       	}
+    }
+
+    // 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() {
+    }
+}