Limelight2.0

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.1.1"
+    id "edu.wpi.first.GradleRIO" version "2023.2.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/frc/robot/Constants.java

@@ -19,7 +19,7 @@
 public final class Constants {
 
   public static class OperatorConstants {
-    public static final int DRIVER_CONTROLLER_PORT = 0;
+    public static final int DRIVER_CONTROLLER_PORT = 1;
     public static final int XBOX_LEFT_Y_AXIS = 1;
     public static final int XBOX_RIGHT_Y_AXIS = 5;
     public static final int XBOX_LEFT_X_AXIS = 0;

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

@@ -5,7 +5,9 @@
 package frc.robot;
 
 import frc.robot.Constants.OperatorConstants;
+import frc.robot.commands.LimelightFollow;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Limelight;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -21,18 +23,21 @@
  */
 public class RobotContainer {
 	// Replace with CommandPS4Controller or CommandJoystick if needed
-	private final CommandXboxController driverController = new CommandXboxController(
-			OperatorConstants.DRIVER_CONTROLLER_PORT);
+	public static final CommandXboxController driverController = new CommandXboxController(
+		OperatorConstants.DRIVER_CONTROLLER_PORT);
 
 	// The robot's subsystems and commands are defined here...
 	private final Drivetrain drivetrain = new Drivetrain(driverController);
+	private final Limelight limelight = new Limelight();
+
 
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
 	public RobotContainer() {
 		// Configure the trigger bindings
 		configureBindings();
+
 	}
 
 	/**
@@ -50,6 +55,8 @@ public RobotContainer() {
 	 * joysticks}.
 	 */
 	private void configureBindings() {
+
+		driverController.b().whileTrue(new LimelightFollow(drivetrain, limelight));
 		// Example: Schedule `ExampleCommand` when `exampleCondition` changes to `true`
 		// new Trigger(drivetrain::exampleCondition)
 		// .onTrue(new ExampleCommand(m_exampleSubsystem));

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

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Limelight;
+
+
+public class LimelightFollow extends CommandBase {
+  /** Creates a new LimelightFollow. */
+
+
+  private Drivetrain drivetrain;
+  private Limelight limelight;
+
+
+
+
+  public LimelightFollow(Drivetrain drivetrain, Limelight limelight) {
+    this.drivetrain = drivetrain;
+    this.limelight = limelight;
+
+    addRequirements(drivetrain);
+    addRequirements(limelight);
+
+
+
+
+
+  }
+
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    Limelight.enableLimelight();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+
+
+
+
+
+
+
+
+         if (limelight.getTV() == 0){
+          drivetrain.setSpeeds(0, .5);
+
+
+         }
+         else if (Math.abs(limelight.getTX()) > 1){
+          drivetrain.setSpeeds(0,limelight.getTX() / Math.abs(limelight.getTX()) * 0.5);
+
+
+        }
+
+
+
+
+
+
+  }
+
+
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+
+    limelight.disableLimelight();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+
+  }
+}

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

@@ -4,86 +4,66 @@
 
 package frc.robot.subsystems;
 
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
+
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.ControlConstants;
 import frc.robot.Constants.OperatorConstants;
 
+
 public class Drivetrain extends SubsystemBase {
 
 	private MotorController frontLeft;
 	private MotorController frontRight;
 	private MotorController backLeft;
 	private MotorController backRight;
 	private CommandXboxController controller;
+
+	private MotorControllerGroup leftMotors;
+	private MotorControllerGroup rightMotors;
+	private DifferentialDrive drive;
 
 	public Drivetrain(CommandXboxController controller) {
 		this.controller = controller;
 
-		frontLeft = new CANSparkMax(0, MotorType.kBrushless);
-		frontRight = new CANSparkMax(1, MotorType.kBrushless);
-		backLeft = new CANSparkMax(2, MotorType.kBrushless);
-		backRight = new CANSparkMax(3, MotorType.kBrushless);
+		frontLeft = new Spark(0);
+		frontRight = new Spark(2);
+		backLeft = new Spark(1);
+		backRight = new Spark(3);
+
+		leftMotors = new MotorControllerGroup(frontLeft,backLeft);
+		rightMotors = new MotorControllerGroup(frontRight, backRight);
+
+		rightMotors.setInverted(true);
+
+		drive = new DifferentialDrive(leftMotors, rightMotors);
 
-		frontRight.setInverted(true);
-		backRight.setInverted(true);
 	}
 
 	@Override
 	public void periodic() {
-		double joyX = controller.getRawAxis(OperatorConstants.XBOX_LEFT_X_AXIS);
-		double joyY = -controller.getRawAxis(OperatorConstants.XBOX_LEFT_Y_AXIS);
-		double rotation = ControlConstants.ROTATION_MULT
-				* (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis());
+		double move = controller.getLeftTriggerAxis();
+		double turn = controller.getLeftX();
 
-		mecanumDrive(joyX, joyY, rotation);
+		drive.arcadeDrive(move, turn);
 	}
 
-	// 2020 mecanum drive code
-	public void mecanumDrive(double joystickX, double joystickY, double rotation) {
-		// Deadband inputs
-		if (Math.abs(rotation) < ControlConstants.ROTATION_DEADBAND)
-			rotation = 0;
-		if (Math.abs(joystickX) < ControlConstants.JOY_DEADBAND)
-			joystickX = 0;
-		if (Math.abs(joystickY) < ControlConstants.JOY_DEADBAND)
-			joystickY = 0;
-
-		// Cap rotation to ControlConstants value
-		if (Math.abs(rotation) > ControlConstants.MAX_TURN_SPEED)
-			rotation *= ControlConstants.MAX_TURN_SPEED / Math.abs(rotation);
-
-		// Calculate speed for each wheel
-		double frontRightPower = joystickY - joystickX - rotation;
-		double frontLeftPower = joystickY + joystickX + rotation;
-		double backLeftPower = joystickY - joystickX + rotation;
-		double backRightPower = joystickY + joystickX - rotation;
-
-		// Cap motor powers
-		if (Math.abs(frontLeftPower) > ControlConstants.MAX_ROBOT_SPEED)
-			frontLeftPower *= ControlConstants.MAX_ROBOT_SPEED / Math.abs(frontLeftPower);
-		if (Math.abs(frontRightPower) > ControlConstants.MAX_ROBOT_SPEED)
-			frontRightPower *= ControlConstants.MAX_ROBOT_SPEED / Math.abs(frontRightPower);
-		if (Math.abs(backLeftPower) > ControlConstants.MAX_ROBOT_SPEED)
-			backLeftPower *= ControlConstants.MAX_ROBOT_SPEED / Math.abs(backLeftPower);
-		if (Math.abs(backRightPower) > ControlConstants.MAX_ROBOT_SPEED)
-			backRightPower *= ControlConstants.MAX_ROBOT_SPEED / Math.abs(backRightPower);
-
-		// Power the motors
-		frontLeft.set(frontLeftPower);
-		frontRight.set(frontRightPower);
-		backLeft.set(backLeftPower);
-		backRight.set(backRightPower);
-	}
+
+
 
 	public void stop() {
 		frontLeft.set(0);
 		frontRight.set(0);
 		backLeft.set(0);
 		backRight.set(0);
 	}
+	public void setSpeeds(double forwardSpeed, double rotationSpeed){
+		drive.arcadeDrive(forwardSpeed,rotationSpeed);
+
+	}
 }

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

@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotContainer;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.XboxController;
+import frc.robot.commands.LimelightFollow;;
+public class Limelight extends SubsystemBase {
+  private static NetworkTable table;
+
+    private double tx;
+    private double ty;
+    private double ta;
+    private double apriltagid;
+
+
+  /** Creates a new Limelight. */
+  public Limelight() {
+    table = NetworkTableInstance.getDefault().getTable("limelight");
+    disableLimelight();
+
+
+  }
+
+public static void enableLimelight() {
+  table.getEntry("ledMode").setNumber(3);
+}
+public void disableLimelight() {
+  table.getEntry("ledMode").setNumber(1);
+}
+
+  @Override
+  public void periodic() {
+    tx = table.getEntry("tx").getDouble(0.0);
+    ty = table.getEntry("ty").getDouble(0.0);
+    ta = table.getEntry("ta").getDouble(0.0);
+
+
+
+
+    apriltagid = table.getEntry("tid").getDouble(0.0);
+
+    SmartDashboard.putNumber("x offset", tx);
+    SmartDashboard.putNumber("y offset", ty);
+    SmartDashboard.putNumber("area", ta);
+
+    SmartDashboard.putNumber("TId", apriltagid);
+
+    limelightCenter();
+
+
+
+  }
+  public double getTX(){
+    return table.getEntry("tx").getDouble(0.0);
+  }
+  public double getTV(){
+    return table.getEntry("tv").getDouble(0.0);
+  }
+
+
+  public void limelightCenter() {
+
+
+
+
+  }
+
+
+  }
+