Hi everyone, I just took over a team last year so this is my second year as a mentor, I have little to no coding experience and have been learning as I have been going. My team purchased a limelight this year to have our Robot line up with the high target. I have tried to use the documentation from the Limelight page (Case Study: Aiming and Range at the same time). I came up with this. Can anyone help to trouble shoot what I am doing wrong?
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj.PWMTalonSRX;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.networktables.*;
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private PWMTalonSRX leftMotor1 = new PWMTalonSRX(0);
private PWMTalonSRX leftMotor2 = new PWMTalonSRX(1);
private SpeedControllerGroup left = new SpeedControllerGroup(leftMotor1, leftMotor2);
private PWMTalonSRX rightMotor1 = new PWMTalonSRX(2);
private PWMTalonSRX rightMotor2 = new PWMTalonSRX(3);
private SpeedControllerGroup right = new SpeedControllerGroup(rightMotor1, rightMotor2);
private DifferentialDrive m_Drive = new DifferentialDrive(left, right);
private XboxController driver = new XboxController(0);
private boolean m_LimelightHasValidTarget = false;
private double m_LimelightDriveCommand = 0.0;
private double m_LimelightSteerCommand = 0.0;
private Command m_autonomousCommand;
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
//Compressor.start();
}
/**
* This function is called every robot packet, no matter the mode. Use this for items like
* diagnostics that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
CommandScheduler.getInstance().run();
}
/**
* This function is called once each time the robot enters Disabled mode.
*/
@Override
public void disabledInit() {
}
@Override
public void disabledPeriodic() {
}
/**
* This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
}
/**
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
}
/**
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
Update_Limelight_Tracking();
double steer = driver.getX(Hand.kRight);
double drive = -driver.getY(Hand.kLeft);
boolean auto = driver.getAButton();
steer *= 0.70;
drive *= 0.70;
if (auto)
{
if (m_LimelightHasValidTarget)
{
m_Drive.arcadeDrive(m_LimelightDriveCommand,m_LimelightSteerCommand);
}
else
{
m_Drive.arcadeDrive(0.0,0.0);
}
}
else
{
m_Drive.arcadeDrive(drive,steer);
}
}
public void Update_Limelight_Tracking(){
final double KpAim = -0.1;
final double KpDistance = -0.1;
final double min_aim_command = 0.05;
double tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
if (driver.getRawButtonPressed(9))
{
double heading_error = -tx;
double distance_error = -ty;
double steering_adjust = 0.0f;
if (tx > 1.0)
{
steering_adjust = KpAim*heading_error - min_aim_command;
}
else if (tx < 1.0)
{
steering_adjust = KpAim*heading_error + min_aim_command;
}
double distance_adjust = KpDistance * distance_error;
m_LimelightDriveCommand = steering_adjust + distance_adjust;
m_LimelightSteerCommand = steering_adjust + distance_adjust;
}
}
}