Fly by angle

build.gradle

@@ -50,7 +50,6 @@ dependencies {
     nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
     nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
     testCompile 'junit:junit:4.12'
-    compile 'com.2train395.limelight.api:limelight-api:1.0.0'
 }
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')

src/main/java/frc/robot/OI.java

@@ -6,9 +6,8 @@
 import edu.wpi.first.wpilibj.buttons.Button;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.buttons.Trigger;
-import frc.robot.commands.*;
-import frc.robot.triggers.*;
-import frc.robot.commands.ElevatorPreset.PresetHeight;;
+import frc.robot.commands.ApproachTarget;
+import frc.robot.enums.TargetType;
 
 public class OI {
   Joystick leftJoystick = new Joystick(0);
@@ -27,13 +26,13 @@ public class OI {
   static final double xboxDeadzone = 0.25;
 
   public void setUpTriggers() {
-        elevatorTrigger = new ElevatorTrigger();
-        elevatorTrigger.whenActive(new ElevatorJoystick());
-        high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight));
-        medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium));
-        low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow));
-        stick.whenPressed(new ElevatorPreset(PresetHeight.kZero));
-        climbMode.whileHeld(new LevelRobot());
+        // elevatorTrigger = new ElevatorTrigger();
+        // elevatorTrigger.whenActive(new ElevatorJoystick());
+        // high.whenPressed(new ElevatorPreset(PresetHeight.kMaxHeight));
+        // medium.whenPressed(new ElevatorPreset(PresetHeight.kCargoMedium));
+        // low.whenPressed(new ElevatorPreset(PresetHeight.kCargoLow));
+        // stick.whenPressed(new ElevatorPreset(PresetHeight.kZero));
+        climbMode.whenPressed(new ApproachTarget(TargetType.kLowTarget));
     }
 
   private double getJoyY(Joystick stick) {

src/main/java/frc/robot/Robot.java

@@ -9,6 +9,8 @@
 import frc.robot.subsystems.DrivetrainGyro;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.RollerIntake;
+import frc.robot.utils.limelight.Corners;
+import frc.robot.utils.limelight.Limelight;
 
 /**
 * The VM is configured to automatically run this class, and to call the
@@ -87,12 +89,15 @@ public void teleopInit() {
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+    Corners corners = Limelight.getContourCorners();
+    if(corners.validCorners)
+      SmartDashboard.putBoolean("Left", corners.topRight.y > corners.topLeft.y);
   }
-  
-  /**
-  * This function is called periodically during test mode.
-  */
-  @Override
-  public void testPeriodic() {
-  }
+	
+	/**
+	* This function is called periodically during test mode.
+	*/
+	@Override
+	public void testPeriodic() {
+	}
 }

src/main/java/frc/robot/commands/AimAtOffset.java

