Python pid.PID类代码示例

def control_velocity():
    global pid, v_est

    rospy.init_node('controller_velocity')

    motor_pub = rospy.Publisher('motor_pwm', ECU, queue_size = 10)
    rospy.Subscriber('v_ref', Vector3, v_ref_callback, queue_size=10)
    rospy.Subscriber('state_estimate', Z_KinBkMdl, state_estimate_callback, queue_size=10)

    # set node rate
    rateHz  = 50
    dt      = 1.0 / rateHz
    rate    = rospy.Rate(rateHz)

    # initialize motor_pwm and v_est at neutral
    v_est = 0
    motor_pwm = 90

    # use pid to correct velocity, begins with set point at zero
    p       = rospy.get_param("controller/p")
    i       = rospy.get_param("controller/i")
    d       = rospy.get_param("controller/d")
    pid     = PID(P=p, I=i, D=d)

    # main loop
    while not rospy.is_shutdown():
        u = pid.update(v_est, dt)

        motor_pwm = motor_pwm + u

        # send command signal
        motor_cmd = ECU(motor_pwm, 0)
        motor_pub.publish(motor_cmd)

        rate.sleep()

    def fitness(self, individual, target):
        """
        Check the final value of running the simulation and the time
        constant tau where y is ~63.2% of the target value.
        """
        dt = self._sample_period
        tau = self._tau

        pid = PID(Kp=individual[0],
                  Ki=individual[1],
                  Kd=individual[2])

        system = self._system_cls(*self._system_args, **self._system_kwargs)

        t = range(0, self._rise_time, dt)
        y = []
        for i in t:
            output = system.measurement
            pid.update(output, target, dt=dt)
            y.append(output)
            system.update(pid.control)

        if tau is not None:
            t632 = self._time_near(t, y, 0.632 * target)
            tau_diff = abs(tau - t632)
        else:
            tau_diff = 0

        return abs(target - y[-1]) + tau_diff

def main_auto():
    # initialize ROS node
    rospy.init_node('auto_mode', anonymous=True)
    rospy.Subscriber('imu', TimeData, imu_callback)
    rospy.Subscriber('encoder', Encoder, encoder_callback)
    rospy.Subscriber('speed', SPEED, speed_callback)
    nh = rospy.Publisher('mot', MOT, queue_size = 10)
    log = rospy.Publisher('state', STATE, queue_size = 10)

	# set node rate
    rateHz  = 50
    rate 	= rospy.Rate(rateHz)
    dt      = 1.0 / rateHz 
    p       = rospy.get_param("longitudinal/p")
    i       = rospy.get_param("longitudinal/i")
    d       = rospy.get_param("longitudinal/d")
    pid     = PID(P=1, I=1, D=0)

    # main loop
    while not rospy.is_shutdown():

        # publish command signal 
        # TODO
        global v_x, Ww_Reff, desired_slip_ratio
        slip_ratio      = (Ww_Reff -v_x) /Ww_Reff
        err_slip_ratio  = slip_ratio -desired_slip_ratio
        motor_PWM       = pid.update(err_slip_ratio)
        # motor_PWM       = 90

        nh.publish(MOT(motor_PWM))
        log.publish(STATE(Vx, Ww_Reff, err))
	
        # wait
        rate.sleep(sleep_time)

