fdsaf

diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
index f2e5494cc875b8e875856c56447ceee72b51f399..663b5fcb1c94a2c056539ab2deeb5ba2744da0d4 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
@@ -3,15 +3,20 @@ package frc.robot.commands.gpm;
 import java.lang.reflect.Field;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.LinearFilter;
 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.units.Unit;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Robot;
+import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.turret.ShotInterpolation;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.util.FieldZone;
 import frc.robot.util.ShootingTarget;
@@ -21,6 +26,7 @@ public class SimpleAutoShoot extends Command {
     private Turret turret;
     private Drivetrain drivetrain;
     private TurretVision turretVision;
+    private Shooter shooter;
 
     private double fieldAngleRad;
     private double turretSetpoint;
@@ -31,9 +37,15 @@ public class SimpleAutoShoot extends Command {
     private boolean turretVisionEnabled = false;
     private boolean SOTM = true;
     private Translation2d drivepose;
-    public SimpleAutoShoot(Turret turret, Drivetrain drivetrain) {
+
+    private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
+    private double lastTurretAngle = 0;
+    private double lastTurretVelocity = 0;
+
+    public SimpleAutoShoot(Turret turret, Drivetrain drivetrain, Shooter shooter) {
         this.turret = turret;
         this.drivetrain = drivetrain;
+        this.shooter = shooter;
         drivepose  = drivetrain.getPose().getTranslation();
         
         addRequirements(turret);
@@ -47,7 +59,7 @@ public class SimpleAutoShoot extends Command {
         double D_y;
         double D_x;
         // TODO: Change time to goal on actual comp bot
-        double timeToGoal = 1.0;
+        double timeToGoal = 0.0;
         
         // If the robot is moving, adjust the target position based on velocity
         if (SOTM) {
@@ -107,12 +119,24 @@ public class SimpleAutoShoot extends Command {
 
     @Override
     public void execute() {
+        //shooter.setShooterPower(ShotInterpolation.shooterPowerMap.get(FieldConstants.getHubTranslation().toTranslation2d().getDistance(drivetrain.getPose().getTranslation())));
         // 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);
+        // double turretVelocity =
+        // turretAngleFilter.calculate(
+        //     new Rotation2d(Units.degreesToRadians(turretSetpoint)).minus(new Rotation2d(Units.degreesToRadians(lastTurretAngle))).getRadians() / Constants.LOOP_TIME);
+
+        double turretAcceleration = ((-drivetrain.getAngularRate(2)) - (lastTurretVelocity)) / Constants.LOOP_TIME;
+        
+        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2));
+        // turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), (-drivetrain.getAngularRate(2)) + turretAcceleration * 0.3);
+        //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity);
+
+        lastTurretAngle = turretSetpoint;
+        lastTurretVelocity = -drivetrain.getAngularRate(2);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 5fe646b3e9e8944a94da7ba7c484088daa2803b5..f7fba67bea3f55897bf1c5448a3dd70a55e9244b 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -85,7 +85,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
                         if (simpleTurretAutoShoot != null && simpleTurretAutoShoot.isScheduled()){
                             simpleTurretAutoShoot.cancel();
                         } else{
-                            simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain());
+                            simpleTurretAutoShoot = new SimpleAutoShoot(turret, getDrivetrain(), shooter);
                             CommandScheduler.getInstance().schedule(simpleTurretAutoShoot);
                         }
                     })

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index e77f3d257cd241c3759bec0a947e9a35cd178c2a..492b2fe2e06d118cd57d85433da2b5db7dbb6fe9 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -24,6 +24,7 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
@@ -186,6 +187,7 @@ public class Drivetrain extends SubsystemBase {
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber("Shot Distance", FieldConstants.getHubTranslation().toTranslation2d().getDistance(getPose().getTranslation()));
         odometryLock.lock(); // Prevents odometry updates while reading data
         gyroIO.updateInputs(gyroInputs);
         Logger.processInputs("Drive/Gyro", gyroInputs);

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 2247f833fd0b9e9fd84d1b7c34b3c02df5211daa..9f5e34092db675ddbdedbc40fd3a91ed2e8dd04e 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -30,6 +30,8 @@ public class Shooter extends SubsystemBase implements ShooterIO {
     // Goal Velocity / Double theCircumfrence
     private double shooterTargetSpeed = 0;
     private double feederPower = 0;
+
+    public double shooterPower = 1.0;
     //Velocity in rotations per second
     VelocityVoltage voltageRequest = new VelocityVoltage(0);
 
@@ -67,11 +69,13 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     public void periodic(){
         updateInputs();
+        SmartDashboard.putNumber("Shot Power", shooterPower);
+        shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower);
 
         shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
         shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed));
-        shooterMotorLeft.set(-1);
-        shooterMotorRight.set(-1);
+        // shooterMotorLeft.set(-shooterPower);
+        // shooterMotorRight.set(-shooterPower);
         feederMotor.set(feederPower);
     }
 
@@ -85,13 +89,15 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         feederPower = power;
     }
 
-    public void setShooter(double linearVelocityMps) {
-        double wheelCircumference = Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER;
-        System.out.println("PRINTING WHEEEEEEEEEEEEL CIRUM:" + wheelCircumference);
-        shooterTargetSpeed = linearVelocityMps * ShooterConstants.SHOOTER_GEAR_RATIO / wheelCircumference; // rps
+    public void setShooter(double velocityRPS) {
+        shooterTargetSpeed = velocityRPS;
         System.out.println("Shooter is working");
     }
 
+    public void setShooterPower(double power){
+        this.shooterPower = power;
+    }
+
     public double getFeederVelocity() {
         return inputs.feederVelocity;
     }

diff --git a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java
index bb1968a656a7c686297406f7bdd952576695fccf..7a5e300a5b2de81a4b54909cfaf543522a5aedd4 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java
+++ b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java
@@ -1,13 +1,17 @@
 package frc.robot.subsystems.turret;
 
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.interpolation.InterpolatingTreeMap;
 
 public class ShotInterpolation {
     public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
-    public static Object timeOfFlight;
+    public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap();
 
     static{
         timeOfFlightMap.put(0.0, 0.67);
         timeOfFlightMap.put(1.0, 0.67);
+
+        shooterPowerMap.put(0.0, 1.0);
+        shooterPowerMap.put(1.0, 1.0);
     }
 }

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 499b1c6379e35bd379101b1e3edf2ac211fb1a48..2fa85ccaf70bfaa7534ef7e15b5642643b415873 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -4,7 +4,9 @@ import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
 import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
@@ -47,6 +49,7 @@ public class Turret extends SubsystemBase {
 
        private static final PIDController velocityPid = new PIDController(15, 0, 0.25);
 
+
     private double lastFrameVelocity = 0;
 
        /* ---------------- Hardware ---------------- */
@@ -73,9 +76,9 @@ public class Turret extends SubsystemBase {
 
        /* ---------------- Gains ---------------- */
 
-       // private static final double kP = 3500.0;
+       private static final double kP = 15.0;
 
-       // private static final double kD = 150.0;
+       private static final double kD = 0.2;
 
        /* ---------------- Visualization ---------------- */
 
@@ -85,6 +88,7 @@ public class Turret extends SubsystemBase {
 
     // 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);
+       private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
 
 
        /* ---------------- Constructor ---------------- */
@@ -92,12 +96,16 @@ public class Turret extends SubsystemBase {
        public Turret() {
                motor.setNeutralMode(NeutralModeValue.Coast);
 
-               // motor.getConfigurator().apply(
-               //              new Slot0Configs()
-               //                              .withKP(kP)
-               //                              .withKD(kD)
-        //                 .withKV(1)
-        //                 .withKA(1));
+               motor.getConfigurator().apply(
+                               new Slot0Configs()
+                                               .withKP(kP)
+                                               .withKD(kD));
+
+               TalonFXConfiguration config = new TalonFXConfiguration();
+               var motionMagicConfigs = config.MotionMagic;
+        motionMagicConfigs.MotionMagicCruiseVelocity = 10 * GEAR_RATIO;
+        motionMagicConfigs.MotionMagicAcceleration = 50 * GEAR_RATIO;
+        motor.getConfigurator().apply(config);
 
                motor.getConfigurator().apply(
                                new com.ctre.phoenix6.configs.TalonFXConfiguration() {
@@ -106,21 +114,6 @@ public class Turret extends SubsystemBase {
                                        }
                                });
 
-               // motor.getConfigurator().apply(
-               //              new TalonFXConfiguration() {
-               //                      {
-               //                              Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
-               //                      }
-               //              });
-
-        // var talonFXConfigs = new TalonFXConfiguration();
-        // var motionMagicConfigs = talonFXConfigs.MotionMagic;
-        // motionMagicConfigs.MotionMagicCruiseVelocity = 10.0;
-        // motionMagicConfigs.MotionMagicAcceleration = 50.0;
-        // motor.getConfigurator().apply(talonFXConfigs);
-
-
-
                setpoint = new State(getPositionRad(), 0.0);
                lastGoalRad = setpoint.position;
 
@@ -203,8 +196,11 @@ public class Turret extends SubsystemBase {
         // double voltage = feedForward.calculateWithVelocities(lastFrameVelocity, targetVelocity);
         double voltage = feedForward.calculate(targetVelocity);
         lastFrameVelocity = targetVelocity;
-
-        motor.setVoltage(voltage);
+               if(Math.abs(setpoint.position * GEAR_RATIO - Units.rotationsToRadians(motor.getPosition().getValueAsDouble())) > Math.PI/2){
+                       motor.setControl(mmVoltageRequest.withPosition(Units.radiansToRotations(setpoint.position * GEAR_RATIO)));
+               } else{
+                       motor.setVoltage(voltage);
+               }
 
                // var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false);
         Logger.recordOutput("Turret Voltage", motor.getMotorVoltage().getValue());