Pathplanner NamedCommands not running

Hello!

I’ve been working on some Pathplanner NamedCommands but every single time I try to run them my path either:

  1. If the NamedCommands are deferred, the first part of the parallel path runs and then the auton stops
  2. If the NamedCommands are instant, the auton doesn’t run at all

The Command I am trying to run works perfectly in TeleOP, which is odd.

Attached below is an image of part of my auton + a Github Gist to parts of my code. Any help here would be super apprecited!

I could be wrong but it seems like your command is just setting the member variable of the class and not telling the arm to actually move.

  public Command raiseArm() {
    return runOnce(() -> goalSetpoint = 2.47);
  }

If you did something more like this it might work

  public Command raiseArm() {
    return runOnce(() -> goalSetpoint = 2.47;  moveArm());
  }

That may not work so creating a new method might be better to encapsulate that code.

public void setArmMoveAuto() {
       goalSetpoint = 2.47;  
       moveArm();
}
  public Command raiseArm() {
    return runOnce(() -> setArmMoveAuto());
  }

Alternatively and likely the more ideal way would be to pass the desired setPoint into the moveArm Method instead of using a global class variable.

public void moveArm(double desiredSetpoint) { 
// do stuff
var tempGoalState = trapProf.calculate(0.02, prevState, new State(desiredSetpoint, 0));
//do stuff
}

Thanks! I’ll try that. The main thing I was thinking is that in my RobotContainer, I set the m_arm Default Command to moveArm (on init, the desired setpoint is where the arm currently is. After either bumper is pressed, the setpoint changes between the upper limit and the lower limt), so just altering the setpoint would work in Auton. If you don’t mind, could you explain where my logic/code is flawed? Thanks again :slight_smile:

I honestly can’t say if it’s flawed, I don’t remember and can’t find anything to confirm if Default commands run in Auton or not. I know we generally set up the default commands based on Teleop and then other commands specific to Auto to make sure actions happen.

The issue is your default command. They do run in auto but the way that requirements work in auto has an unintended impact of preventing the default command from running during auto if ANY other command in your auto uses the subsystem ANY time during auto.

So…. You have 2 decent options.

1 - You can explicitly run your default command in your auto after every time you run a different command on your subsystem.

2 - Implement your default command logic in the periodic method of your subsystem.

For an arm where you are just changing the set points I would recommend #2. Basically you always want your arm moving to where it should be…. So it works well in that scenario in the periodic.

1 Like

Thanks! Could you help me with the code?

    NamedCommands.registerCommand("raiseArm", Commands.run(() -> m_arm.raiseArm().asProxy()));


    NamedCommands.registerCommand("lowerArm", Commands.run(() -> m_arm.lowerArm().asProxy()));

and

package frc.robot.subsystems;

import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkPIDController;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.wpilibj.DriverStation;
// import edu.wpi.first.units.Units;
// import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.ArmConstants;
import monologue.Logged;
import monologue.Annotations.Log;

import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.SparkAbsoluteEncoder.Type;

public class Arm extends SubsystemBase implements Logged {

  //TODO: Try decreasing motion profile speed but increase kP to a higher value


  // kS, kG, kV, and kA values for Feedforward Control
  // We will most likely just be using kG to fight gravity, deferring to the PID
  // for main control
  public static final double kArmS = 0.072657; // Volts
  public static final double kArmG = 0.33; //0.29615; // Volts
  public static final double kArmV = 8.1772; // Volts * Seconds / Radians
  public static final double kArmA = 1.2082; // Volts * Seconds^2 / Radians

  // //kP, kI, and kD values for PID Control
  // We will most likely just be using kP to control the robot, don't use kI or kD
  // TODO: Increment kP in very small values after kG is tuned (a good kG holds position when parallel with the ground)
  public static final double kArmP = 5.90; //5.7942 - Pos
  public static final double kArmI = 0;
  public static final double kArmD = 0; //1.3463 - Pos

  // Create a new CANSparkMax motor controller
  private final CANSparkMax m_armMotor;

  // Create a new ArmFeedforward controller
  private final ArmFeedforward m_armFeedforward;

  // Create a new SparkPIDController
  private final SparkPIDController m_armPIDController;

  // Create a new AbsoluteEncoder for the arm
  private final AbsoluteEncoder m_armAbsoluteEncoder;

  // Create a new SysIDRoutine for SysID tuning
  // private final SysIdRoutine routine;

  public double goalSetpoint;

  public boolean slowMode;

//Create a new TrapezoidProfile for rate limiting/smooth control
//TODO: Before testing autons, reduce this to lower values for safety
TrapezoidProfile trapProf = new TrapezoidProfile(new Constraints(Math.PI, Math.PI));
State prevState = new State(0, 0);


