Auto Align with Limelight Issue

Our team just finish a auto align code for speaker. However, it do not do anything. We have tried various way to slove it, but have not successed yet. Can anyone provide some suggestions? Below are some details and code.

Basically, the issue we meet now is that our arm do nothing when we press the button. To check if it run the command, I put a smartdashboard value in command, and it show that it runs the command. Furthermore, it also not PID’s problem because we have buttons that make our arm to specific positions and it works well.

Can anyone tell what might be the problem?

what file and what method?

It is autoAlign.java in commands, based on ArmSubsystem.java and Limelight.java.

The execute command doesn’t move the arm. It calls m_armSubsystem.pidCommand(), which creates a new command, but then you never schedule it.

autoAlign.java

 @Override
 public void execute() {
     double tagSize = m_limelightSubsystem.getTA();
     double autoAglinGoal = 0.1-((tagSize-0.1)/5);
     SmartDashboard.putNumber("ta", tagSize);
     SmartDashboard.putNumber("autoAglin", autoAglinGoal);
     m_armSubsystem.pidCommand(autoAglinGoal);
}

The last line calls the pidCommand() method. It seems like you’re expecting that to do something, but it just creates a command:

public Command pidCommand(double position) {
    return run( () -> leftArmMotor.setControl(m_mmReq.withPosition(position).withSlot(0)))
            .until( () -> atSetPoint(position))
            .andThen(() -> this.stopArm());
}

Thanks for your response.

Do you mean I should change this to void?

No, that method returns a command. I think you intended to call a method that makes the leftArmMotor do something. You probably need a different public method that takes a position and calls leftMotor.setControl().