Merge pull request #34 from FRC1360/tuning-for-dcmp

update main to latest code

src/main/java/frc/robot/Constants.java

@@ -65,7 +65,7 @@ public static final class Drivetrain {
     public static final int SHOULDER_MOTOR_SLAVE = 51;
     public static final int SHOULDER_ENCODER = 0;
     //public static final double SHOULDER_ENCODER_OFFSET = 0.542;
-    public static final double SHOULDER_ENCODER_OFFSET = 0.194;
+    public static final double SHOULDER_ENCODER_OFFSET = 0.095;
     public static final double SHOULDER_GEAR_RATIO = (11.0 / 52.0) * (30.0 / 68.0) * (12.0 / 60.0);
     public static final double SHOULDER_MANUAL_OVERRIDE_RANGE = 20.0;
     public static final double MAX_SHOULDER_ANGLE = 90.0; 
@@ -105,7 +105,7 @@ public static final class Drivetrain {
 
 
     // CONE_INTAKE_POSITION
-    public static final double CONE_INTAKE_POSITION_WRIST = 48.0;
+    public static final double CONE_INTAKE_POSITION_WRIST = 53.0;
     public static final double CONE_INTAKE_POSITION_ARM = 5.8;
     public static final double CONE_INTAKE_POSITION_SHOULDER = -48.0;
 
@@ -116,8 +116,8 @@ public static final class Drivetrain {
 
     // CONE_SCORE_HIGH_POSITION
     public static final double CONE_SCORE_HIGH_POSITION_WRIST = -40.0;
-    public static final double CONE_SCORE_HIGH_POSITION_ARM = 19.0;
-    public static final double CONE_SCORE_HIGH_POSITION_SHOULDER = 44.0;
+    public static final double CONE_SCORE_HIGH_POSITION_ARM = 20.0;
+    public static final double CONE_SCORE_HIGH_POSITION_SHOULDER = 37.0;
 
     // CUBE_SCORE_HIGH_POSITION
     public static final double CUBE_SCORE_HIGH_POSITION_WRIST = 130.0;

src/main/java/frc/robot/Robot.java

@@ -9,6 +9,8 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.intake.IntakeHoldCommand;
+import frc.robot.subsystems.IntakeSubsystem;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -80,17 +82,19 @@ public void autonomousInit() {
     m_robotContainer.getShoulderZeroCommand().schedule();*/
 
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
-      (m_robotContainer.getArmHomeCommand()
+      (((m_robotContainer.getArmHomeCommand()
       .andThen(m_robotContainer.getGoToZeroWristCommand())
-      .andThen(m_robotContainer.getShoulderZeroCommand()))
+      .andThen(m_robotContainer.getShoulderZeroCommand())))
+        .raceWith(m_robotContainer.getIntakeHoldCommand()))
+      .andThen(m_robotContainer.setSMHomeCommand())
       .andThen(m_autonomousCommand).schedule();
     } else {
       (m_robotContainer.getArmHomeCommand()
       .andThen(m_robotContainer.getGoToZeroWristCommand())
-      .andThen(m_robotContainer.getShoulderZeroCommand())).schedule(); 
+      .andThen(m_robotContainer.getShoulderZeroCommand()))
+      .andThen(m_robotContainer.setSMHomeCommand()).schedule(); 
     }
 
    // m_robotContainer.getArmHomeCommand().schedule(); 
@@ -125,7 +129,8 @@ public void teleopInit() {
 
       (m_robotContainer.getArmHomeCommand()
         .andThen(m_robotContainer.getGoToZeroWristCommand())
-        .andThen(m_robotContainer.getShoulderZeroCommand())).schedule(); 
+        .andThen(m_robotContainer.getShoulderZeroCommand())
+        .andThen(m_robotContainer.setSMHomeCommand())).schedule(); 
     }
 
 

src/main/java/frc/robot/RobotContainer.java

@@ -15,12 +15,11 @@
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.autos.LeftSide2ConeAuto;
-import frc.robot.autos.RightSide2ConeAuto;
 import frc.robot.autos.AutoBalance;
-import frc.robot.autos.ConeHighAndBalanceAuto;
+import frc.robot.autos.CubeHighAndBalanceAuto;
+import frc.robot.autos.DriveStraightAuto;
 import frc.robot.autos.ConeHighAndDriveAuto;
-import frc.robot.autos.ConeScoreHighAuto;
+import frc.robot.autos.procedures.ConeScoreHighAuto;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.arm.ArmHoldCommand;
 import frc.robot.commands.arm.ArmHomeCommand;
@@ -44,6 +43,7 @@
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.vision.Vision;
+import frc.robot.util.StateMachine;
 import frc.robot.subsystems.ShoulderSubsystem;
 import frc.robot.subsystems.WristSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -70,23 +70,24 @@ public class RobotContainer {
   private final Vision vision = new Vision(); 
   public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
   public final LEDSubsystem ledSubsystem = new LEDSubsystem();
+  private final StateMachine sm = new StateMachine();
 
-  private final SendableChooser<Command> autoChooser = new SendableChooser<Command>(); 
+  public final SendableChooser<Command> autoChooser = new SendableChooser<Command>(); 
 
-  // private final RightSide2ConeAuto rightConeAuto = new RightSide2ConeAuto(m_drivetrainSubsystem); 
-
-  private final LeftSide2ConeAuto leftConeAuto = new LeftSide2ConeAuto(m_drivetrainSubsystem);
-
-  private final ConeHighAndBalanceAuto highConeAndBalanceAuto = new ConeHighAndBalanceAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, 
-                                                                                            wristSubsystem, armSubsystem, intakeSubsystem, armMessenger);
+  private final CubeHighAndBalanceAuto highConeAndBalanceAuto = new CubeHighAndBalanceAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, 
+                                                                                            wristSubsystem, armSubsystem, intakeSubsystem, armMessenger, ledSubsystem, sm);
   private final ConeHighAndDriveAuto highConeAndDriveAuto = new ConeHighAndDriveAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, 
