ros_controle_pd

#!/usr/bin/env python

import rospy

from std_msgs.msg import Float64

angulo = 0.0;
setpoint = 20.0
erro = 0
termo_pd = 0
termo_pd_escalonado = 0

def posi_callback(msg):
    global angulo
    angulo = msg.data
    #rospy.loginfo("Recebi: " % (posicao))

def corr_callback(msg):
    global corrente
    mVperAmp = 100;
    ACSoffset = 2500;

    corrente = msg.data
    mV = (corrente / 1024.0) * 5000;
    corrente = ((mV - ACSoffset) / mVperAmp);
    #rospy.loginfo("Recebi: " % (corrente))

def pd_calc():
    global setpoint, termo_pd_escalonado
    Kp = 5.0
    Kd = 10.0
    ultimo_erro = 0
    #FREIO NO ARDUINO
    erro = setpoint - angulo
    delta_erro = erro - ultimo_erro
    termo_pd = (Kp * erro) + (Kd * delta_erro)
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;

def constrain(x,a,b):
    if x < a:
        x = a
    elif x > b:
        x = b
    else:
        x = x
    return x

if __name__=='__main__':
    rospy.init_node('no')

    sub_p = rospy.Subscriber('posicao', Float64, posi_callback)
    sub_c = rospy.Subscriber('corrente', Float64, corr_callback)
    pub_ctr = rospy.Publisher('controle', Float64, queue_size = 10)
    pub_set = rospy.Publisher('setpt', Float64, queue_size = 10)
    rate = rospy.Rate(50)

    while not rospy.is_shutdown():
        pd_calc()
        pub_ctr.publish(termo_pd_escalonado)
        pub_set.publish(setpoint)
        rate.sleep()