  // Arm Constructor
  public Arm(int armCanID) {

    // Initialize the arm motor with the correct CANID and motor type
    m_armMotor = new CANSparkMax(armCanID, MotorType.kBrushless);

    // // Create the SysId routine for SysID tuning
    // routine = new SysIdRoutine(
    //     new SysIdRoutine.Config(
    //         Volts.of(1).per(Units.Second), // Ramp Rate of 1v/s (default)
    //         Volts.of(2), // Step Voltage of 2 volts
    //         Units.Seconds.of(10) // 10 Second timeout (default)
    //     ),
    //     new SysIdRoutine.Mechanism(
    //         (voltage) -> m_armMotor.setVoltage(voltage.in(Volts)),
    //         null, // No log consumer, since data is recorded by URCL
    //         this));

    // Initialize the arm's absolute encoder in DutyCycle mode
    m_armAbsoluteEncoder = m_armMotor.getAbsoluteEncoder(Type.kDutyCycle);

    // Initialize the SparkMax PID Controller
    m_armPIDController = m_armMotor.getPIDController();

    // Make sure the PID controller is linked to the absolute encoder
    m_armPIDController.setFeedbackDevice(m_armAbsoluteEncoder);

    // //Input PID values for the SparkMax PID
    m_armPIDController.setP(kArmP);
    m_armPIDController.setI(kArmI);
    m_armPIDController.setD(kArmD);

    // Add a ConversionFactor the AbsoluteEncoder
    m_armMotor.getAbsoluteEncoder().setPositionConversionFactor(ArmConstants.kArmEncoderPositionFactor); // Units are radians

    m_armMotor.getAbsoluteEncoder().setVelocityConversionFactor(ArmConstants.kArmEncoderVelocityFactor); // Units are radians/second


    // No need to add a zeroing of the Abs Encoder as it is already set up in the
    // Rev-HWC

    // Do NOT have PID wrapping as having PID wrapping will have the arm move in a
    // circle (which is very bad)
    m_armPIDController.setPositionPIDWrappingEnabled(false);

    // Burn settings to flash in case of a brownout/power failure
    m_armMotor.burnFlash();

    // Create a new ArmFeedforward with the constants kS, kG, kV, and kA
    m_armFeedforward = new ArmFeedforward(kArmS, kArmG, kArmV, kArmA);

    // Set the goalSetpoint to the current position
    // This is temporary, just for init.
    goalSetpoint = getAbsoluteEncoderPosition();

    // Set the prev_state
    prevState = new State(getAbsoluteEncoderPosition(), 0);

    //Set slowMode to false on init
    slowMode = false;

  }

  // Get the current position of the absolute encoder
  public double getAbsoluteEncoderPosition() {
    return m_armAbsoluteEncoder.getPosition();
  }

  // @Override
  // public void periodic() {
  //   Commands.run(() -> moveArm());
  //   // moveArm();
  // }

  @Log.NT
  public double getGoalSetpoint(){
    return goalSetpoint;
  }




  /**
   * Method that moves the arm to a setpoint via a ArmFeedforward controller.
   * 
   * This method uses a position (angle) setpoint relative to the zeroed Absolute
   * Encoder.
   * The desired setpoint should take into account the numerical offset of the
   * Absolute Encoder
   */

  public void moveArm() {
    // Calculate the voltage based on the setpoint
    // The setpoint takes into account the Absolute Encoder offset of 1.4217575
    // double FFvoltage = m_armFeedforward.calculate(goalSetpoint - 1.4217575, 0);
    // If slowMode is active, reduce the max speed/accel, otherwise keep them at default

    if(slowMode){
      trapProf = new TrapezoidProfile(new Constraints(Math.PI/4, Math.PI/2));
    } else {
      trapProf = new TrapezoidProfile(new Constraints(Math.PI, Math.PI));
    }

    //TODO: Test this code
    var tempGoalState = trapProf.calculate(0.02, prevState, new State(goalSetpoint, 0));
    prevState = tempGoalState;
    double FFvoltage = m_armFeedforward.calculate(tempGoalState.position + 0.023805, tempGoalState.velocity);

    // var tempGoalState = trapProf.calculate(0.02, prevState, new State(goalSetpoint, 0));

    // prevState = tempGoalState;

    // double FFvoltage = m_armFeedforward.calculate(tempGoalState.position + 0.023805, 0);


    // Leave pidSlot at 0
    m_armPIDController.setReference(tempGoalState.position, CANSparkMax.ControlType.kPosition, 0, FFvoltage,
      SparkPIDController.ArbFFUnits.kVoltage);
  }

  // Set goalSetpoint to move arm for AMP scoring
  public Command raiseArm() {
    DriverStation.reportError("ARM RAISED RAHHHHHHHH", false);
    return runOnce(() -> goalSetpoint = 2.47);
  }

  // Set goalSetpoint to move arm for note pickup
  //Old arm val: 0.040
  public Command lowerArm() {
    DriverStation.reportError("ARM RAISED LOWERED RAHHHHHHHHH", false);
    return runOnce(() -> goalSetpoint = 0.081);
  }

  // //Set goalSetpoint to current pos. to prevent movement
  // public Command stopArmSetpoint() {
  //   // return runOnce(() -> goalSetpoint = getAbsoluteEncoderPosition());
  //   //TODO: Test this
  //    return runOnce(() -> goalSetpoint = prevState.position);
  // }

  // // Command factories for SysID
  // // Returns a command to run a Quasistatic test in the desired direction
  // // TODO: Not sure if I should have the max setpoint in here or in my
  // // RobotContainer.java
  // public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
  //   return routine.quasistatic(direction);
  // }

  // // Returns a command to run a Dynamic test in the desired direction
  // public Command sysIdDynamic(SysIdRoutine.Direction direction) {
  //   return routine.dynamic(direction);
  // }

}

I ran this code and I noticed the set point wasn’t adjusting at all.

Pinging @Hollisms here as well.

I modified my commands to the code below and got an odd result. The commands would not run unless I added a DriverStation.reportError (as well as moving the arm in teleOP beforehand, the effect was somewhat repreatable). I found that the command ran BUT the setpoint wasn’t altered (which is super odd, considering that’s what the command does. Not sure where my logic is flawed tbh. I can provide more of my code if that helps.

How is moveArm being called?

Perhaps put a print statement in it to make sure it is being called…. I suspect it is not being called because the only reference I see to it is the default command.

I think the issue was the command not being scheduled (I was using runOnce and not Commands.runOnce). I am not 100% sure and I will update this thread if it now works or I’m still having the issue

:slight_smile: