Python PID.update方法代码示例

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
    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

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
class ControllerDaemon(AtlasDaemon):
    def _init(self):
        self.logger.info("Initializing controller daemon.")
        self.temp_target = 19
        self.update_time = 0
        self.period = 5 * 60
        self.duty_cycle = 0
        self.pid = PID()
        self.pid.setPoint(self.temp_target)
        self.pin = OutputPin("P8_10")
        self.pin.set_low()
        self.heater = State.off

        self.subscriber = self.get_subscriber(Topic("temperature", "ferm1_wort"))
        self.wort_temp = Average(self.subscriber.topic.data)
        topic = Topic("temperature", "ferm1_wort_average", self.wort_temp.get_value())
        self.publisher_average_temp = self.get_publisher(topic)
        topic = Topic("pid", "ferm1_pid", self.pid.update(self.wort_temp.get_value()))
        self.publisher_pid = self.get_publisher(topic)
        topic = Topic("state", "ferm1_heating", self.heater)
        self.publisher_heater = self.get_publisher(topic)
        topic = Topic("temperature", "ferm1_target", self.temp_target)
        self.publisher_target = self.get_publisher(topic)
        topic = Topic("percentage", "ferm1_duty_cycle", self.duty_cycle)
        self.publisher_duty_cycle = self.get_publisher(topic)

        self.logger.info("Controller initialized.")

    def _loop(self):
        self.wort_temp.update(self.subscriber.topic.data)
        self.publisher_average_temp.publish(self.wort_temp.get_value())
        pid = self.pid.update(self.wort_temp.get_value())
        self.publisher_pid.publish(pid)
        self.publisher_target.publish(self.temp_target)
        if pid < 1:
            self.duty_cycle = pid
        else:
            self.duty_cycle = 1
        self.publisher_duty_cycle.publish(self.duty_cycle)

        if self.update_time + self.period < time.time():
            self.update_time = time.time()
            if 0 < self.duty_cycle:
                self.heater = State.on
                self.pin.set_high()
        elif self.update_time + self.duty_cycle * self.period < time.time():
            if self.heater == State.on:
                self.heater = State.off
                self.pin.set_low()

        self.publisher_heater.publish(self.heater)
        time.sleep(1)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
  class FinalApproach(Node):
    targetDistance = 100
    def __init__(self):
      Node.__init__(self)
      self.x = PID(0.5, 0.001, 0.001, 300)
      self.y = PID(0.5, 0.001, 0.0, 180) # was 0.0005
      self.target_pos = None
      self.last_seen = 0
      self.kick_offset = 50
      self.finishCt = 0
      b = world_objects.getObjPtr(core.WO_BALL)
      self.initialTheta = b.visionBearing 

    def run(self):
      ball = world_objects.getObjPtr(core.WO_BALL)
      if ball.seen:
        print "Ball Distance: ", ball.visionDistance
        self.last_seen = 0
        ball_x, ball_y = self.get_object_position(ball)
        if ball_x < 160 and abs(ball_y - self.kick_offset) < 30:
          self.finishCt += 1
        else:
          self.finishCt = max(self.finishCt - 1, 0)

        xVel = self.x.update(ball_x)
        yVel = self.y.update(ball_y - self.kick_offset)
        tVel = (ball.visionBearing - self.initialTheta) / 0.87

        xVel = max(min(xVel,0.5),-0.5)
        yVel = max(min(yVel,1.0),-1.0)
        tVel = 0 #max(min(tVel,1.0),-1.0)

        if tVel * yVel > 0:
          tVel = tVel * -1

        print "Final: {0}, {1}, {2} --> Walk Velocity: {3}, {4}, {5}".format(ball_x, ball_y - self.kick_offset, ball.visionBearing - self.initialTheta, xVel, yVel, tVel)
        commands.setWalkVelocity(xVel, yVel, tVel)
      else:
        self.last_seen += 1
      if self.last_seen > 10:
        commands.setWalkVelocity(0, 0, 0)
        self.finishCt = max(self.finishCt - 1, 0)
      if self.finishCt > 3:
        self.x.reset()
        self.y.reset()
        self.finish()

    def get_object_position(self, obj):
      obj_x = math.cos(obj.visionBearing) * obj.visionDistance
      obj_y = math.sin(obj.visionBearing) * obj.visionDistance
      return (obj_x, obj_y)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
  class Approach(Node):
    targetDistance = 300
    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 run(self):
      commands.setHeadPan(0.0,0.2)
      ball = world_objects.getObjPtr(core.WO_BALL)
      if ball.seen and ball.visionDistance < 3000:
        print "Ball Distance: ", ball.visionDistance
        self.last_seen = 0
        ball_x, ball_y = self.get_object_position(ball)        
        bearing = ball.visionBearing;
        if ball_x < 500 and abs(bearing) < 10 * core.DEG_T_RAD:
          self.finishCt += 1
        else:
          self.finishCt = max(self.finishCt - 1, 0)

        xVel = self.x_pid.update(ball_x)
        yVel = 0 #self.y.update(ball_y - self.kick_offset) / 200.0
	tVel = self.t_pid.update(bearing)

        xVel = max(min(xVel,0.5),-0.5)
        yVel = max(min(yVel,1.0),-1.0)
        tVel = max(min(tVel,1.0),-1.0)

        print "Approach: {0}, {1} --> Walk Velocity: {2}, {3}".format(ball_x, bearing, xVel, tVel)
        commands.setWalkVelocity(xVel, yVel, tVel)
      else:
        self.last_seen += 1
      if self.last_seen > 10:
        print "Search!"
        commands.setWalkVelocity(0, 0, 0.4)
        self.finishCt = max(self.finishCt - 1, 0)
      if self.finishCt > 3:
        self.x_pid.reset()
        self.t_pid.reset()
        self.finish()

    def get_object_position(self, obj):
      obj_x = math.cos(obj.visionBearing) * obj.visionDistance
      obj_y = math.sin(obj.visionBearing) * obj.visionDistance
      return (obj_x, obj_y)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
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)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
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()

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
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()

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
  def update(self, current_value):
    if self.data_number < 5:
      self.current[self.data_number]=current_value
      if self.data_number==0:
        effective_current_value=current_value * 4.0
      else: 
        effective_current_value=4.0* (self.current[self.data_number]-\
          self.current[0]) /(self.data_number)
      self.data_number += 1
      print effective_current_value

      return PID.update(self, effective_current_value)
    else:
      self.current=[self.current[1], self.current[2], self.current[3],\
      self.current[4], current_value]
      print "current array: ", self.current
      print "error value: ", self.current[4]-self.current[0]

      return PID.update(self, self.current[4]-self.current[0])

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
def main():
    pylab.clf()

    resistance = 4000.0
    generations = 1000
    population = 100
    steps = 100
    setpoint = 100  # Watts

    tuner = GeneticTuner(population, steps / 8, 1, Circuit,
                         system_args=(resistance, ),
                         max_p=1.0,
                         max_i=1.0,
                         max_d=0.1,
                         tau=steps / 64,
                         )
    Kp, Ki, Kd = tuner.find_gains(setpoint, iterations=generations)
    fitness = tuner.fitness((Kp, Ki, Kd), setpoint)

    c = Circuit(resistance)
    pid = PID(Kp=Kp, Ki=Ki, Kd=Kd)

    power = []
    voltage = []
    for i in xrange(steps):
        if i < steps / 2:
            pid.update(c.power, setpoint)
        else:
            pid.update(c.power, setpoint / 2)
        c.set_source(pid.control)
        voltage.append(pid.control)
        power.append(c.power)

    pylab.plot(xrange(steps), power)
    pylab.xlabel("Time")
    pylab.ylabel("Power")
    pylab.title("Power across {} Ohm resistor. (Setpoint of {} W)".format(resistance, setpoint))
    pylab.savefig("power.png")
    print("Kp:{}, Ki:{}, Kd:{}".format(Kp, Ki, Kd))
    print("fitness:{}".format(fitness))

    return 0

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
  class FinalApproach(Node):
    targetDistance = 100
    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

    def run(self):
      ball = world_objects.getObjPtr(core.WO_BALL)
      if ball.seen:
        print "Ball Distance: ", ball.visionDistance
        self.last_seen = 0
        ball_x, ball_y = self.get_object_position(ball)
        if ball_x < 150 and abs(ball_y - self.kick_offset) < 40:
          self.finishCt += 1
        else:
          self.finishCt = max(self.finishCt - 1, 0)

        xVel = self.x.update(ball_x) / 300.0 
        yVel = self.y.update(ball_y - self.kick_offset) / 200.0

        xVel = max(min(xVel,0.5),-0.5)
        yVel = max(min(yVel,1.0),-1.0)

        print "Error: {0}, {1} --> Walk Velocity: {2}, {3}".format(ball_x, ball_y - self.kick_offset, xVel, yVel)
        commands.setWalkVelocity(xVel, yVel, 0.0)
      else:
        self.last_seen += 1
      if self.last_seen > 10:
        commands.setWalkVelocity(0, 0, 0)
        self.finishCt = max(self.finishCt - 1, 0)
      if self.finishCt > 3:
        self.finish()

    def get_object_position(self, obj):
      obj_x = math.cos(obj.visionBearing) * obj.visionDistance
      obj_y = math.sin(obj.visionBearing) * obj.visionDistance
      return (obj_x, obj_y)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
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

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
class MotionControl(object):
    def __init__(self):
        rospy.on_shutdown(self.stop)
        rospy.wait_for_service('uniboard_service')
        self.uniboard_service = rospy.ServiceProxy('uniboard_service', communication)
        rospy.Subscriber("/cmd_vel", Twist, self.set_vel)
        rospy.Subscriber("/vel", vel_update, self.update)
        self.vel_pid = PID(self.drive, 0.5, 0.01, 0.0,[-2, 2], [-1, 1], 'vel_pid')
        self.vel_pid.start()
        self.rot_pid = PID(self.rotation, 0.5, 0.01, 0.0, [-2.0, 2.0], [-0.5, 0.5], 'rot_pid')
        self.rot_pid.start()
        # The offset value between wheel power to drive rotation
        self.rotation_offset = 0.0

    def drive(self, power):
        self.uniboard_service('motor_left', 3, str(power), rospy.Time.now())
        self.uniboard_service('motor_right', 3, str(power+self.rotation_offset), rospy.Time.now())

    def set_vel(self, twist):
        self.vel_pid.set_target(twist.linear.x)
        self.rot_pid.set_target(twist.angular.z)


    def rotation(self, diff):
        self.rotation_offset = diff

    def stop(self):
        self.vel_pid.stop()
        self.rot_pid.stop()

    def start(self):
        self.vel_pid.start()
        self.vel_pid.reset()
        self.rot_pid.start()
        self.rot_pid.reset()

    def update(self, vel):
        linear = vel.linear_x
        angular = vel.angular_z
        self.vel_pid.update(linear)
        self.rot_pid.update(angular)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