-                                                                                      wristSubsystem, armSubsystem, intakeSubsystem, armMessenger, ledSubsystem); 
+                                                                                      wristSubsystem, armSubsystem, intakeSubsystem, armMessenger, ledSubsystem, sm); 
+
+  private final ConeScoreHighAuto highConeAuto = new ConeScoreHighAuto(m_drivetrainSubsystem, shoulderSubsystem, shoulderMessenger, wristSubsystem, 
+                                                                          armSubsystem, intakeSubsystem, armMessenger, ledSubsystem, sm); 
 
-  private final ConeScoreHighAuto highConeAuto = new ConeScoreHighAuto(shoulderSubsystem, shoulderMessenger, wristSubsystem, 
-                                                                          armSubsystem, intakeSubsystem, armMessenger, ledSubsystem); 
+  private final DriveStraightAuto driveStraightAuto = new DriveStraightAuto(m_drivetrainSubsystem); 
 
   private final Simulator sim = new Simulator(m_drivetrainSubsystem); 
 
+
+
   // private final DriveStraightAuto driveStraightAuto = new DriveStraightAuto(m_drivetrainSubsystem, wristSubsystem); 
   // private final EngageStationAuto engageStationAuto = new EngageStationAuto(m_drivetrainSubsystem); 
   //private final Simulator sim = new Simulator(m_drivetrainSubsystem); 
