X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fchristmas.py;h=9f493320037980db2d2f1df6a19c7045c9c32338;hp=6c53dce82ef11482fada2c3f5ab618485b5f392c;hb=2f194b3fe82009a77b79021f0c57e6ca55c1706f;hpb=54f503a6e7a839b1359a2cdc0241a1a77621bfab diff --git a/scripts/christmas.py b/scripts/christmas.py index 6c53dce..9f49332 100755 --- a/scripts/christmas.py +++ b/scripts/christmas.py @@ -3,22 +3,36 @@ import rospy from random import * -from wild_thumper.msg import LedStripe, Led -from time import sleep +from datetime import datetime +from wild_thumper.msg import * max_val = 10 +light = 0 +ldr_thres = 8 + +def sensorReceived(msg): + global light + light = msg.light if __name__ == "__main__": rospy.init_node('christmas') pub = rospy.Publisher('led_stripe', LedStripe, queue_size=10) + rospy.Subscriber("/sensors", Sensor, sensorReceived) rate = rospy.Rate(2) + val = max_val while not rospy.is_shutdown(): - msg = LedStripe() - msg.leds = [ - Led(4, randint(0, max_val), randint(0, max_val), randint(0, max_val)), - Led(5, randint(0, max_val), randint(0, max_val), randint(0, max_val)), - Led(6, randint(0, max_val), randint(0, max_val), randint(0, max_val)), - Led(7, randint(0, max_val), randint(0, max_val), randint(0, max_val)) - ] - pub.publish(msg) + if light <= ldr_thres or val != 0: + now = datetime.now() + if light <= ldr_thres and now.hour >= 18 and now.hour < 22: + val = max_val + else: + val = 0 + msg = LedStripe() + msg.leds = [ + Led(4, randint(0, val), randint(0, val), randint(0, val)), + Led(5, randint(0, val), randint(0, val), randint(0, val)), + Led(6, randint(0, val), randint(0, val), randint(0, val)), + Led(7, randint(0, val), randint(0, val), randint(0, val)) + ] + pub.publish(msg) rate.sleep()