score l1 mabe

BitBucketsFRC4183 · Mar 7, 2025 · f2f8946 · f2f8946
1 parent a9a0780
commit f2f8946
Show file tree

Hide file tree

Showing 4 changed files with 39 additions and 17 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -193,14 +193,15 @@ public RobotContainer() {
         break;
     }
 
-    this.autoSubsystem = new AutoSubsystem(clawSubsystem, driveSubsystem);
+    this.autoSubsystem = new AutoSubsystem(driveSubsystem, elevatorSubsystem, armSubsystem);
     autoChooser = new AutoChooser();
 
-    autoChooser.addRoutine("FourL4CoralBottom", AutoSubsystem::FourL4CoralBottomRoutine);
-    autoChooser.addRoutine("FourL4CoralTop", AutoSubsystem::FourL4CoralTopRoutine);
-    autoChooser.addRoutine("ThreeL4CoralBottom", AutoSubsystem::ThreeL4CoralBottomRoutine);
-    autoChooser.addRoutine("ThreeL4CoralTop", AutoSubsystem::ThreeL4CoralTopRoutine);
-    autoChooser.addRoutine("OneL4CoralMid", AutoSubsystem::OneL4CoralMidRoutine);
+//    autoChooser.addRoutine("FourL4CoralBottom", AutoSubsystem::FourL4CoralBottomRoutine);
+//    autoChooser.addRoutine("FourL4CoralTop", AutoSubsystem::FourL4CoralTopRoutine);
+//    autoChooser.addRoutine("ThreeL4CoralBottom", AutoSubsystem::ThreeL4CoralBottomRoutine);
+//    autoChooser.addRoutine("ThreeL4CoralTop", AutoSubsystem::ThreeL4CoralTopRoutine);
+//    autoChooser.addRoutine("OneL4CoralMid", AutoSubsystem::OneL4CoralMidRoutine);
+    autoChooser.addCmd("Score1L4Coral", autoSubsystem::OneL4Score);
 
     autoChooser.addCmd("DriveSysIDQuasistaticForward", () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
     autoChooser.addCmd("DriveSysIDQuasistaticReverse", () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));

diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java
@@ -46,8 +46,10 @@ public class ArmConstants {
     // so if encoder is 0.8, offset is 0.8 and not 0.8
     public static final double encoderOffsetRots = 0.630;
     public static boolean encoderInverted;
-    public static double setpointUp = Math.PI / 4;
-    public static double setpointDown =  -Math.PI / 4;
+    public final static double armL1Angle = 0;
+    public final static double armL2Angle = 0;
+    public final static double armL3Angle = 0;
+    public final static double armL4Angle = 0;
     public static InvertedValue arm1Inverted = InvertedValue.Clockwise_Positive;
     public static InvertedValue arm2Inverted = InvertedValue.CounterClockwise_Positive;
 }
diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -43,10 +43,10 @@ public class ElevatorConstants {
     public static double kD = 0;
 
 
-    public static double L1 = 0.635; // THESE VALUES ARE IN Meters. FOR EVERY CALCULATION, ASSUME THESE ARE INCHES.
-    public static double L2 = 0.635;
-    public static double L3 = 1.04775;
-    public static double L4 = 1.7018;
+    public static double L1 = 0; // THESE VALUES ARE IN Meters. FOR EVERY CALCULATION, ASSUME THESE ARE INCHES.
+    public static double L2 = 0;
+    public static double L3 = 0;
+    public static double L4 = 0;
     public static double carriageMass = 20.0;
     public static double maxHeight = Units.inchesToMeters(50);
     public static double minHeight = 0;

diff --git a/src/main/java/frc/robot/subsystems/AutoSubsystem/AutoSubsystem.java b/src/main/java/frc/robot/subsystems/AutoSubsystem/AutoSubsystem.java
@@ -4,21 +4,28 @@
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
 import edu.wpi.first.wpilibj2.command.*;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.commands.ArmElevatorToSetpoint;
+import frc.robot.commands.DriveCommands.RobotRelativeDriveCommand;
+import frc.robot.constants.ArmConstants;
+import frc.robot.constants.ElevatorConstants;
+import frc.robot.subsystems.ArmSubsystem.ArmSubsystem;
 import frc.robot.subsystems.ClawSubsystem.ClawSubsystem;
 import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem.ElevatorSubsystem;
+
 
 public class AutoSubsystem extends SubsystemBase {
     private final DriveSubsystem drive;
-    private final ClawSubsystem claw;
+    private final ElevatorSubsystem elevator;
+    private final ArmSubsystem arm;
     private static AutoFactory autoFactory;
 
 
-    public AutoSubsystem(ClawSubsystem claw,
-                         DriveSubsystem drive) {
+    public AutoSubsystem(DriveSubsystem drive, ElevatorSubsystem elevator, ArmSubsystem arm) {
         this.drive = drive;
-        this.claw = claw; //TODO!!!!!! (fix to end effector)
+        this.elevator = elevator;
+        this.arm = arm;
         this.autoFactory = new AutoFactory(drive::getPose, drive::setPose, drive::followTrajectorySample, true, drive);
 
     }
@@ -408,6 +415,18 @@ public static AutoRoutine OneL4CoralMidRoutine() {
 
     return OneL4CoralMidRoutine;
 
+
+    }
+
+
+    public Command OneL4Score() {
+            double yValue = 0.2;
+            return Commands.sequence(
+                    new ArmElevatorToSetpoint(elevator, arm, ElevatorConstants.L4, ArmConstants.armL4Angle),
+                    Commands.waitSeconds(1),
+                    Commands.deadline(Commands.waitSeconds(0.1), new RobotRelativeDriveCommand(drive, () -> 0, () -> yValue, () -> 0))
+                    );
+
 }}