Limelight

src/main/java/frc/robot/RobotContainer.java

@@ -15,9 +15,10 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.VisionCode;
+import frc.robot.commands.LimeCheckDistance;
 import frc.robot.subsystems.GearShiftSubsystem;
 import frc.robot.subsystems.IRSensor;
+import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.Motor;
 import frc.robot.subsystems.PneumaticsSubsystem;
 import frc.robot.subsystems.TankDrive;
@@ -27,17 +28,7 @@
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 
-import frc.robot.commands.MoveDistance;
-import frc.robot.commands.MoveMotor;
-import frc.robot.commands.gearshift.GearShiftCommand;
-import frc.robot.commands.pneumatics.BothDownCommand;
-import frc.robot.commands.pneumatics.BothUpCommand;
-import frc.robot.commands.pneumatics.LeftDownCommand;
-import frc.robot.commands.pneumatics.LeftOffCommand;
-import frc.robot.commands.pneumatics.LeftUpCommand;
-import frc.robot.commands.pneumatics.RightDownCommand;
-import frc.robot.commands.pneumatics.RightOffCommand;
-import frc.robot.commands.pneumatics.RightUpCommand;
+import frc.robot.commands.ToggleLimelight;
 
 
 
@@ -49,8 +40,13 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-  private final VisionCode vision = new VisionCode();
+
+  //public static final WPI_TalonSRX limedrive = new WPI_TalonSRX(6);
+
+  public static final Limelight limelight = new Limelight();
+
   private final ExampleCommand m_autoCommand = new ExampleCommand();
+
   private final Motor motorSub = new Motor();
 
   public static IRSensor sensor = new IRSensor();
