Skip to content

Commit

Permalink
added limelight port fowarding
Browse files Browse the repository at this point in the history
  • Loading branch information
DriverStationComputer committed Jul 20, 2024
1 parent 218bf3e commit b62942a
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 16 deletions.
11 changes: 10 additions & 1 deletion src/main/java/org/carlmontrobotics/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,12 @@

package org.carlmontrobotics;

import static org.carlmontrobotics.Constants.Limelightc.INTAKE_LL_NAME;
import static org.carlmontrobotics.Constants.Limelightc.SHOOTER_LL_NAME;

import com.pathplanner.lib.commands.FollowPathCommand;

import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
Expand All @@ -31,7 +35,12 @@ public void robotInit() {
DataLogManager.start();
DriverStation.startDataLog(DataLogManager.getLog());
FollowPathCommand.warmupCommand().schedule();

for (int port = 5800; port <= 5809; port++) {
PortForwarder.add(port, INTAKE_LL_NAME + ".local", port);
}
for (int port = 5800; port <= 5809; port++) {
PortForwarder.add(port + 10, SHOOTER_LL_NAME + ".local", port);
}
}

@Override
Expand Down
22 changes: 7 additions & 15 deletions src/main/java/org/carlmontrobotics/subsystems/Limelight.java
Original file line number Diff line number Diff line change
Expand Up @@ -87,9 +87,8 @@ public double getDistanceToSpeakerMeters() {
Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER)
.plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); // because limelight is mounted
// horizontally
double distance =
(SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER)
/ angleToGoal.getTan();
double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER)
/ angleToGoal.getTan();
// SmartDashboard.putNumber("limelight distance", distance);
return distance;
} else {
Expand All @@ -113,37 +112,30 @@ public double getDistanceToNoteMeters() {
}

public double getArmAngleToShootSpeakerRad() {
double armRestingHeightToSubwooferMeters =
HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS;
double horizontalDistanceMeters =
getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH;
double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS;
double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH;
return END_EFFECTOR_BASE_ANGLE_RADS - Math
.atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters);
}


public double getRotateAngleRadMT2() {
Pose3d targetPoseRobotSpace =
LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target
Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target

double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the
// robot and target
double targetZ = -targetPoseRobotSpace.getZ(); // the sideways offset

double targetOffsetRads =
MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI);
double targetOffsetRads = MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI);

return targetOffsetRads;
}

public double getDistanceToSpeakerMetersMT2() {
Pose3d targetPoseRobotSpace =
LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME);
Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME);

double x = targetPoseRobotSpace.getX();
double z = targetPoseRobotSpace.getZ();


return Math.hypot(x, z);
}

Expand Down

0 comments on commit b62942a

Please sign in to comment.