@@ -121,9 +122,10 @@ public RobotContainer() {
   public void initializeRobot() { 
     //autoChooser.setDefaultOption("One side, two cargo, balance", leftConeAuto);
     autoChooser.setDefaultOption("No auto", new WaitCommand(15));
-    autoChooser.addOption("High cone and balance", highConeAndBalanceAuto);
+    autoChooser.addOption("High cube and balance", highConeAndBalanceAuto);
     autoChooser.addOption("High cone and drive straight", highConeAndDriveAuto);
     autoChooser.addOption("Only high cone score", highConeAuto);
+    autoChooser.addOption("Only drive straight", driveStraightAuto);
     SmartDashboard.putData("Auto Chooser", autoChooser);
   }
 
@@ -142,20 +144,22 @@ private void configureButtonBindings() {
 
     left_controller.button(1).onTrue(new InstantCommand(m_drivetrainSubsystem::zeroGyroscope)); 
 
-    operatorController.a().onTrue((new AssemblyGoToCubeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem)));
-    operatorController.y().onTrue(new AssemblyGoToConeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem));
-    operatorController.b().onTrue(new AssemblyMidScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem, () -> operatorController.leftBumper().getAsBoolean())); 
-    operatorController.x().onTrue(new AssemblyHomePositionCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem)); 
-    operatorController.povUp().onTrue(new AssemblyHighScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, () -> operatorController.leftBumper().getAsBoolean(), ledSubsystem)); 
-    operatorController.povDown().onTrue(new AssemblyPickUpSingleSubstationCommand(shoulderSubsystem, wristSubsystem, armSubsystem, shoulderMessenger, armMessenger, intakeSubsystem, ledSubsystem)); 
+    operatorController.a().and(() -> sm.getAtHome()).onTrue((new AssemblyGoToCubeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem, sm)));
+    operatorController.y().and(() -> sm.getAtHome()).onTrue(new AssemblyGoToConeIntakeCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, intakeSubsystem, ledSubsystem, sm));
+    operatorController.b().and(() -> sm.getAtHome()).onTrue(new AssemblyMidScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem, () -> operatorController.leftBumper().getAsBoolean(), sm)); 
+    operatorController.x().and(() -> !sm.getAtHome()).onTrue(new AssemblyHomePositionCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, ledSubsystem, sm)); 
+    operatorController.povUp().and(() -> sm.getAtHome()).onTrue(new AssemblyHighScoreCommand(shoulderSubsystem, shoulderMessenger, wristSubsystem, armSubsystem, armMessenger, () -> operatorController.leftBumper().getAsBoolean(), ledSubsystem, sm)); 
+    operatorController.povDown().and(() -> sm.getAtHome()).onTrue(new AssemblyPickUpSingleSubstationCommand(shoulderSubsystem, wristSubsystem, armSubsystem, shoulderMessenger, armMessenger, intakeSubsystem, ledSubsystem, sm)); 
 
 
     new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.05)
      .whileTrue(new ManualIntakeCommand(intakeSubsystem, () -> operatorController.getLeftTriggerAxis()));
     new Trigger(() -> operatorController.getRightTriggerAxis() > 0.05)
      .whileTrue(new ManualPutdownCommand(intakeSubsystem, () -> operatorController.getRightTriggerAxis())); 
 
-    left_controller.button(2).whileTrue(new StrafeAlign(m_drivetrainSubsystem, vision, left_controller::getX, left_controller::getY));
+    //left_controller.button(2).whileTrue(new StrafeAlign(m_drivetrainSubsystem, vision, left_controller::getX, left_controller::getY));
+    right_controller.button(1).whileTrue(new ManualPutdownCommand(intakeSubsystem, () -> 1.0)); 
+    left_controller.button(3).whileTrue(new InstantCommand( () -> m_drivetrainSubsystem.lockWheels = true)).whileFalse( new InstantCommand( () -> m_drivetrainSubsystem.lockWheels = false));
   }
 
 
@@ -166,10 +170,8 @@ private void configureButtonBindings() {
    */
   public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
-    //return rightConeAuto; 
-    // return leftConeAuto; 
     //return null; 
-    return autoChooser.getSelected();  
+    return autoChooser.getSelected(); 
     //return highConeAndBalanceAuto; 
   }
 
@@ -187,6 +189,14 @@ public Command getGoToZeroWristCommand() {
     return new WristGoToPositionCommand(wristSubsystem, Constants.HOME_POSITION_WRIST); 
   }
 
