package frc.robot.commands.gpm;
+import java.lang.reflect.Field;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Robot;
+import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.turret.Turret;
+import frc.robot.util.FieldZone;
+import frc.robot.util.ShootingTarget;
+import frc.robot.util.Vision.TurretVision;
public class SimpleAutoShoot extends Command {
-
+ private Turret turret;
+ private Drivetrain drivetrain;
+ private TurretVision turretVision;
+
+ private double fieldAngleRad;
+ private double turretSetpoint;
+ private double adjustedSetpoint;
+ private double yawToTagCamera;
+ private double yawToTag;
+
+ private boolean turretVisionEnabled = false;
+ private boolean SOTM = true;
+ private Translation2d drivepose;
+ public SimpleAutoShoot(Turret turret, Drivetrain drivetrain) {
+ this.turret = turret;
+ this.drivetrain = drivetrain;
+ drivepose = drivetrain.getPose().getTranslation();
+
+ addRequirements(turret);
+ }
+
+ public void updateTurretSetpoint(Translation2d drivepose) {
+
+ //FieldZone currentZone = getZone(drivepose);
+ Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
+
+ double D_y;
+ double D_x;
+ // TODO: Change time to goal on actual comp bot
+ double timeToGoal = 1.0;
+
+ // If the robot is moving, adjust the target position based on velocity
+ if (SOTM) {
+ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
+ ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
+ double xVel = fieldRelVel.vxMetersPerSecond;
+ double yVel = fieldRelVel.vyMetersPerSecond;
+
+ D_y = target.getY() - drivepose.getY() - timeToGoal * yVel;
+ D_x = target.getX() - drivepose.getX() - timeToGoal * xVel;
+ } else {
+ D_y = target.getY() - drivepose.getY();
+ D_x = target.getX() - drivepose.getX();
+ }
+
+ // Calculate the field-relative angle
+ fieldAngleRad = Math.atan2(D_y, D_x);
+
+ // Calculate robot heading and adjust for reverse drive
+ double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment
+
+ // Calculate turret setpoint (angle relative to robot heading)
+ turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0);
+
+ // System.out.println("Aligning the turn to degree angle: " + turretSetpoint);
+ }
+
+ public void adjustWithTurretCam() {
+ if(turretVision.canSeeTag(26) || turretVision.canSeeTag(10)){
+ // Adjust turret setpoint based on vision input
+ if(Robot.getAlliance() == Alliance.Blue){
+ yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(26).get());
+ }
+ else{
+ yawToTagCamera = -1 * Units.radiansToDegrees(turretVision.getYawToTagRad(10).get());
+ }
+ double error = yawToTagCamera - yawToTag;
+ adjustedSetpoint = turretSetpoint + error;
+ }
+ }
+
+ public void updateYawToTag(){
+ // Calculate the yaw offset to the tag
+ double D_y = FieldConstants.getHubTranslation().getY() - drivetrain.getPose().getY();
+ double D_x = FieldConstants.getHubTranslation().getX() - drivetrain.getPose().getX();
+ double angleToTag = Units.radiansToDegrees(Math.atan(D_y / D_x));
+ yawToTag = angleToTag - Units.radiansToDegrees(fieldAngleRad);
+ }
+
+ @Override
+ public void initialize() {
+ // Initialize setpoint calculation and set the initial goal for the turret
+ updateTurretSetpoint(drivepose);
+ // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), drivetrain.getAngularRate(2));
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0);
+ }
+
+ @Override
+ public void execute() {
+ // Continuously update setpoints and adjust based on vision if available
+ drivepose = drivetrain.getPose().getTranslation();
+ updateTurretSetpoint(drivepose);
+ updateYawToTag();
+
+ turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2) * 1.0);
+ }
+
+ @Override
+ public void end(boolean interrupted) {
+ // Set the turret to a safe position when the command ends
+ turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
+ }
+
+ public boolean leftSide(Translation2d drivepose) {
+ if (drivepose.getY() > (FieldConstants.FIELD_WIDTH / 2)) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ public FieldZone getZone(Translation2d drivepose) {
+ return FieldConstants.getZone(drivepose);
+ }
}
+
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.robot.Robot;
+import frc.robot.commands.gpm.SimpleAutoShoot;
import frc.robot.commands.gpm.TurretAutoShoot;
import frc.robot.commands.gpm.TurretJoyStickAim;
import frc.robot.constants.Constants;
private Pose2d alignmentPose = null;
private Command turretAutoShoot;
+ private Command simpleTurretAutoShoot;
private TurretJoyStickAim turretJoyStickAim;
public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret) {
driver.get(PS5Button.SQUARE).onTrue(
new InstantCommand(()->{
- if (turretAutoShoot != null && turretAutoShoot.isScheduled()){
- turretAutoShoot.cancel();
+ if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){
+ simpleTurretAutoShoot.cancel();
} else{
- turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain());
- CommandScheduler.getInstance().schedule(turretAutoShoot);
+ simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain());
+ CommandScheduler.getInstance().schedule(simpleTurretAutoShoot);
}
})
);
private static final double TURRET_RATIO = 140.0 / 10.0;
private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO;
- private static final PIDController velocityPid = new PIDController(15, 0, 0.2);
+ private static final PIDController velocityPid = new PIDController(15, 0, 0.25);
+
+ private double lastFrameVelocity = 0;
/* ---------------- Hardware ---------------- */
private final MechanismRoot2d root = mech.getRoot("turret", 50, 50);
private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0));
- private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0);
+ // private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010);
+ private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0);
+
/* ---------------- Constructor ---------------- */
// .withKV(1)
// .withKA(1));
- // motor.getConfigurator().apply(
- // new com.ctre.phoenix6.configs.TalonFXConfiguration() {
- // {
- // MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
- // }
- // });
+ motor.getConfigurator().apply(
+ new com.ctre.phoenix6.configs.TalonFXConfiguration() {
+ {
+ MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ }
+ });
// motor.getConfigurator().apply(
// new TalonFXConfiguration() {
@Override
public void periodic() {
-
-
double robotRelativeGoal = goalAngle.getRadians();
// --- MA-style continuous wrap selection ---
motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
setpoint.position * GEAR_RATIO);
- targetVelocity += Units.radiansToRotations(motorVelRotPerSec);
+ targetVelocity += Units.rotationsToRadians(motorVelRotPerSec);
+ // double voltage = feedForward.calculateWithVelocities(lastFrameVelocity, targetVelocity);
double voltage = feedForward.calculate(targetVelocity);
+ lastFrameVelocity = targetVelocity;
motor.setVoltage(voltage);
SmartDashboard.putNumber("Turret targetVelocity",
Units.radiansToDegrees(targetVelocity));
SmartDashboard.putNumber("Turret Position Deg",
- Units.radiansToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO);
+ Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / GEAR_RATIO);
// SmartDashboard.putData("Spin to 90 deg", new
// InstantCommand(()->{setFieldRelativeTarget(new Rotation2d(Math.PI), 0);}));