@@ -85,6 +81,7 @@ public class RobotContainer {
 
 
   public static final Joystick stick = new Joystick(0);
+  public static final Joystick stick2 = new Joystick (1);
 
   //public static final JoystickButton tankDriveButton = new JoystickButton(stick, 2);//change back to 5
 
@@ -95,6 +92,7 @@ public class RobotContainer {
 
   public static JoystickButton intakeButton = new JoystickButton(stick,2);
 
+  public static JoystickButton turnOnLimelight = new JoystickButton(stick,3);
 
   public static JoystickButton LeftUp = new JoystickButton(stick,7);
   public static JoystickButton LeftOff = new JoystickButton(stick,9);
@@ -105,6 +103,8 @@ public class RobotContainer {
   public static JoystickButton RightDown = new JoystickButton(stick,12);
 
 
+
+
   //public static JoystickButton BothUp = new JoystickButton(stick,9);
   //public static JoystickButton BothDown = new JoystickButton(stick,11);
 
@@ -116,11 +116,12 @@ public class RobotContainer {
 
   public static final DifferentialDrive difDrive = new DifferentialDrive(leftSide, rightSide);
 
-
+  
   /**
    * The container for the robot.  Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
+    /*
     RobotContainer.frontLeft.follow(RobotContainer.middleLeft);
     RobotContainer.rearLeft.follow(RobotContainer.middleLeft);
 
@@ -134,7 +135,9 @@ public RobotContainer() {
     RobotContainer.rearRight.setInverted(true);
 
     // Configure the button bindings
+    */
     configureButtonBindings();
+
   }
 
   /**
@@ -143,9 +146,19 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
+
+  // Objects for Lime light
+
+  public static JoystickButton check = new JoystickButton(stick,3);
+  public static JoystickButton alignButton = new JoystickButton(stick,4);
+
   private void configureButtonBindings() {
-   // tankDriveButton.whenPressed(new MoveDistance(tankDriveSubsystem, 36));
+    turnOnLimelight.whenPressed(new ToggleLimelight(limelight));
+    check.whenPressed(new LimeCheckDistance(limelight));
+
 
+    // tankDriveButton.whenPressed(new MoveDistance(tankDriveSubsystem, 36));
+    /*
     LeftUp.whenPressed(new LeftUpCommand(m_pneumaticssubsytem), true);
     LeftOff.whenPressed(new LeftOffCommand(m_pneumaticssubsytem), true);
     LeftDown.whenPressed(new LeftDownCommand(m_pneumaticssubsytem), true);
@@ -160,6 +173,7 @@ private void configureButtonBindings() {
 
     //BothUp.whileHeld(new BothUpCommand(m_pneumaticssubsytem));
     //BothDown.whileHeld(new BothDownCommand(m_pneumaticssubsytem));
+    */
   }
 
 

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

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* 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.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Limelight;
+import frc.robot.subsystems.TankDrive;
+
+public class Align extends CommandBase {
+    public static Limelight limelightsubsytem;
+    public static TankDrive tankdrivesubsystem;
+
+  public Align(Limelight sub,TankDrive subtwo) {
+    // Use requires() here to declare subsystem dependencies
+    // eg. requires(chassis);
+    limelightsubsytem = sub;
+    tankdrivesubsystem = subtwo;
+    addRequirements(limelightsubsytem); 
+    addRequirements(tankdrivesubsystem); 
+
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  public void execute() {
+
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() { //write comments!
+
+    double offset = limelightsubsytem.getXOffset();
+    if (offset < -1.5){
+      tankdrivesubsystem.limelightAlign();
+      //tankdrivesubsytem.turn(false);
+    } else if(offset > 1.5){
+
+      tankdrivesubsystem.limelightAlign();
+      //tankdrivesubsytem.turn(true);
+    } else {
+      tankdrivesubsystem.stop();
+      return true;
+    }
+
+
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  protected void end() {
+    tankdrivesubsystem.stop();
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  protected void interrupted() {
+  }
+}
+

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

@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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.smartdashboard.SmartDashboard;
+//import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Limelight;
+
+public class LimeCheckDistance extends CommandBase {
+
+  public final Limelight limelight;
+
+  public LimeCheckDistance(Limelight xlimelight) {
+      limelight = xlimelight;
+  }
+
+  // Called just before this Command runs the first time
+  @Override
+  public void initialize() {
+
+
+
+  }
+
+  // Called repeatedly when this Command is scheduled to run
+  @Override
+  public void execute() {
+
+    int x = 1;
+    int z = 0;
+    if (limelight.getXOffset() > 0.5 || limelight.getXOffset()<0.5){
+      //RobotContainer.limedrive.set(0.5);
+      SmartDashboard.putNumber("XOffset", x);
+    }
+    else{
+      //RobotContainer.limedrive.set(0);
+      SmartDashboard.putNumber("Xoffset", z);
+    }
+  }
+
+  // Make this return true when this Command no longer needs to run execute()
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+  // Called once after isFinished returns true
+  public void end() {
+  }
+
+  // Called when another command which requires one or more of the same
+  // subsystems is scheduled to run
+  public void interrupted() {
+  }
+}

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

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Limelight;
+
+public class ToggleLimelight extends CommandBase {
+  /**
+   * Creates a new TurnOnLimelight.
+   */
+  public final Limelight limelight;
+  public ToggleLimelight(Limelight pLimelight) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    limelight = pLimelight;
+    addRequirements(limelight);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    limelight.switchPipelines(1);
+    limelight.toggle();
+
+  }
+
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/Limelight.java

@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 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.subsystems;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Limelight extends SubsystemBase {
+  /**
+   * Creates a new Limlight.
+   */
+  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+
+  public Limelight() {
+  }
+
+  public void switchPipelines(int pipline){
+    table.getEntry("pipeline").setNumber(pipline);
+
+  }
+  public void toggle (){
+    if (table.getEntry("ledMode").getDouble(0)== 1)
+    {
+        table.getEntry("ledMode").setNumber(3);
+    }
+    else
+    {
+        table.getEntry("ledMode").setNumber(1);
+    }
+  }
+  public double getXOffset(){
+    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+    NetworkTableEntry tx = table.getEntry("tx");
+
+    return tx.getDouble(0);
+
+  }
+
+
+
+
+  public void display(){
+
+
+
+    NetworkTableEntry tx = table.getEntry("tx");
+    NetworkTableEntry ty = table.getEntry("ty");
+    NetworkTableEntry ta = table.getEntry("ta");
+    NetworkTableEntry ts = table.getEntry("ts");
+
+
+    //read values periodically
+    double x = tx.getDouble(0.0);
+    double y = ty.getDouble(0.0);
+    double targetArea = ta.getDouble(0.0);
+    double skew = ts.getDouble(0.0);
+
+
+    //post to smart dashboard periodically
+    SmartDashboard.putNumber("LimelightX", x);
+    SmartDashboard.putNumber("LimelightY", y);
+    SmartDashboard.putNumber("Target Area", targetArea);
+    SmartDashboard.putNumber("Skew", skew);
+
+  }
+
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    display();
+  }
+}

src/main/java/frc/robot/subsystems/Motor.java

@@ -15,7 +15,7 @@ public class Motor extends SubsystemBase {
   /**
    * Creates a new Motor.
    */
-  private final WPI_TalonSRX motor = new WPI_TalonSRX(4);
+  private final WPI_TalonSRX motor = new WPI_TalonSRX(6);
   public Motor() {
 
   }