D10d3

Robot Control

Nov 13th, 2016
539
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import RPi.GPIO as gpio
  2. import time
  3. from time import sleep
  4. import os
  5. from sense_hat import SenseHat
  6. sense = SenseHat()
  7.  
  8.  
  9.  
  10. def init():
  11.     gpio.setmode(gpio.BOARD)
  12.     gpio.setup(33,gpio.OUT)
  13.     gpio.setup(35,gpio.OUT)
  14.     gpio.setup(37,gpio.OUT)
  15.     gpio.setup(38,gpio.OUT)
  16.  
  17. def forward(tf):
  18.     init()
  19.     gpio.output(33,False)
  20.     gpio.output(35,True)
  21.     gpio.output(37,False)
  22.     gpio.output(38,True)
  23.     time.sleep(tf)
  24.     gpio.cleanup()
  25.  
  26. def back(tf):
  27.     init()
  28.     gpio.output(33,True)
  29.     gpio.output(35,False)
  30.     gpio.output(37,True)
  31.     gpio.output(38,False)
  32.     time.sleep(tf)
  33.     gpio.cleanup()
  34.  
  35. def right(tf):
  36.     init()
  37.     gpio.output(33,False)
  38.     gpio.output(35,True)
  39.     gpio.output(37,True)
  40.     gpio.output(38,False)
  41.     time.sleep(tf)
  42.     gpio.cleanup()
  43.  
  44. def left(tf):
  45.     init()
  46.     gpio.output(33,True)
  47.     gpio.output(35,False)
  48.     gpio.output(37,False)
  49.     gpio.output(38,True)
  50.     time.sleep(tf)
  51.     gpio.cleanup()
  52.  
  53. def sensors():
  54.     t = sense.get_temperature()
  55.     p = sense.get_pressure()
  56.     h = sense.get_humidity()
  57.     f = ((t * 9)/5)+32
  58.  
  59.     t = round(t, 1)
  60.     p = round(p, 1)
  61.     h = round(h, 1)
  62.     f= round(f, 1)
  63.  
  64.  
  65.     print "Temperature Celsius = %r" % (t)
  66.     print "Temperature Fahrenheit = %r" % (f)
  67.     print "Pressure = %r Millibars" % (p)
  68.     print "Humidity = %r Percent" % (h)
  69.  
  70.     orientation = sense.get_orientation_degrees()
  71.     print("p: {pitch}, r: {roll}, y: {yaw}".format(**orientation))
  72.  
  73. def control():
  74.     valid = False
  75.     light = 0
  76.     while not valid:
  77.         os.system('clear')
  78.         print "Robot Control"
  79.         print ""
  80.         print "-=SENSORS=-"
  81.         sensors()
  82.         print ""
  83.         print "-=MOTOR CONTROLS=-"
  84.         print "Drive Forward Left: Q"
  85.         print "Drive Forward Right: E"
  86.         print "Drive Forward: W"
  87.         print "Drive Back: S"
  88.         print "Turn Left: A"
  89.         print "Turn Right: D"
  90.         print ""
  91.         print"Toggle Light: L"
  92.         print "Exit: Z"
  93.         input = raw_input("Command>")
  94.         if input == "w":
  95.             s = float(raw_input("Seconds?>"))
  96.             forward(s)
  97.         elif input =="s":
  98.             s = float(raw_input("Seconds?>"))
  99.             back(s)
  100.         elif input =="a":
  101.             s = float(raw_input("Seconds?>"))
  102.             left(s)
  103.         elif input =="d":
  104.             s = float(raw_input("Seconds?>"))
  105.             right(s)
  106.         elif input == "q":
  107.             left(0.5)
  108.             forward(0.5)
  109.             left(0.5)
  110.             forward(0.5)
  111.         elif input == "e":
  112.             right(0.5)
  113.             forward(0.5)
  114.             right(0.5)
  115.             forward(0.5)
  116.         elif input == "z":
  117.             quit()
  118.         elif input =="l":
  119.             if light == 0:
  120.                 light = 1
  121.                 O = [255, 255, 255]
  122.                 grid = [
  123.                 O, O, O, O, O, O, O, O,
  124.                 O, O, O, O, O, O, O, O,
  125.                 O, O, O, O, O, O, O, O,
  126.                 O, O, O, O, O, O, O, O,
  127.                 O, O, O, O, O, O, O, O,
  128.                 O, O, O, O, O, O, O, O,
  129.                 O, O, O, O, O, O, O, O,
  130.                 O, O, O, O, O, O, O, O
  131.                 ]
  132.                 sense.set_pixels(grid)
  133.             else:
  134.                 light = 0
  135.                 sense.clear()
  136.  
  137. control()
RAW Paste Data