def main_auto():
    # initialize ROS node
    rospy.init_node('auto_mode', anonymous=True)
    nh = rospy.Publisher('serv', SERV, queue_size = 10)
    rospy.Subscriber('imu', TimeData, imu_callback)

	# set node rate
    rateHz  = 50
    dt      = 1.0 / rateHz
    rate 	= rospy.Rate(rateHz)
	
    # use simple pid control to keep steering straight
    p 		= rospy.get_param("lateral/p")
    i 		= rospy.get_param("lateral/i")
    d 		= rospy.get_param("lateral/d")
    pid     = PID(P=p, I=i, D=d)

    # main loop
    while not rospy.is_shutdown():
        # get steering wheel command
        u         = pid.update(err, dt)
        servoCMD  = angle_2_servo(u)
			
        # send command signal
        serv_cmd = SERV(servoCMD)
        nh.publish(serv_cmd)
	
        # wait
        rate.sleep()

 def __init__(self):
     # Set PID structures to intial values
     self.rightPID = PID(-200, 0, 0, 0, 0, 0, 0)
     self.rightPID.setPoint(0)
     self.leftPID = PID(200, 0, 0, 0, 0, 0, 0)
     self.leftPID.setPoint(0)
     
     self.somethingWentWrong = False
     
     # Ensure that data is fresh from both wheels
     self.newDataM1 = False
     self.newDataM2 = False
     self.newData = False
     
     # IMU structures
     self.currentLeftV = MovingAverage()
     self.currentRightV = MovingAverage()
     
     # Roboclaw
     self.myRoboclaw = RoboClaw()
     
     # ROS subscribers
     rospy.Subscriber("/cmd_vel",  Twist,  self.commandCallback)
     rospy.Subscriber("/imu1", Imu, self.imu1Callback)
     rospy.Subscriber("/imu2", Imu, self.imu2Callback)
     time.sleep(1)
     
     self.lastDataTime = time.time()
     
     self.repeatCount = 0
     self.currentM1Measurement = 0
     self.currentM2Measurement = 0
     self.lastM1Measurement = 0
     self.lastM2Measurement = 0

    def __init__(self):
        self.left_wall_pid = PID(14, 2, 0.5)
        self.right_wall_pid = PID(14, 2, 0.5)
        self.left_wall_pid.setpoint = 0.055
        self.right_wall_pid.setpoint = 0.055

        self.goal_angle = None
        self.goal_distance = None

        self.start_position = None
        self.position = None
        self.start_angle = None
        self.angle = None

        self.left_dist = 0
        self.right_dist = 0
        self.front_dist = 0

        self.commands = []

        self.busy = False

        self.callback = None

        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        rospy.Subscriber("/wheel_odom", Odometry, self.update)
        rospy.Subscriber("/sensors/ir_left", Float32, self.ir_left)
        rospy.Subscriber("/sensors/ir_right", Float32, self.ir_right)
        rospy.Subscriber("/sensors/ir_front", Float32, self.ir_front)

    def __init__(self, quad, max_force=10, min_force=0.02):
        self.quad = quad
        # set max and min forces
        self.max_force = max_force
        self.min_force = min_force
        # commands
        self.command_pitch = 0
        self.command_roll = 0
        self.command_yaw = 0
        self.command_acceleration = 0

        # acceleration set points
        self.set_point_acceleration = 0
        self.acceleration = PID(4, 0.8, 2)
        self.acceleration.setPoint(self.set_point_acceleration)
        # Pitch set point
        self.set_point_pitch = 0
        self.pitch = PID(0.02, 0.001, 0.10)
        self.pitch.setPoint(self.set_point_pitch)
        # Roll set point
        self.set_point_roll = 0
        self.roll = PID(0.02, 0.001, 0.10)
        self.roll.setPoint(self.set_point_roll)
        # Yaw set point
        self.lock_yaw = True
        self.set_point_yaw = 90
        self.yaw = PID(0.2, 0.001, 1)
        self.yaw.setPoint(self.set_point_yaw)

 def __init__(self):
   Node.__init__(self)
   self.x_pid = PID(0.5, 0.0001, 0.001, 300)
   self.t_pid = PID(0.5, 0.005, 0.0, 0.87)
   self.target_pos = None
   self.last_seen = 0
   self.kick_offset = 50 
   self.finishCt = 0

 def __init__(self):
   Node.__init__(self)
   self.x = PID(0.5, 0.0001, 0.001)
   self.y = PID(0.5, 0.005, 0.0)
   self.target_pos = None
   self.last_seen = 0
   self.kick_offset = 50 
   self.finishCt = 0

class PID_Posicion(object):
    """docstring for PID_Posicion"""
    def __init__(self, kp = 1.0 , ki =0.0, kd =0.0 , pinA = 4, pinB = 21):
        self.pid  = PID(kp , ki , kd)        
        self.motor = Motor_encoder(pinA , pinB)

    def SetPoint(self , setpoint):
        self.pid.setPoint(abs(setpoint))	
        self.motor.avance(setpoint)

  def __init__(self, P=2.0, I=0.0, D=1.0, Derivator=0, Integrator=0, \
    Integrator_max=500, Integrator_min=-500, set_point=10):
    print "in pid44: ", P,I,D

    self.current=[0, 0, 0, 0, 0]
    self.data_number=0

    PID.__init__(self, P, I, D, Derivator=0, Integrator=0, \
      Integrator_max=500, Integrator_min=-500)
    PID.setPoint(self, set_point)

 def integrate(self, n, state, dt):
     pid = PID(kp=self.kp, ki=self.ki, kd=self.kd, angle=self.pidAngle, dt=dt)
     history = np.array([state])
     theta = Robot._get_gyroscope(state[2])
     for i in range(n):
         self.u = pid.execute_pid(theta)
         state = rk4(self.dynamics, state, i, dt)
         theta = Robot._get_gyroscope(Robot._get_gyroscope(state[2]))
         history = np.append(history, [state], axis=0)
     return history

class Interrupt(object):
	"""Clase elaborada por Luis Borbolla para interrupcion por hardware en raspberry pi """
	def __init__(self, pin , pin_pwm , pin_pwmR):
		self.avance = 0 
		self.pin = pin
		self.pin_pwmR = pin_pwmR
		self.cuenta = 0
		self.index = 0 
		self.velocidad = 0
		self.tiempo_anterior = time.time()
		self.tiempo_actual = 0
		self.pin_pwm = pin_pwm
		GPIO.setmode(GPIO.BOARD)
		GPIO.setup(self.pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)
		GPIO.add_event_detect(self.pin, GPIO.FALLING, callback=self.interrupcion, bouncetime=5)
		GPIO.setup(24, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
		self.pwm_llanta = PWM(self.pin_pwm)
		self.pwm_llantar = PWM(self.pin_pwmR)
		self.pid_vel  = PID()


	def setup(self , setPoint):
		self.pwm_llanta.start()
		if setPoint >0:
		    self.pwm_llanta.set_duty(setPoint/15.0)

		else:
		    self.pwm_llantar.set_duty(setPoint/15.0)
		self.pid_vel.setPoint(setPoint)
		
	def interrupcion(self,channel):
		#print 'velocidad = %s' % self.pin
		self.cuenta += 1
		self.tiempo_actual = time.time()
		self.avance += 10		
		prom_tiempos = (self.tiempo_actual-self.tiempo_anterior)/2
		self.velocidad = 9.974556675147593/prom_tiempos   #2.5*25.4*m.pi/20 == 9.974556675147593
	    	self.tiempo_anterior = self.tiempo_actual
		error = self.pid_vel.update(self.velocidad)
  		self.pwm_llanta.update(error/15.0)


	

	def esperar_int(self):
    	
		try:
    		    print "Waiting for rising edge on port 24"
    		    GPIO.wait_for_edge(24, GPIO.RISING)
    		    print "Rising edge detected on port 24. Here endeth the third lesson."

		except KeyboardInterrupt:
    		    GPIO.cleanup()       # clean up GPIO on CTRL+C exit
		GPIO.cleanup()           # clean up GPIO on normal exit

 def __init__(self, sensor1, sensor2, ir_device_func, diff_k_values=(0,0,0), dist_k_values=(0,0,0)):
     self.sensor1 = sensor1
     self.sensor2 = sensor2
     self.get_values = ir_device_func
     self.diff_pid = PID()
     p1,d1,i1 = diff_k_values
     # self.diff_pid.set_k_values(1.1, 0.04, 0.0)
     self.diff_pid.set_k_values(p1, d1, i1)
     self.dist_pid = PID()
     p2,d2,i2 = dist_k_values
     self.dist_pid.set_k_values(p2, d2, i2)

class motor_control():
  def __init__(self): 
    #pid loop for each motor
    self.pid0=PID(P=0.0001, I=0.00001)
    self.pid1=PID(P=0.0001, I=0.00001)
    
    self.high_power_limit=0.70
    self.low_power_limit=0.15
    self.low_rpm_limit=250
    self.deadband=0.1
      
    self.pub = rospy.Publisher('motor_power', MotorPower, queue_size=10)
    
    self.motor_power=MotorPower()
    self.motor_power.power1=0
    self.motor_power.power2=0
    
  def set_motor_power(self):
    self.motor_power.power1=self.pid0.PID+self.motor_power.power1
    self.motor_power.power2=self.pid1.PID+self.motor_power.power2
    
    if self.pid0.getPoint() > 0 and self.pid0.getPoint() < self.low_rpm_limit:
      self.motor_power.power1=self.low_power_limit
    elif self.pid0.getPoint() < 0 and self.pid0.getPoint() > -self.low_rpm_limit:
      self.motor_power.power1=-self.low_power_limit
    elif self.pid0.getPoint() == 0:
      self.motor_power.power1=0
    elif self.motor_power.power1 > self.high_power_limit:
      self.motor_power.power1=self.high_power_limit
    elif self.motor_power.power1 < self.low_power_limit:
     # if self.motor_power.power1 > -self.low_power_limit+self.deadband and self.motor_power.power1 < self.low_power_limit-self.deadband: 
     #   self.motor_power.power1=0
      if self.motor_power.power1 < 0 and self.motor_power.power1 > -self.low_power_limit:
        self.motor_power.power1 = -self.low_power_limit
      elif self.motor_power.power1 < -self.high_power_limit:
        self.motor_power.power1= -self.high_power_limit
      else:
        self.motor_power.power1=self.low_power_limit

    if self.pid1.getPoint() > 0 and self.pid1.getPoint() < self.low_rpm_limit:
      self.motor_power.power2=self.low_power_limit
    elif self.pid1.getPoint() < 0 and self.pid1.getPoint() > -self.low_rpm_limit:
      self.motor_power.power2=-self.low_power_limit
    elif self.pid1.getPoint() == 0:
      self.motor_power.power2=0
    elif self.motor_power.power2 > self.high_power_limit:
      self.motor_power.power2=self.high_power_limit
    elif self.motor_power.power2 < self.low_power_limit:
      if self.motor_power.power2 < 0 and self.motor_power.power2 > -self.low_power_limit:
        self.motor_power.power2 = -self.low_power_limit
      elif self.motor_power.power2 < -self.high_power_limit:
        self.motor_power.power2= -self.high_power_limit
      else:
        self.motor_power.power2=self.low_power_limit