@@ -177,15 +177,18 @@ private Command getAlignCommand(ReefSelect select) {
177177            case  L4 : target  = CougarUtil .addDistanceToPose (target , Units .inchesToMeters (2 )); break ;
178178            case  drive : default : /* do nothing */  break ;
179179          }
180-         } 
180+         }
181+ 
182+         Command  finalAlign  = new  AlignCommand (m_swerve , target );
183+         if  (DriverStation .isAutonomous ()) finalAlign  = finalAlign .withTimeout (1.5 );
181184
182185        if (CougarUtil .getDistance (target , m_swerve .getPose ()) > 0.2 )
183186          return  Commands .sequence (
184187            AutoBuilder .pathfindToPose (target , TunerConstants .kAutoAlignConstraints ),
185-             new   AlignCommand ( m_swerve ,  target ) 
188+             finalAlign 
186189          );
187190        else 
188-           return  new   AlignCommand ( m_swerve ,  target ) ;
191+           return  finalAlign ;
189192      }, Set .of (m_swerve )),
190193      Blackbox .setAligningCmd (false )
191194    ).finallyDo ((interrupted ) -> {
@@ -416,8 +419,8 @@ private void configureBindings() {
416419      Commands .waitUntil (() -> Blackbox .isCoralLoaded ()));
417420    NamedCommands .registerCommand ("WaitForSetpoint" , 
418421      new  WaitUntilDebounced (() -> m_wrist .isAtSetpoint () && m_elevator .isAtSetpoint (), 0.1 ).withTimeout (3 ));
419-     NamedCommands .registerCommand ("ReefAlignL" , getAlignCommand (Blackbox .ReefSelect .LEFT ). withTimeout ( 2 ) );
420-     NamedCommands .registerCommand ("ReefAlignR" , getAlignCommand (Blackbox .ReefSelect .RIGHT ). withTimeout ( 2 ) );
422+     NamedCommands .registerCommand ("ReefAlignL" , getAlignCommand (Blackbox .ReefSelect .LEFT ));
423+     NamedCommands .registerCommand ("ReefAlignR" , getAlignCommand (Blackbox .ReefSelect .RIGHT ));
421424    NamedCommands .registerCommand ("Loading" , Blackbox .robotStateCmd (Blackbox .State .loading ));
422425
423426
0 commit comments