ROS PID fluctuates between min and max values

edit

I have a weird issue that no matter what I set my constants to my control effort just fluctuates between the set maximum and minimum values. I've tried a variety of solutions but none seem to work.

I am currently running Ubuntu 16.04 with ROS kinetic, but I don't think this is a kernel or anything low level specific issue.

Hardware wise I have the data from an IMU (just the yaw values aka -180 to 180) going into the plant state and each time that is updated (a few times per second) the set point is set to 0. The control effort fluctuates between -100 and 100 (or -10 and 10 based on the max values) even with extremely small kps and no kd and ki. Eventually, my control effort becomes just NaN.

Here is my launch file:

<launch>
        <node pkg="imu_vn_100" name="imu" type="imu_vn_100_cont_node" output="screen">
                <param name="port" type="string" value="/dev/ttyUSB0"/>
                <param name="baudrate" type="int" value="921600"/>
                <param name="frame_id" type="string" value="imu"/>
                <param name="imu_rate" type="int" value="100"/>
                <param name="binary_output" type="bool" value="true"/>
                <param name="enable_mag" type="bool" value="true"/>
                <param name="enable_pres" type="bool" value="true"/>
                <param name="enable_temp" type="bool" value="true"/>
                <param name="enable_frame_rotation" type="bool" value="true"/>
                <param name="frame_rotation_matrix" value="[1.0, 1.0, 1.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0]"/>
                <param name="sync_rate" type="int" value="20"/>
                <param name="sync_pulse_width_us" type="int" value="1000"/>
                <param name="imu_compensated" value="true"/>
        </node>

        <node pkg="bauv_motor_control" name="valueFeeder" type="valueFeeder.py" output="screen">
        </node>

        <node name="yaw_pid" pkg="pid" type="controller" output="screen" >
                <param name="Kp" value="1.0" />
                <param name="Ki" value="0.0" />
                <param name="Kd" value="0.0" />
                <param name="upper_limit" value="100" />
                <param name="lower_limit" value="-100" />
                <param name="windup_limit" value="100" />
                <param name="cutoff_frequency" value="20" />
                <param name="max_loop_frequency" value="105.0" />
                <param name="min_loop_frequency" value="95.0" />
                <param name="topic_from_controller" value="yaw_effort" />
                <param name="topic_from_plant" value="yaw_plant" />
                <param name="setpoint_topic" value="yaw_setpoint" />
                <param name="node_name" value="yaw_pid" />
                <param name="angle_error" value="true" />
                <param name="angle_wrap" value="2.0*180.0" />
        </node>
</launch>

Here is the file that pushes the values from the IMU to the PID loops (just from tests the data is pushed to the loop, so I believe the issue is somewhere with the loop and how I'm using it):

#valueFeeder.py
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
from std_msgs.msg import Float64

def pusher(data):
        yaw_plant_pub.publish(data.orientation.z)
        yaw_setpoint_pub.publish(0.0)

def passit(data):
        pass

def main():
        rospy.init_node('value_Feeder', anonymous=True)
        rospy.Subscriber('imu/imu', Imu, pusher)
        rospy.Subscriber('yaw_effort', Float64, passit)
        rospy.spin()

if __name__ == '__main__':
        first_time = True
        yaw_plant_pub = rospy.Publisher('yaw_plant', Float64, queue_size=10)
        yaw_setpoint_pub = rospy.Publisher('yaw_setpoint', Float64, queue_size=10)
        main()

I've spent a lot of time trying to figure it and at this point I don't remotely know where it ... (more)