Python PID.setPoint方法代码示例

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

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

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [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 setPoint [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 setPoint [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 setPoint [as 别名]
def main_auto():
    global read_yaw0, yaw_local

    # initialize ROS node
    init_node('auto_mode', anonymous=True)
    ecu_pub = Publisher('ecu', ECU, queue_size = 10)
    se_sub = Subscriber('imu/data', Imu, imu_callback)

	# set node rate
    rateHz      = get_param("controller/rate") 
    rate 	    = Rate(rateHz)
    dt          = 1.0 / rateHz

    # get PID parameters
    p 		= get_param("controller/p")
    i 		= get_param("controller/i")
    d 		= get_param("controller/d")
    pid     = PID(P=p, I=i, D=d)
    setReference    = False
    
    # get experiment parameters 
    t_0             = get_param("controller/t_0")     # time to start test
    t_f             = get_param("controller/t_f")     # time to end test
    FxR_target      = get_param("controller/FxR_target")
    t_params        = (t_0, t_f, dt)

    while not is_shutdown():

        # OPEN LOOP 
        if read_yaw0:
            # set reference angle
            if not setReference:
                pid.setPoint(yaw_local)
                setReference    = True
                t_i             = 0.0
            # apply open loop command
            else:
                (FxR, d_f)      = straight(t_i, pid, t_params, FxR_target)
                ecu_cmd             = ECU(FxR, d_f)
                ecu_pub.publish(ecu_cmd)
                t_i += dt
	
        # wait
        rate.sleep()

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
SETPOINT = 35.7

logger = logging.getLogger()
logging.basicConfig(level=logging.INFO)

lights = Relay(19,'lights')
# sun = Sun(lights,settings,MAXTEMP)
print 'lights set up on pin 19'
heatsinksensor = w1therm()

fans = Wind(13,18)
print 'fans set up on pin 13'
p = PID(1, 0, 0.5,0,0,100,0)
print 'PID set up'
p.setPoint(SETPOINT)
print 'PID set point to {}'.format(SETPOINT)
def main():
    try:
        lights.on()
        print 'lights on'
        fans.speed(50)
        sleep(60)
        while True:
            # read heatsink temps, return max
            listoftemps = []
            temps = heatsinksensor.gettemps()
            for temp in temps:  # temps is a dict: {'28-031655df8bff': 18.625, 'timestamp': datetime.datetime(2016, 11, 11, 22, 47, 35, 344949)}
                if temp == 'timestamp':
                    continue
                else:

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
    f.close()
    return lines

def read_temp():
    lines = read_temp_raw()
    while lines[0].strip()[-3:] != 'YES':
        time.sleep(0.2)
        lines = read_temp_raw()
    equals_pos = lines[1].find('t=')
    if equals_pos != -1:
        temp_string = lines[1][equals_pos+2:]
        temp_c = float(temp_string) / 1000.0
        return temp_c

controller = PID(0.1, 0.001, 1)
controller.setPoint(54)

while True:
	temperature = read_temp()

	control_signal = controller.update(temperature)

	if control_signal > 0:
		io.output(relay_pin, True)
		status = '1'
	else:
		io.output(relay_pin, False)
		status = '0'

	message = str(time.time()) + '\t' + str(status) + '\t' + str(temperature) + '\t' + str(control_signal)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
class BaseController:
    """
    coordinates motor commands with imu odometry to follow velocity commands
    """
    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 imu1Callback(self,data):
        self.newDataM1 = True
        self.currentM1Measurement = data.angular_velocity.z
        self.currentLeftV.add(data.angular_velocity.z)
        
    def imu2Callback(self,data):
        self.newDataM2 = True
        self.currentM2Measurement = data.angular_velocity.z
        self.currentRightV.add(data.angular_velocity.z)
    
    def commandCallback(self, data):
        """
        Will set the internal PID controller's setpoints according to the command
        """
        print "****NEW COMMAND RECEIVED***"
        linearV=data.linear.x
        angularV=data.angular.z
        # calculate desired velocities for wheels see Georgia Tech mobile robot 2.2 video for equation
        L=.57
        R=.9424
        self.leftPID.setPoint( -(2*linearV - angularV*L)/(2*R))
        self.rightPID.setPoint( (2*linearV + angularV*L)/(2*R))
        
    def shutdown(self):
        """
        To be called if something went wrong, stop the roboclaw and exit out
        """
        print "shutting down"
        self.myRoboclaw.writeM1M2(0,0)
        exit()
        
    def commandLoop(self):
        """
        Main loop that writes the commands to the roboclaw.  If new data is received
        at a rate lower than 4Hz, then the wheelchair will shutdown
        """
        
        # Thigns to check:
        #  1) We got new data at the correct frequency
        #  2) We are not getting repeat data from our imus (imus are still working)
        
        # run loop 20 times a second
        newLeft = 0
        newRight = 0
        r = rospy.Rate(30)
        while not rospy.is_shutdown():
            # Wait until we get new data from both motors
            if self.newDataM1 and self.newDataM2:
                self.newData = True
                
            if not self.newData:
                if time.time() - self.lastDataTime > .5:
                    print "Too long between messages!"
                    self.shutdown()
                else:
                    continue
            
            # We have data, ensure that it isnt repeated data
            else:
                self.lastDataTime = time.time()
#.........这里部分代码省略.........

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
class MoveGantryPlayback():

    def __init__(self):

        self.is_running = True
        self.step_size = 0.05
	self.gantry_cmd_vel = Twist()

	self.current_x = 0.0
	self.current_y = 0.0
	self.current_z = 0.0

        # Set PID Gains
        self.pid_x = PID (0.1, 0.0, 0.0)
        self.pid_y = PID (0.0, 0.0, 0.0)
        self.pid_z = PID (0.0, 0.0, 0.0)

        rospy.init_node('move_gantry_playback', anonymous=True)
	self.sub_pos = rospy.Subscriber("/Optitrack/RB0", Pose, self.pos_call)
	self.gantry_cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist)

    def pos_call(self,data):
	self.current_x = data.position.x
	self.current_y = data.position.y
	self.current_z = data.position.z

    def update_gantry_velocity(self):

        # Intialize desired position with first odom position
	self.x_pos_des = 147
	self.y_pos_des = 558
	self.z_pos_des = 2063

        r = rospy.Rate(10)

	while self.is_running:

            # Update PID with new setpoint
            self.pid_x.setPoint(self.x_pos_des)
            self.pid_y.setPoint(self.y_pos_des)
            self.pid_z.setPoint(self.z_pos_des)

            # Update PID with new measurement 
            self.gantry_cmd_vel.linear.x = self.pid_x.update(self.current_x)
            self.gantry_cmd_vel.linear.y = self.pid_y.update(self.current_y)
            self.gantry_cmd_vel.linear.z = self.pid_z.update(self.current_z)
          
            # Scale gantry velocities
	    if (self.gantry_cmd_vel.linear.x > 0.25):
                self.gantry_cmd_vel.linear.x = 0.25
	    elif (self.gantry_cmd_vel.linear.x < -0.25):
                self.gantry_cmd_vel.linear.x = -0.25
	    if (self.gantry_cmd_vel.linear.y > 0.25):
                self.gantry_cmd_vel.linear.y = 0.25
	    if (self.gantry_cmd_vel.linear.z > 0.25):
                self.gantry_cmd_vel.linear.z = 0.25
	    elif (self.gantry_cmd_vel.linear.z < -0.25):
                self.gantry_cmd_vel.linear.z = 0.25

            # Publish gantry velocity
            #self.gantry_cmd_vel_pub.publish(self.gantry_cmd_vel)
            r.sleep()

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]

#.........这里部分代码省略.........

        self.game.accept("shift-mouse1", self.setKey, ["attack", 1])
        self.game.accept("shift-mouse1-up", self.setKey, ["attack", 0])

        self.game.accept("x", self.setKey, ["attack", 1])
        self.game.accept("x-up", self.setKey, ["attack", 0])

        self.game.accept("c", self.setKey, ["brake", 1])
        self.game.accept("c-up", self.setKey, ["brake", 0])

        self.game.accept("arrow_up", self.setKey, ["up", 1])
        self.game.accept("arrow_up-up", self.setKey, ["up", 0])

        self.game.accept("arrow_down", self.setKey, ["down", 1])
        self.game.accept("arrow_down-up", self.setKey, ["down", 0])

        self.game.accept("arrow_left", self.setKey, ["left", 1])
        self.game.accept("arrow_left-up", self.setKey, ["left", 0])

        self.game.accept("arrow_right", self.setKey, ["right", 1])
        self.game.accept("arrow_right-up", self.setKey, ["right", 0])

    def setKey(self, key, value):
        self.keyMap[key] = value

    def draw(self):
        self.model.reparentTo(render)

        self.p = ParticleEffect()
        self.p.loadConfig(Filename('data/particles/fireish.ptf'))

        self.p.setR(180)
        self.p.setScale(0.25)
        # Sets particles to birth relative to the teapot, but to render at
        # toplevel
        self.p.start(self.model)
        self.p.setPos(0.000, 0.000, 0.000)

    def update(self, task):
        dt = globalClock.getDt()

        if self.model:

            # Movement

            if self.keyMap["brake"]:
                self.speed = self.low_speed
            else:
                self.speed = self.normal_speed

            if self.game.ship_control_type == 0:

                if self.keyMap["up"]:
                    self.model.setZ(self.model, self.speed * dt)

                elif self.keyMap["down"]:
                    self.model.setZ(self.model, -self.speed * dt)

                if self.keyMap["left"]:
                    self.model.setX(self.model, -self.speed * dt)

                elif self.keyMap["right"]:
                    self.model.setX(self.model, self.speed * dt)

            elif self.game.ship_control_type == 1:

                self.x_pos = self.shipPoint.getX()
                self.z_pos = self.shipPoint.getZ()

                self.x_pid.setPoint(self.x_pos)
                self.z_pid.setPoint(self.z_pos)

                pid_x = self.x_pid.update(self.model.getX())
                pid_z = self.z_pid.update(self.model.getZ())

                self.x_speed = pid_x
                self.z_speed = pid_z

                self.x_speed = min(self.speed, self.x_speed)
                self.x_speed = max(-self.speed, self.x_speed)

                self.z_speed = min(self.speed, self.z_speed)
                self.z_speed = max(-self.speed, self.z_speed)

                self.model.setX(self.model, self.x_speed * dt)
                self.model.setZ(self.model, self.z_speed * dt)

            self.model.setR(self.x_speed)

            # Shoot

            if self.keyMap["attack"]:
                current_shoot_time = task.time
                if current_shoot_time - self.last_shoot >= self.cool_down:

                    self.last_shoot = current_shoot_time

                    self.bullet = Bullet(self)

        return task.cont

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
class PositionController():

    def __init__(self, autopilot, state_estimation, **kwargs):
        self.sm = SensorModel()
        self.heading = None
        self.targets = {}
        self.autopilot = autopilot
        self.state_estimation = state_estimation
        self.maximum_thrust = 1850
        self.minimum_thrust = 1350
       # self.altitude_pid = kwargs.get('altitude_pid')
        p = 25
        i = 0.6
        d = 0
        max_t = 50
        min_t = -5

        self.altitude_pid = PID(
            P=p,
            I=i,
            D=d,
            Derivator=0,
            Integrator=0,
            Integrator_max=25,
            Integrator_min=-25,
            maximum_thrust=max_t,
            minimum_thrust=min_t,
        )
        self.heading_pid = PID(
            P=1,
            I=0,
            D=0,
            Derivator=0,
            Integrator=0,
            Integrator_max=25,
            Integrator_min=-25,
            maximum_thrust=25,
            minimum_thrust=-25,
        )

    def headingHold(self):
        if not self.targets.get('heading'):
            #self.targets['heading'] = self.state_estimation.getHeading()
            self.targets['heading'] = self.autopilot.heading
            self.heading_pid.setPoint(self.targets.get('heading'))
        thrust_correction = self.altitude_pid.update(self.autopilot.heading)
       # thrust_correction = self.althold_pid.constraint(thrust_correction)
        self.autopilot.yaw = self.autopilot.yaw + thrust_correction

    def holdAltitude(self):
        if not self.targets.get('altitude'):
            self.targets['altitude'] = self.state_estimation.getAltitude()
            self.altitude_pid.setPoint(self.targets.get('altitude'))
            self.autopilot.meta_pid = MetaPid(
                P=self.altitude_pid.Kp,
                I=self.altitude_pid.Ki,
                D=self.altitude_pid.Kd,
                maximum_thrust=self.altitude_pid.maximum_thrust,
                minimum_thrust=self.altitude_pid.minimum_thrust,
                )
        altitude = self.state_estimation.getAltitude()
        thrust_correction = self.altitude_pid.update(altitude)
        thrust_correction = self.altitude_pid.constraint(thrust_correction)
        thrust = self.autopilot.throttle + thrust_correction
        print 'target: %f altitude: %f  corretion: %d current: %d new thrust: %d ' % (self.altitude_pid.set_point, self.state_estimation.getAltitude(), thrust_correction, thrust, self.autopilot.throttle)
        self.autopilot.throttle = self.constraint(thrust)
        self.autopilot.pid_log(
            PIDlog(
                corretion=thrust_correction,
                altitude=altitude,
                #  altitude_raw=self.autopilot.altitude_barometer,
                altitude_raw=self.autopilot.altitude_barometer,
                target=self.altitude_pid.set_point,
                thrust=self.autopilot.throttle,
                error=self.altitude_pid.error,
            )
        )
       # print 'target: %f altitude: %f' % (self.altitude_pid.set_point,
       # self.state_estimation.getAltitude())

    def reset_targets(self):
        self.targets.clear()

    def set_target_altitude(self, altitude):
        self.targets['altitude'] = altitude

    def constraint(self, value):
        if value > self.maximum_thrust:
            return self.maximum_thrust
        elif value < self.minimum_thrust:
            return self.minimum_thrust
        else:
            return int(round(value))

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
from adxl345 import ADXL345
from mpu6050 import MPU6050
from dcmotor2 import dcmotor
from pid import PID
import math
import time

adxl345 = ADXL345()
mpu6050 = MPU6050()
lmotor = dcmotor(14, 15, 18)
rmotor = dcmotor(23, 24, 12)
pid = PID(2.0, 0, 0, 0, 0, 1024, -1024)
pid.setPoint(0.0)
xa = 0
za = 0
while True:
	for i in range(5):
		axesa = adxl345.getAxes(True)
		axesm = mpu6050.getAxes()
		xa = xa + axesa['x'] + axesm['x']
		za = za + axesa['z'] + axesm['z']
	x = xa/10
	z = za/10
	ang = math.atan2(z, math.fabs(x))
	print ang
	sped = pid.update(ang)
	print sped
	lmotor.rotate(sped)
	rmotor.rotate(sped)
	time.sleep(1)

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
class Flight(object):

    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 update_forces(self, forces):
        # Update forces in min and max values
        for i, force in enumerate(forces):
            force = max(self.min_force, force)
            force = min(self.max_force, force)
            forces[i] = force
        # set values to quad
        self.quad.motors_force = forces

    def stabilize(self):
        forces = [0, 0, 0, 0]
        # acceleration
        aforce = self.acceleration.update(self.quad.vel[1])
        forces = [aforce, aforce, aforce, aforce]
        # pitch
        pforce = self.pitch.update(self.quad.pitch)
        forces[0] += pforce
        forces[1] -= pforce
        # roll
        rforce = self.roll.update(self.quad.roll)
        forces[3] += rforce
        forces[2] -= rforce
        # yaw
        if self.lock_yaw:
            yforce = self.yaw.update(self.quad.yaw)
            forces[0] += yforce
            forces[1] += yforce
            forces[2] -= yforce
            forces[3] -= yforce

        self.update_forces(forces)

    def apply_commands(self):
        if self.lock_yaw and self.command_yaw:
            self.lock_yaw = False
        if not self.command_yaw and not self.lock_yaw:
            self.set_point_yaw = self.quad.yaw
            self.yaw.setPoint(self.set_point_yaw)
            self.lock_yaw = True


        if self.set_point_acceleration != self.command_acceleration:
            self.set_point_acceleration = -self.command_acceleration
            self.acceleration.setPoint(self.set_point_acceleration)

        forces = [
            self.quad.motors_force[0] + self.command_pitch + self.command_yaw,
            self.quad.motors_force[1] + -self.command_pitch + self.command_yaw,
            self.quad.motors_force[2] + self.command_roll - self.command_yaw,
            self.quad.motors_force[3] + -self.command_roll - self.command_yaw,
        ]
        self.update_forces(forces)

    def step(self):
        self.stabilize()
        self.apply_commands()
        self.quad.step()

# 需要导入模块: from pid import PID [as 别名]
# 或者: from pid.PID import setPoint [as 别名]
class WaterTank(object):
    liters = 100
    desired_temp = 27
    input_wattage = 500 
    outside_temp = 20

    def __init__(self, desired_temp, outside_temp, input_wattage, r, h, thickness):
        self.r = r
        self.h = h
        self.d = 2*r
        self.thickness = thickness
        self.volume = volume = (PI*h*math.pow(r, 2))/1000#liters
        self.current_temp = desired_temp

        sides = (2*PI)*r*h
        base = PI*math.pow(r, 2)
        self.surface_area = (sides+base)/10000
        print "Surface Area: %sm" % self.surface_area
        print "Liters: %s" % self.volume

        self.weight = volume#kg
        self.total_joules = self.weight*JOULES_CALORIE

        print "Total Joules: %s" % self.total_joules

        self.desired_temp = desired_temp
        self.outside_temp = outside_temp
        self.input_wattage = input_wattage

        self.pid = PID(3.0,0.4,1.2)
        self.pid.setPoint(self.desired_temp)

        self.run()
        

    def heat(self, time):
        global JOULES_CELCIUS
        print "Heating!"
        total_watts = self.input_wattage*time
        self.current_temp+= (total_watts*JOULES_CELCIUS)/(self.volume*1000)
        print self.current_temp
        gevent.sleep(time)
        return

    def run(self):
        global POLYETHALENE_K, JOULES_CELCIUS
        counter = 0
        time_check = 60*5#1 minutes
        while True:
            print "Counter: %s" % counter
            pid = self.pid.update(self.current_temp)
            print "PID: %s" % pid
            if counter == time_check:
                self.heat(pid)
                counter = 0
                continue

            counter+=1
            heat_loss = (POLYETHALENE_K*self.surface_area*(self.desired_temp-self.outside_temp)/self.thickness)/3600
            celc_change = (heat_loss*JOULES_CELCIUS)
            print "Heat Loss: %s" % celc_change
            self.current_temp-= celc_change
            print "Temp: %s" % self.current_temp
            gevent.sleep(1)