odo okkeyyyy

Scorpions-Robotics · Mar 12, 2024 · 7a52ac7 · 7a52ac7
1 parent d21da9f
commit 7a52ac7
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 53 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -98,9 +98,9 @@ public static final class AutoConstants {
         DriveConstants.kPhysicalMaxAngularSpeedRadiansPerSecond / 10;
     public static final double kMaxAccelerationMetersPerSecondSquared = 3;
     public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4;
-    public static final double kPXController = 0.05;
-    public static final double kPYController = 0.05;
-    public static final double kPThetaController = 0.1;
+    public static final double kPXController = 0.2;
+    public static final double kPYController = 0.2;
+    public static final double kPThetaController = 0.05;
 
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = //
         new TrapezoidProfile.Constraints(

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -44,7 +44,6 @@
 import frc.robot.commands.Shooter.ShooterModeChange;
 import frc.robot.commands.Shooter.ShooterSetDegree;
 import frc.robot.commands.Swerve.SwerveJoystickCmd;
-import frc.robot.commands.common.FedX;
 import frc.robot.commands.common.FeedingPosition;
 import frc.robot.commands.common.IntakeInputPosition;
 import frc.robot.commands.common.dalhacan;
@@ -94,7 +93,7 @@ private void configureBindings() {
                 new JoystickButton(driverJoytick, 6).whileFalse(new InstantCommand(()->m_intake.StopNoteMotor()));
 
 
-                new JoystickButton(driverJoytick, 5).whileTrue(new RunTillSwitch(m_intake,false));
+               // new JoystickButton(driverJoytick, 5).whileTrue(new RunTillSwitch(m_intake,false,m_feeder,m_joystick,m_shooter));
                 new JoystickButton(driverJoytick, 5).whileFalse(new InstantCommand(()->m_intake.StopNoteMotor()));
 
             //  new JoystickButton(driverJoytick, 6).whileTrue(new FedX(m_intake, m_feeder, m_joystick,m_shooter));
@@ -218,20 +217,28 @@ public Command getAutonomousCommand() {
         //                 .setKinematics(DriveConstants.kDriveKinematics);
 
         TrajectoryConfig trajectoryConfig = new TrajectoryConfig(
-                3,
-                1)
+                4,
+                3)
                         .setKinematics(DriveConstants.kDriveKinematics);
 
-
                         var trajectoryOne =
                         TrajectoryGenerator.generateTrajectory(
-                           new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-                           List.of(new Translation2d(-1, 0), new Translation2d(-3, 1)),
-                           new Pose2d(-2, 0, Rotation2d.fromDegrees(0)),
-                           new TrajectoryConfig(Units.feetToMeters(0.5), Units.feetToMeters(0.5)));
+                           new Pose2d(0, 0, new Rotation2d(0)),
+                           List.of(new Translation2d(-1.28, 0),new Translation2d(-0.56, -0.91)),
+                           new Pose2d(-1.28, -1.44, new Rotation2d(0)),
+                           trajectoryConfig);
+
+                        // var trajectoryOne =
+                        //    TrajectoryGenerator.generateTrajectory(
+                        //         List.of(
+                        //                 new Pose2d(0, 0, new Rotation2d(0)),
+                        //                 new Pose2d(-1.4, -1.8, new Rotation2d(1.2)),
+                        //                 new Pose2d(0, -1, new Rotation2d(-2)),
+                        //                 new Pose2d(-1, -1, new Rotation2d(0))),
+                        //       trajectoryConfig);
 
-                        PIDController xController = new PIDController(5, 0, 0);
-                        PIDController yController = new PIDController(AutoConstants.kPYController, 0, 0);
+                        PIDController xController = new PIDController(0.08, 0, 0);
+                        PIDController yController = new PIDController(0.08, 0, 0);
 
         ProfiledPIDController thetaController = new ProfiledPIDController(
                 AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);

diff --git a/src/main/java/frc/robot/commands/Intake/RunTillSwitch.java b/src/main/java/frc/robot/commands/Intake/RunTillSwitch.java
@@ -4,21 +4,30 @@
 
 package frc.robot.commands.Intake;
 
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.commands.common.fedleme;
+import frc.robot.subsystems.FeederSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.JoystickSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 
 public class RunTillSwitch extends Command {
   IntakeSubsystem m_intake;
   boolean dursunmu;
+  FeederSubsystem m_feeder;
+  JoystickSubsystem m_joystick;
+  ShooterSubsystem m_shooter;
 boolean varmi = false;
 
 
-  public RunTillSwitch(IntakeSubsystem m_intake, boolean dursunmu) {
+  public RunTillSwitch(IntakeSubsystem m_intake, boolean dursunmu,FeederSubsystem m_feeder,JoystickSubsystem m_joystick, ShooterSubsystem m_shooter) {
 
     this.m_intake = m_intake;
-
+    this.m_feeder = m_feeder;
+    this.m_joystick = m_joystick;
+    this.m_shooter = m_shooter;
   }
 
   // Called when the command is initially scheduled.
@@ -51,6 +60,7 @@ public void execute() {
 
           m_intake.StopNoteMotor();
           varmi = true;
+          new fedleme(m_intake, m_feeder, m_joystick, m_shooter);
 
         }
       }

diff --git a/src/main/java/frc/robot/commands/common/FedX.java b/src/main/java/frc/robot/commands/common/FedX.java
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -55,7 +55,7 @@ public SwerveModule(int driveMotorId, int turningMotorId, boolean driveMotorReve
     turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderRot2Rad);
     turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderRPM2RadPerSec);
 
-    turningPidController = new PIDController(ModuleConstants.kPTurning, 0, 0);
+    turningPidController = new PIDController(0.5, 0, 0);
     turningPidController.enableContinuousInput(-Math.PI, Math.PI);
     driveMotor.setOpenLoopRampRate(0.5);
     turningMotor.setOpenLoopRampRate(0.5);
@@ -136,7 +136,7 @@ public void OtosetDesiredState(SwerveModuleState state) {
       return;
     }
     state = SwerveModuleState.optimize(state, getState().angle);
-    driveMotor.set(3*(state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
+    driveMotor.set((state.speedMetersPerSecond) / DriveConstants.kPhysicalMaxSpeedMetersPerSecond);
     turningMotor.set((turningPidController.calculate(getTurningPosition(), state.angle.getRadians())));
     SmartDashboard.putString("Swerve[" + absoluteEncoder.getDeviceID() + "] state", state.toString());
     SmartDashboard.putNumber("OHOOHOHOHOHOHOOHOHOOHOHOOHO",