+  public Command getIntakeHoldCommand() { 
+    return new IntakeHoldCommand(intakeSubsystem); 
+  }
+
+  public Command setSMHomeCommand() {
+    return new InstantCommand( () -> sm.setAtHome(true));
+  }
+
   private static double deadband(double value, double deadband) {
     if (Math.abs(value) > deadband) {
       if (value > 0.0) {

src/main/java/frc/robot/autos/AutoBalanceCommand.java

@@ -42,7 +42,7 @@ public void execute() {
 
     @Override
     public void end(boolean interrupted) { 
-        this.speeds.vyMetersPerSecond = 0.01; 
+        this.speeds.omegaRadiansPerSecond = 0.0001; 
         dt.drive(this.speeds);
     }
 

src/main/java/frc/robot/autos/ConeHighAndDriveAuto.java

@@ -4,7 +4,8 @@
 import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.autos.basic.Drive;
-import frc.robot.autos.basic.DriveSpeed;
+import frc.robot.autos.basic.LockWheels;
+import frc.robot.autos.procedures.ConeScoreHighAuto;
 import frc.robot.commands.arm.ArmHoldCommand;
 import frc.robot.commands.assembly.AssemblyHomePositionCommand;
 import frc.robot.commands.assembly.autoAssembly.AutoAssemblyConeHighScoreCommand;
@@ -17,19 +18,19 @@
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShoulderSubsystem;
 import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger;
+import frc.robot.util.StateMachine;
 import frc.robot.subsystems.WristSubsystem;
 
 public class ConeHighAndDriveAuto extends SequentialCommandGroup {
 
     public ConeHighAndDriveAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, 
                                     WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, 
                                         ArmShoulderMessenger armMessenger,
-                                        LEDSubsystem ledSubsystem) { 
+                                        LEDSubsystem ledSubsystem,
+                                        StateMachine sm) { 
 
-        addCommands(new AutoAssemblyConeHighScoreCommand(shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger),
-                       // .raceWith(new DriveSpeed(dt, -0.5, 0.0)),   // New - To be tested!!!!
-                    new AssemblyHomePositionCommand(shoulder, shoulderWristMessenger, wrist, arm, armMessenger, ledSubsystem), 
-                    new Drive(dt, 8.0, 0.0)
+        addCommands(new ConeScoreHighAuto(dt, shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger, ledSubsystem, sm), 
+                    new Drive(dt, 8.2, 0.0)
                         .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0))
                         .raceWith(new WristHoldCommand(wrist, () -> 0.0))
                         .raceWith(new ArmHoldCommand(arm))

src/main/java/frc/robot/autos/ConeHighAndBalanceAuto.java → src/main/java/frc/robot/autos/CubeHighAndBalanceAuto.java

@@ -2,25 +2,28 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.autos.basic.DriveEncoder;
+import frc.robot.autos.procedures.CubeScoreHighAuto;
 import frc.robot.commands.arm.ArmHoldCommand;
 import frc.robot.commands.shoulder.ShoulderHoldCommand;
 import frc.robot.commands.wrist.WristHoldCommand;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.ArmSubsystem.ArmShoulderMessenger;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShoulderSubsystem;
 import frc.robot.subsystems.ShoulderSubsystem.ShoulderWristMessenger;
+import frc.robot.util.StateMachine;
 import frc.robot.subsystems.WristSubsystem;
 
-public class ConeHighAndBalanceAuto extends SequentialCommandGroup {
+public class CubeHighAndBalanceAuto extends SequentialCommandGroup {
 
-    public ConeHighAndBalanceAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, 
+    public CubeHighAndBalanceAuto(DrivetrainSubsystem dt, ShoulderSubsystem shoulder, ShoulderWristMessenger shoulderWristMessenger, 
                                     WristSubsystem wrist, ArmSubsystem arm, IntakeSubsystem intake, 
-                                        ArmShoulderMessenger armMessenger) { 
+                                        ArmShoulderMessenger armMessenger, LEDSubsystem ledSubsystem, StateMachine sm) { 
 
-        addCommands(//new ConeScoreHighAuto(shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger), 
-                    new DriveEncoder(dt, 3.5, 0.0)
+        addCommands(new CubeScoreHighAuto(dt, shoulder, shoulderWristMessenger, wrist, arm, intake, armMessenger, ledSubsystem, sm), 
+                    new DriveEncoder(dt, 4.0, 0.0)
                         .raceWith(new ShoulderHoldCommand(shoulder, armMessenger, () -> 0.0))
                         .raceWith(new WristHoldCommand(wrist, () -> 0.0))
                         .raceWith(new ArmHoldCommand(arm)),

src/main/java/frc/robot/autos/Right2CargoBalance.java → src/main/java/frc/robot/autos/DriveStraightAuto.java

@@ -4,9 +4,12 @@
 import frc.robot.autos.basic.Drive;
 import frc.robot.subsystems.DrivetrainSubsystem;
 
-public class Right2CargoBalance extends SequentialCommandGroup {
+public class DriveStraightAuto extends SequentialCommandGroup {
 
-    public Right2CargoBalance(DrivetrainSubsystem dt) { 
-        //addCommands(new Drive());
+    public DriveStraightAuto(DrivetrainSubsystem dt) { 
+
+        addCommands(
+            new Drive(dt, 8.0, 0.0)
+        );
     }
 }