The TurnToHub command looks good! Currently the command ends when the robot is pointed +- 5 degrees of the target. In teleop, robots could bump us around. It would be good to have the command indicate it is in range, and then keep running to keep us pointed there. You could run the command, and then follow it immediately with another with the end condition removed:https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html#perpetually
I often found it useful to have toggle buttons on shuffleboard, to switch the RPM setpoint between 0, and the value set on the shuffleboard. This allows you to quickly iterate on pid gains by having the wheels stop and start again. Or, you could disabled the robot to let the wheels stop. But make sure to set the sparkmax setpoint to 0 when disabled! Otherwise when the robot is enabled the wheel will automatically start spinning again. Although this is worth testing.
Finally, when stopping the wheel, consider setting it to 0 open loop, instead of 0 rpm. That way it will simply coast to 0, instead of the PID loop trying to force it to 0, which seems unnecessary
The TurnToHub command looks good! Currently the command ends when the robot is pointed +- 5 degrees of the target. In teleop, robots could bump us around. It would be good to have the command indicate it is in range, and then keep running to keep us pointed there. You could run the command, and then follow it immediately with another with the end condition removed:https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html#perpetually
Or, the command never ends, but
Math.abs(modAngle(error)) <= errorTolerance
is moved into a function calledinRange
in PhotonLimelight, which is used to trigger the shooting to start with a WaitUntilCommand https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html#waituntilcommandOr something else, there's lots of ways of doing this.