auto + arm commands (from tuesday)

.pathplanner/settings.json

@@ -0,0 +1,14 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [
+    "RightStackBucket"
+  ],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}

.vscode/settings.json

@@ -0,0 +1,3 @@
+{
+    "java.configuration.updateBuildConfiguration": "interactive"
+}

build.gradle

@@ -41,6 +41,7 @@ deploy {
 
 def deployArtifact = deploy.targets.roborio.artifacts.frcJava
 
+
 // Set to true to use debug for JNI.
 wpi.java.debugJni = false
 

src/main/deploy/pathplanner/autos/2m forward.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "2m Forward"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/2m Forward.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/New Path.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.7867317512548395,
+        "y": 7.006864390158862
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7867317512548397,
+        "y": 7.006864390158862
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "RightStackBucket",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -0,0 +1,7 @@
+package frc.robot;
+
+public class Constants {
+    public static final double scorePreset = 85;
+    public static final double sourcePreset = 0;
+    public static final double groundPreset = 115;
+}

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

@@ -4,23 +4,38 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.commands.PathfindingCommand;
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.ArmCommands;
+import frc.robot.commands.IntakeCommands;
 
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
 
+
   @Override
   public void robotInit() {
     m_robotContainer = new RobotContainer();
+    PathfindingCommand.warmupCommand().schedule();
   }
 
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run(); 
+
+    SmartDashboard.putNumber("Top Limit Switch", m_robotContainer.m_arm.armMotor.getForwardLimit().getValueAsDouble());
+    SmartDashboard.putNumber("Arm Position", m_robotContainer.m_arm.getPosition());
+    SmartDashboard.putNumber("Test", System.currentTimeMillis());
+
+    // if(m_robotContainer.armLimitSwitch.get()){
+    //   m_robotContainer.m_arm.changeOffset();
+    // } 
   }
 
   @Override
@@ -34,7 +49,8 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+   // m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+   m_autonomousCommand = ArmCommands.movearmPosition(90);
 
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();

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

@@ -7,34 +7,45 @@
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.ArmCommands;
+import frc.robot.commands.AutoCommands;
 import frc.robot.commands.IntakeCommands;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.ArmSystem;
 import frc.robot.subsystems.Intake;
 
+import frc.robot.Constants;
+
 public class RobotContainer {
   public static ArmSystem m_arm = new ArmSystem();
   public static Intake intake = new Intake();
   private double MaxSpeed = TunerConstants.kSpeedAt12VoltsMps; // kSpeedAt12VoltsMps desired top speed
   //originally 1.5  radians per second
-  private double MaxAngularRate = 0.5 * Math.PI; // 3/4 of a rotation per second max angular velocity
+  private double MaxAngularRate = 1.0 * Math.PI; // 3/4 of a rotation per second max angular velocity
 
+  private boolean slow = false;
   /* Setting up bindings for necessary control of the swerve drive platform */
   private final CommandXboxController joystick1 = new CommandXboxController(0); // driver
   private final CommandXboxController joystick2 = new CommandXboxController(1); // operator
   private final CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain
 
+  public DigitalInput armLimitSwitch = new DigitalInput(9);
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
       .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric
@@ -43,13 +54,15 @@ public class RobotContainer {
   private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
 
   private final Telemetry logger = new Telemetry(MaxSpeed);
+
+  private final SendableChooser<Command> autoChooser;
 
   private void configureBindings() {
     drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
-        drivetrain.applyRequest(() -> drive.withVelocityX(-joystick1.getLeftY() * MaxSpeed) // Drive forward with
+        drivetrain.applyRequest(() -> drive.withVelocityX(-joystick1.getLeftY() * MaxSpeed * (slow?0.3:1)) // Drive forward with
                                                                                            // negative Y (forward)
             .withVelocityY(-joystick1.getLeftX() * MaxSpeed) // Drive left with negative X (left)
-            .withRotationalRate(-joystick1.getRightX() * MaxAngularRate * 0.5) // Drive counterclockwise with negative X (left)
+            .withRotationalRate(-joystick1.getRightX() * MaxAngularRate * 0.5* (slow?0.3:1)) // Drive counterclockwise with negative X (left)
         ));
 
     joystick1.a().whileTrue(drivetrain.applyRequest(() -> brake));
@@ -92,17 +105,40 @@ private void configureBindings() {
     moveArmUp.whileTrue(ArmCommands.MoveArmUpCommand());
     Trigger moveArmDown = new Trigger(() -> joystick2.getLeftY()< -0.1);
     moveArmDown.whileTrue(ArmCommands.MoveArmDownCommand());
-    Trigger stopArm = new Trigger(() -> Math.abs(joystick2.getLeftY())<0.1);
+    Trigger moveArmUpSlow = new Trigger(() -> joystick2.getRightY()>0.1);
+    moveArmUpSlow.whileTrue(ArmCommands.MoveArmUpSlowCommand());
+    Trigger moveArmDownSlow = new Trigger(() -> joystick2.getRightY()<-0.1);
+    moveArmDownSlow.whileTrue(ArmCommands.MoveArmDownSlowCommand());
+    Trigger stopArm = new Trigger(() -> Math.abs(joystick2.getLeftY())<0.1 && Math.abs(joystick2.getRightY())<0.1);
     stopArm.whileTrue(ArmCommands.stopArm());
-    // Trigger presetArm = new Trigger(() -> joystick2.getLeftY()== 0.0);
-    // presetArm.whileTrue(ArmCommands.setArmCommand(0));
+
+    Trigger armToScore = new Trigger(joystick2.rightBumper());
+    armToScore.onTrue(ArmCommands.setTargetPositionCommand(Constants.scorePreset));
+
+    Trigger armToSource = new Trigger(joystick2.b());
+    armToSource.onTrue(ArmCommands.setTargetPositionCommand(Constants.sourcePreset));
+
+    Trigger armToGround = new Trigger(joystick2.a());
+    armToGround.onTrue(ArmCommands.setTargetPositionCommand(Constants.groundPreset));
+
+      Trigger slowMode = new Trigger(joystick1.rightTrigger());
+      slowMode.onTrue( new InstantCommand(()->{slow=true;}));
+      slowMode.onFalse( new InstantCommand(()->{slow=false;}));
+
+
   } 
 
   public RobotContainer() {
     configureBindings();
+    // NamedCommands.registerCommand("StackBucket", AutoCommands.stackBucket());
+    // NamedCommands.registerCommand("ScoreBucket", AutoCommands.scoreBucket());
+
+    autoChooser = AutoBuilder.buildAutoChooser();
+
+    SmartDashboard.putData(autoChooser);
   }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return autoChooser.getSelected();
   }
 }