package frc.robot;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
- 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 {
CANSparkMax leftFront = new CANSparkMax(10, MotorType.kBrushless);
CANSparkMax leftRear = new CANSparkMax(11, MotorType.kBrushless);
CANSparkMax rightFront = new CANSparkMax(20, MotorType.kBrushless);
CANSparkMax rightRear = new CANSparkMax(21, MotorType.kBrushless);
SpeedControllerGroup leftMotorGroup = new SpeedControllerGroup(leftFront, leftRear);
SpeedControllerGroup rightMotorGroup = new SpeedControllerGroup(rightFront, rightRear);
DifferentialDrive drive = new DifferentialDrive(leftMotorGroup, rightMotorGroup);
XboxController controller = new XboxController(0);
Timer timer = new Timer();
//LimeLight
private boolean m_LimelightHasValidTarget = false;
private double m_LimelightDriveCommand = 0.0;
private double m_LimelightSteerCommand = 0.0;
/**
- This function is run when the robot is first started up and should be
- used for any initialization code.
*/
@Override
public void robotInit() {
//ShuffleBoard
SmartDashboard.putNumber("Joystick X value", controller.getX());
SmartDashboard.putNumber("RPM", leftFront.getAppliedOutput());
leftMotorGroup.setInverted(true);
rightMotorGroup.setInverted(true);
}
/**
/**
/**
- This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
// Drive for 2 seconds
if (timer.get() < 2.0) {
drive.arcadeDrive(0.5, 0.0); // drive forwards half speed
} else {
drive.stopMotor(); // stop robot
}
}
/**
- This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
drive.tankDrive(controller.getRawAxis(1) , controller.getRawAxis(5));
//drive.arcadeDrive(controller.getRawAxis(1), controller.getRawAxis(5));
Update_Limelight_Tracking();
double left = controller.getY(Hand.kLeft);
double right = -controller.getY(Hand.kRight);
double drive = controller.getY(Hand.kLeft); //this will only be used for arcadeDrive
double steer = -controller.getX(Hand.kLeft);//this will only be used for arcadeDrive
final boolean auto = controller.getAButton();
left *= 0.70;
right *= 0.70;
drive *= 0.70; // this will only be used for arcadeDrive
steer *= 0.70; // this will only be used for arcadeDrive
if (auto) {
if (m_LimelightHasValidTarget) {
drive.tankDrive(m_LimelightDriveCommand, m_LimelightSteerCommand);
// drive.arcadeDrive(m_LimelightDriveCommand,m_LimelightSteerCommand); //this
// will only be used for arcadeDrive
} else {
drive.tankDrive(0.0, 0.0);
// drive.arcadeDrive(0.0,0.0); //this will only be used for arcadeDrive
}
} else {
drive.tankDrive(left, right);
// drive.arcadeDrive(drive,steer); //this will only be used for arcadeDrive
}
}
/**
- This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
teleopPeriodic();
autonomousPeriodic();
}
public void Update_Limelight_Tracking() {
// These numbers must be tuned for your Robot! Be careful!
final double STEER_K = 0.03; // how hard to turn toward the target
final double DRIVE_K = 0.26; // how hard to drive fwd toward the target
final double DESIRED_TARGET_AREA = 13.0; // Area of the target when the robot reaches the wall
final double MAX_DRIVE = 0.7; // Simple speed limit so we don’t drive too fast
final double tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
final double tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
final double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
final double ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0);
if (tv < 1.0) {
m_LimelightHasValidTarget = false;
m_LimelightDriveCommand = 0.0;
m_LimelightSteerCommand = 0.0;
return;
}
m_LimelightHasValidTarget = true;
// Start with proportional steering
final double steer_cmd = tx * STEER_K;
m_LimelightSteerCommand = steer_cmd;
// try to drive forward until the target area reaches our desired area
double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;
// don't let the robot drive too fast into the goal
if (drive_cmd > MAX_DRIVE) {
drive_cmd = MAX_DRIVE;
}
m_LimelightDriveCommand = drive_cmd;
}
}