@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.utils.limelight.Contour;
+import frc.robot.Robot;
+import frc.robot.utils.limelight.*;
+import frc.robot.enums.TargetType;
+
+public class AimAtOffset extends Command {
+
+	TargetType targetType;
+	Contour contour;
+
+	//Full power at 15 degrees
+	double p = 1.0/50;
+	//We want to overshoot/not settle this turn
+	double d = p * 10; 
+	double maxOutput = 0.3;
+	//We only need this to roughly align, this gives time to either stop early or accelerate past
+	double acceptableErrorDegrees = 5;
+
+	//Initalize a PIDController with a 10 ms period
+	PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput(), 0.01);
+
+	public AimAtOffset(TargetType targetType) {
+		requires(Robot.drivetrain);
+		setInterruptible(false);
+
+		this.targetType = targetType;
+		pidController.setOutputRange(-maxOutput, maxOutput);
+	}
+
+	// Called just before this Command runs the first time
+	@Override
+	protected void initialize() {
+		contour = Limelight.getBestContour();
+
+		if(contour != null) {
+			Corners corners = Limelight.getContourCorners();
+
+			double totalOffset = HeadingOffsetCalculator.calculateTotalOffset(contour, corners, targetType);
+			pidController.setSetpoint(Robot.gyro.getYaw() + totalOffset);
+			pidController.enable();
+		} 
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	@Override
+	protected void execute() {
+
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	@Override
+	protected boolean isFinished() {
+		//Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all
+		return Math.abs(pidController.getError()) < acceptableErrorDegrees || contour == null;
+	}
+
+	// Called once after isFinished returns true
+	@Override
+	protected void end() {
+		pidController.disable();
+		Robot.drivetrain.tankDrive(0,0);
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	@Override
+	protected void interrupted() {
+		end();
+	}
+}

src/main/java/frc/robot/commands/AimAtOffset.java~df54caca7c2c7f198fa8a60ba76eb01d56961919

@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.commands.DriveToTarget.Side;
+import frc.robot.utils.limelight.Contour;
+import frc.robot.Robot;
+import frc.robot.utils.limelight.*;
+
+public class AimAtOffset extends Command {
+  static final double aggressiveSkewThreshold = 100000; //TODO: Tune
+  static final double maxOffsetAggressive = 20; //TODO: Tune
+  static final double maxOffsetNormal = 15;
+  static final double maxOffsetDistance = 6.0;
+
+  Contour contour;
+  Side side;
+  final double targetHeight;
+  double initialSkewAbs;
+  double xOffset;
+  double distance;
+
+  static final double cameraHeight = 8.375;
+  static final double cameraAngle = 30;
+
+  static final double lowTargetHeight = 28.337;
+  static final double highTargetHeight = 35.962;
+  //Full power at 15 degrees
+  double p = 1.0/50;
+  //We want to overshoot/not settle this turn
+  double d = p * 10; 
+  double maxOutput = 0.3;
+  //We only need this to roughly align, this gives time to either stop early or accelerate past
+  double acceptableErrorDegrees = 5;
+
+  //Initalize a PIDController with a 10 ms period
+  PIDController pidController = new PIDController(p, 0, d, Robot.gyro.getYawSource(), Robot.drivetrain.getTurnOutput(), 0.01);
+
+  public AimAtOffset() {
+     requires(Robot.drivetrain);
+     targetHeight = lowTargetHeight;
+  }
+
+   // Called just before this Command runs the first time
+   @Override
+   protected void initialize() {
+    contour = Limelight.getBestContour();
+    if(contour != null) {
+      Corners corners = Limelight.getContourCorners();
+      if(corners.validCorners) {
+          side = corners.topRight.y > corners.topLeft.y ? Side.kRight : Side.kLeft;            
+      }
+
+      double yOffset = contour.ty;
+      xOffset = contour.tx;
+
+      distance = (targetHeight - cameraHeight) / 
+      Math.tan(Math.toRadians(cameraAngle + yOffset)) / 12;
+
+      double maxOffset = initialSkewAbs > aggressiveSkewThreshold ? maxOffsetAggressive : maxOffsetNormal;
+      double additionalOffsetSign = side == Side.kLeft ? -1 : 1;
+      double distanceScaling = Math.min(1, (distance * distance) / (maxOffsetDistance * maxOffsetDistance));
+
+      double additionalOffset = maxOffset * distanceScaling * additionalOffsetSign;
+
+      pidController.setOutputRange(-maxOutput, maxOutput);
+
+      pidController.setSetpoint(Robot.gyro.getYaw() - (xOffset + additionalOffset));		
+      pidController.enable();
+    } 
+   }
+
+   // Called repeatedly when this Command is scheduled to run
+   @Override
+   protected void execute() {
+   }
+
+   // Make this return true when this Command no longer needs to run execute()
+   @Override
+   protected boolean isFinished() {
+     //Not using onTarget as that may do stability checking, this should end as soon as we hit the range at all
+     return Math.abs(pidController.getError()) < acceptableErrorDegrees ||
+       contour == null;
+   }
+
+   // Called once after isFinished returns true
+   @Override
+   protected void end() {
+     pidController.disable();
+     Robot.drivetrain.tankDrive(0,0);
+   }
+
+   // Called when another command which requires one or more of the same
+   // subsystems is scheduled to run
+   @Override
+   protected void interrupted() {
+     end();
+  }
+}

src/main/java/frc/robot/commands/ApproachTarget.java

@@ -0,0 +1,13 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import frc.robot.enums.TargetType;
+
+public class ApproachTarget extends CommandGroup {
+
+  public ApproachTarget(TargetType targetType) {
+    addSequential(new AimAtOffset(targetType));
+    addSequential(new DriveToTarget(targetType));
+  }
+
+}

src/main/java/frc/robot/commands/DriveToTarget.java

@@ -0,0 +1,111 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.Robot;
+import frc.robot.subsystems.Drivetrain.Gear;
+import frc.robot.utils.LinearOutput;
+import frc.robot.utils.limelight.Contour;
+import frc.robot.utils.limelight.Corners;
+import frc.robot.utils.limelight.HeadingOffsetCalculator;
+import frc.robot.utils.limelight.Limelight;
+import frc.robot.enums.TargetType;
+
+/**
+ * A command designed to approach a target perpendicularly which the robot is already pointed at.
+ * All units are in feet and degrees unless otherwise notated.
+ */
+public class DriveToTarget extends Command {
+
+    //The amount of time the command will continue to run without seeing a contour in seconds.
+    static final double maxTimeWithoutContour = 0.30;
+    //A double variable holding the last time a valid contour was seen.
+    double contourLastSeenTime;
+
+    //The PID coefficients of the linear drive.
+    static final double p = 0.075;
+    static final double i = 0;
+    static final double d = 0;
+    static final double maxOutput = 0.3;
+    static final double minOutput = 0.15;
+
+    //The proportional constant for the angle adjustment 
+    static final double rotationP = -0.005;
+
+    //The PID controller and PIDOutput. We maintain a reference to linearOutput to set the heading correction.
+    LinearOutput linearOutput = Robot.drivetrain.getLinearOutput();
+    PIDController pidController = new PIDController(p, i, d, Robot.encoders, linearOutput);
+
+    TargetType targetType;
+
+    Contour contour;
+
+    public DriveToTarget(TargetType targetType) {
+        requires(Robot.drivetrain);
+        setInterruptible(false);
+
+        this.targetType = targetType;
+
+        pidController.setOutputRange(minOutput, maxOutput);
+    }
+
+
+
+    // Called just before this Command runs the first time
+    @Override
+    protected void initialize() {
+        //Drive slow in 
+        Robot.drivetrain.shift(Gear.kLow);
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    @Override
+    protected void execute() {    
+        contour = Limelight.getBestContour();
+
+        if(contour != null) {
+            contourLastSeenTime = Timer.getFPGATimestamp();            
+
+            Corners corners = Limelight.getContourCorners();
+
+            //Set the setpoint for distance relative to current position
+            pidController.setSetpoint(
+                Robot.encoders.getAveragedEncoderFeet() + 
+                HeadingOffsetCalculator.calculateDistance(contour, targetType)
+            );
+
+            linearOutput.setHeadingCorrection(
+                rotationP * HeadingOffsetCalculator.calculateTotalOffset(contour, corners, targetType)
+            );
+        } else {
+            linearOutput.setHeadingCorrection(0);
+        }
+
+        if(!pidController.isEnabled()){
+            pidController.enable();
+        }
+
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    @Override
+    protected boolean isFinished() {
+        return Timer.getFPGATimestamp() - contourLastSeenTime > maxTimeWithoutContour;
+    }
+
+    // Called once after isFinished returns true
+    @Override
+    protected void end() {
+        Robot.drivetrain.tankDrive(0, 0);
+        pidController.disable();
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    @Override
+    protected void interrupted() {
+        end();
+    }
+
+}