FairportRobotics
/
Castor2026
mirror of https://github.com/FairportRobotics/Castor2026.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Much better Turret PID

This commit is contained in:
Tyler Wilcox 2026-04-04 13:38:28 -04:00
parent 05807a663e
commit 317d21d000
3 changed files with 33 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/main/java/frc/robot/Utils.java Normal file
View File

@ -0,0 +1,25 @@
package frc.robot;
public class Utils {
/**
* Calculates the needed angle of the turret for a distance from the HUB.
* @param distanceToTag
* @return number of rotations of the turret, robot relative
*/
public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
// Calculate turret Angle
double cameraDistanceToTarget = distanceToTag + .6;
double cameraToShootDistance = 0.26035;
// a^2 + b^2 = c^2
double shooterDistanceToTarget = Math
.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
double robotTurretRotations = (robotTurretAngle / Math.PI);
return robotTurretRotations;
}
}

15
src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
View File

@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.Utils;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.HopperSubsystem;
import frc.robot.subsystems.TurretSubsystem;
@ -117,18 +118,8 @@ public class AutoShootCommandChassisTurret extends Command{
Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
// Calculate turret Angle
double cameraDistanceToTarget = targetsPose[2];
double cameraToShootDistance = 0.26035;
// a^2 + b^2 = c^2
double shooterDistanceToTarget = Math.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
Logger.recordOutput("AutoAlign-TurretAngleOfTriagle", angleAtShooter);
double robotTurretAngle = (Math.PI/2) - angleAtShooter;
Logger.recordOutput("AutoAlign-TurretAngleRelativeToRobot", robotTurretAngle);
double robotTurretRotations = (robotTurretAngle / Math.PI) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
Logger.recordOutput("AutoAlign-TurretRotations", robotTurretRotations);
turretSubsystem.setTurretMotorRotation(-robotTurretRotations);
double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
turretSubsystem.setTurretMotorRotation(-turretAngle);
driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
}

10
src/main/java/frc/robot/subsystems/TurretSubsystem.java
View File

@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem {
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
.withKP(3)
.withKI(1.5)
.withKD(0)
.withKS(0.3)
.withKP(6)
.withKI(0)
.withKD(0.3)
.withKS(0)
)
.withMotorOutput(
new MotorOutputConfigs()
@ -131,7 +131,7 @@ public class TurretSubsystem extends TestableSubsystem {
public boolean isLauncherUpToSpeed() {
// Valid if velcity is within 10% of setpoint.
// May need to lower this
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.075;
return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
}
public void setTargetElevation(double elev)