class Roll(object):
    def __init__(self, quad):
        self.quad = quad
        self.command_roll = 0
        self.roll = PID(0.02, 0.001, 0.1)
        self.roll.setPoint(self.command_roll)

    def step(self):
        print self.command_roll
        if self.roll.getPoint() != self.command_roll:
            self.roll.setPoint(self.command_roll)
        pforce = self.roll.update(self.quad.roll)
        self.quad.motors_force = (0, 0, -pforce, pforce)
        self.quad.step()

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
    def autonomousControl(self):

        # Print system status and self test result.
        status, self_test, error = self.bno.get_system_status()
        print('System status: {0}'.format(status))
        print('Self test result (0x0F is normal): 0x{0:02X}'.format(self_test))
        # Print out an error if system status is in error mode.
        if status == 0x01:
            print('System error: {0}'.format(error))
            print('See datasheet section 4.3.59 for the meaning.')

        # Print BNO055 software revision and other diagnostic data.
        sw, bl, accel, mag, gyro = self.bno.get_revision()
        print('Software version:   {0}'.format(sw))
        print('Bootloader version: {0}'.format(bl))
        print('Accelerometer ID:   0x{0:02X}'.format(accel))
        print('Magnetometer ID:    0x{0:02X}'.format(mag))
        print('Gyroscope ID:       0x{0:02X}\n'.format(gyro))

        pitchPIDController = PID(0.7,0.1,0.1)

        self.setPwmForAllMotors(300)

        while True:
            heading, roll, pitch = self.bno.read_euler()

            pitchPIDOutput = pitchPIDController.update(pitch, 0)

            print('Heading={0:0.2F} Roll={1:0.2F} Pitch={2:0.2F}'.format(heading, roll, pitch))

            print('PITCH PID OUTPUT: '+str(pitchPIDOutput))

            motor1Value = int(round(self.motor1.getPwmValue()+pitchPIDOutput))
            motor2Value = int(round(self.motor2.getPwmValue()+pitchPIDOutput))

            motor3Value = int(round(self.motor3.getPwmValue()-pitchPIDOutput))
            motor4Value = int(round(self.motor4.getPwmValue()-pitchPIDOutput))

            print('motor1={0} motor2={1} motor3={2} motor4={3}'.format(motor1Value, motor2Value, motor3Value, motor4Value))

            self.motor1.setPwmValue(motor1Value)
            self.motor2.setPwmValue(motor2Value)
            self.motor3.setPwmValue(motor3Value)
            self.motor4.setPwmValue(motor4Value)

            time.sleep(1)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import update [as 别名]
    def test_return_to_zero_when_target_met(self):
        pid = PID(0.7,0.1,0.1)

        pitch = 100;
        counter = 0;

        while (pitch != 0 and counter != 100):
            output = pid.update(pitch, 0)

            pitch = pitch + output;

            if(pitch < 0):
              pitch = 0


            counter = counter + 1
            time.sleep(1)

        self.assertEqual(pitch, 0)