Hi, I am working on a command that auto aligns to the center of the goal by rotation and we are getting some overshoot with it so I am implementing a PID control loop. However, the PIDController class takes in a PIDSource and outputs a PIDOutput, so I wrote wrapper classes to write and read raw values (like ints and floats) to a PIDOutput and PIDSource. However, when I call the Enable function on the controller, it instantly crashes. I don’t really know why this is happening.
It looks like you are using the wrong shared pointer, both Robot and RobotMap they have a shared pointer named visionAlignPID. It looks like the one in Robot does not contain a pointer, while in RobotMap you set the visionAlignPID shared pointer to a PIDController.
It looks like you’re using a system that centers itself in the pixel. Jared Russel has talked before about why this isn’t a good idea, but the general complaint is that a camera has too high of a latency (even when plugged in via USB or equivalent) to be used in a PID loop. Instead of calculating the offset from the center, try and calculate values that might help you align. The distance, azimuth, and altitude are values that you might be most concerned about.