Advertisement
AverageMan

AverageBot Final Code

Dec 9th, 2015
352
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #! /usr/bin/python
  2. # Code explanation & breakdown: http://www.averagemanvsraspberrypi.com/2015/12/averagebot-devblog-10.html
  3. # PiWars 2015 python code for 'AverageBot'
  4. # Written by @AverageManVsPi - Lots of code borrowed from the 4Tronix PiRoCon libraries
  5. # This robot used a Model A Raspberry Pi and 4Tronix PiRoCon motor controller (http://4tronix.co.uk/store/index.php?rt=product/product&product_id=182)
  6. # See the full development blog series at http://www.AverageManVsRaspberryPi.com/tag/pi-wars
  7.  
  8. #======================================================================
  9. # 8x PIROCON GPIO BLOCK AVAILABILITY (RPI-GPIO NOT BCM!)
  10.  
  11.     # 7  *** FREE ***
  12.     # 11 [USED BY RIGHT LINE SENSOR]
  13.     # 12 [USED BY MIDDLE LINE SENSOR]
  14.     # 13 [USED BY LEFT LINE SENSOR]
  15.     # 15 [USED BY SERVO 1]
  16.     # 16 [USED BY SERVO 2]
  17.     # 18 [USED BY SWITCH 2]
  18.     # 22 [USED BY SWITCH 1]
  19.  
  20. print "- - - - - SCRIPT STARTED"
  21. print "- - - - - - - - - - - - "
  22.  
  23. print ""
  24.  
  25.  
  26. #======================================================================
  27. # IMPORTS
  28.  
  29. print "- - - - - START IMPORTS"
  30.  
  31. # IMPORT OS FOR PYGAME SCREEN FRIG
  32. import os
  33. print "imported os"
  34.  
  35. # IMPORT SBMUS FOR 4TRONIX DISPLAY BOARD
  36. import smbus
  37. print "imported smbus"
  38.  
  39. # IMPORT GPIO FOR MOTOR CONTROLLER
  40. import RPi.GPIO as GPIO
  41. print "imported GPIO"
  42.  
  43. # IMPORT SUBPROCESS FOR OS. COMMANDS
  44. import subprocess
  45. print "imported subprocess"
  46.  
  47. # IMPORT PYGAME FOR 2.4GHZ KEYBOARD CONTROL
  48. import pygame
  49. print "imported pygame"
  50.  
  51. # IMPORT TIME FOR THE PAUSES IN OUR CODE
  52. import time
  53. print "imported time"
  54.  
  55. # IMPORT SYS FOR MOTOR CONTROLLER CODE
  56. import sys
  57. print "imported sys"
  58.  
  59. # IMPORT THREADING FOR MOTOR CONTROLLER CODE
  60. import threading
  61. print "imported threading"
  62.  
  63. print ""
  64.  
  65.  
  66. #======================================================================
  67. # SET GPIO MODE
  68.  
  69. print "- - - - - START GPIO SETMODE"
  70. time.sleep(0.5)
  71.  
  72. GPIO.setmode(GPIO.BOARD) # SET GPIO MODE TO BOARD
  73. print "GPIO setmode complete"
  74.  
  75. print ""
  76.  
  77.  
  78. #======================================================================
  79. # PYGAME SETUP
  80.  
  81. print "- - - - - START PYGAME SETUP"
  82. time.sleep(0.5)
  83.  
  84. os.environ["SDL_VIDEODRIVER"] = "dummy" # SET UP FAKE VIDEO DRIVER FOR PYGAME
  85. print "PyGame - fake video driver set up"
  86.  
  87. pygame.init() # INITIALISE PYGAME
  88. print "PyGame - initialised"
  89.  
  90. # SET UP FAKE SCREEN SETTINGS FOR PYGAME
  91. width = 320
  92. height = 240
  93. display = pygame.display.set_mode((width,height))
  94. print "PyGame - fake screen settings complete"
  95.  
  96. print ""
  97.  
  98.  
  99. #======================================================================
  100. # SET UP SERVOS
  101.  
  102. print "- - - - - START SERVO SETUP"
  103. time.sleep(0.5)
  104.  
  105. # SET UP GPIO NUMBERS
  106. servo1 = 15
  107. servo2 = 16
  108.  
  109. # SET GPIO DIRECTIONS FOR SERVOS (OUT)
  110. GPIO.setup(servo1, GPIO.OUT) # SET SERVO1 AS A GPIO OUTPUT
  111. GPIO.setup(servo2, GPIO.OUT) # SET SERVO2 AS A GPIO OUTPUT
  112. print "GPIO set up"
  113.  
  114. servo1 = GPIO.PWM(servo1,50) # SERVO1 PWM FREQUENCY SET TO 5OHZ
  115. servo2 = GPIO.PWM(servo2,50) # SERVO2 PWM FREQUENCY SET TO 5OHZ
  116. time.sleep(0.5) # BREAK TO ALLOW SERVOS TO SET
  117. print "Servo frequency set"
  118.  
  119. # SET INITIAL SERVO POSITION
  120. servo1.start(4) # SET SERVO 1 INTO INITIAL POSITION
  121. servo2.start(11) # SET SERVO 2 INTO INITIAL POSITION
  122. print "Servos set up"
  123.  
  124. print ""
  125.  
  126.  
  127. #======================================================================
  128. # SET UP SWITCHES
  129.  
  130. print "- - - - - START SWITCH SETUP"
  131. time.sleep(0.5)
  132.  
  133. # SET UP GPIO NUMBERS
  134. switch1 = 22
  135. switch2 = 18
  136. print "Switch GPIO numbers set"
  137.  
  138. # SET GPIO DIRECTIONS FOR SWITCHES (IN)
  139. GPIO.setup(switch1,GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
  140. GPIO.setup(switch2,GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
  141. print "Swich directions set to inputs"
  142.  
  143. print ""
  144.  
  145.  
  146. #======================================================================
  147. # SET UP SMBUS FOR 4TRONIX DISPLAY BOARD
  148.  
  149. print "- - - - - START SMBUS SETUP"
  150. time.sleep(0.5)
  151.  
  152. # SET SMBUS NUMBER
  153. bus = smbus.SMBus(1) # 0 for Revision 1 Pi
  154. print "smbus ready"
  155.    
  156. print ""
  157.  
  158.  
  159. #======================================================================
  160. # SET UP 4TRONIX DISPLAY BOARD
  161.  
  162. print "- - - - - START DISPLAY BOARD SETUP"
  163. time.sleep(0.5)
  164.  
  165. # SET UP CHARACTERS FOR DISPLAY BOARD (HEX CODES)
  166. N0 = 0x3F
  167. N1 = 0x06
  168. N2 = 0x5B
  169. N3 = 0x4F
  170. N4 = 0x66
  171. N5 = 0x6D
  172. N6 = 0x7D
  173. N7 = 0x07
  174. N8 = 0x7F
  175. N9 = 0x6F
  176. dash = 0x40
  177. underscore = 0x08
  178. print "characters ready"
  179.  
  180. # SET UP MCP23017 FOR DISPLAY BOARD
  181. addr = 0x20 # I2C address of MCP23017
  182. bus.write_byte_data(addr, 0x00, 0x00) # SET ALL OF BANK 0 TO OUTPUTS
  183. bus.write_byte_data(addr, 0x01, 0x00) # SET ALL OF BANK 1 TO OUTPUTS
  184. print "MCP23017 ready"
  185.  
  186. # CLEAR DISPLAY
  187. bus.write_byte_data(addr, 0x13, 0xff) # SET ALL OF BANK 1 TO HIGH (OFF)
  188. print "display cleared"
  189.  
  190. print ""
  191.  
  192.  
  193. #======================================================================
  194. # SET UP SR-04 GPIO PINS
  195.  
  196. print "- - - - - START SR-04 SETUP"
  197. time.sleep(0.5)
  198.  
  199. # SET SR-04 GPIO PIN
  200. sonar = 8
  201. print "SR-04 GPIO pin set"
  202.  
  203. print ""
  204.  
  205.  
  206. #======================================================================
  207. # SET UP MOTORS
  208.  
  209. print "- - - - - START MOTOR GPIO SETUP"
  210. time.sleep(0.5)
  211.  
  212. # INITIALISE THE PWM DEVICE USING DEFAULT ADDRESS
  213. global p, q, a, b
  214. print "PWM address set"
  215.  
  216. # MOTOR GPIO PINS
  217. R1 = 24
  218. R2 = 26
  219. L1 = 19
  220. L2 = 21
  221. print "Motor GPIO pins set"
  222.  
  223. # USE PWM TO CONTROL MOTOR SPEED
  224. GPIO.setup(L1, GPIO.OUT) # SET LEFT MOTOR GPIO 1 AS OUTPUT
  225. p = GPIO.PWM(L1, 20) # SET LEFT MOTOR GPIO 1 AS PWM
  226. p.start(0) # START LEFT MOTOR 1 PWM AT ZERO
  227.  
  228. GPIO.setup(L2, GPIO.OUT) # SET LEFT MOTOR GPIO 2 AS OUTPUT
  229. q = GPIO.PWM(L2, 20) # SET LEFT MOTOR GPIO 2 AS PWM
  230. q.start(0) # START LEFT MOTOR 2 PWM AT ZERO
  231.  
  232. GPIO.setup(R1, GPIO.OUT) # SET RIGHT MOTOR GPIO 1 AS OUTPUT
  233. a = GPIO.PWM(R1, 20) # SET RIGHT MOTOR GPIO 1 AS PWM
  234. a.start(0) # START RIGHT MOTOR 1 PWM AT ZERO
  235.  
  236. GPIO.setup(R2, GPIO.OUT) # SET RIGHT MOTOR GPIO 2 AS OUTPUT
  237. b = GPIO.PWM(R2, 20) # SET RIGHT MOTOR GPIO 2 AS PWM
  238. b.start(0) # START RIGHT MOTOR 2 PWM AT ZERO
  239. print "Motor GPIO pins set"
  240.  
  241. print ""
  242.  
  243.  
  244. #======================================================================
  245. # SET UP SPEED LEVELS
  246. #    - This section sets a number of speed levels
  247. #    - Allows us to change the PWM tuning for the entire program
  248. #    - First number is the left motor, second is the right motor
  249. #    - Motors have different values to compensate for inaccuracies
  250.  
  251. print "- - - - - START SPEED LEVEL SETUP"
  252. time.sleep(0.5)
  253.  
  254. # TURN SPEED SETTINGS (PWM)
  255. SpeedLow = 60
  256. SpeedMedium = 80
  257. SpeedHigh = 100
  258. print "Turn speeds set"
  259.  
  260. # FORWARD SPEED SETTINGS (PWM)
  261. ForwardLowL = 18
  262. ForwardLowR = 20
  263. ForwardMediumL = 57
  264. ForwardMediumR = 60
  265. ForwardHighL = 98
  266. ForwardHighR = 99
  267. print "Forward speeds set"
  268.  
  269. # REVERSE SPEED SETTINGS (PWM)
  270. ReverseLowL = 26
  271. ReverseLowR = 20
  272. ReverseMediumL = 60
  273. ReverseMediumR = 56
  274. ReverseHighL = 98
  275. ReverseHighR = 94
  276. print "Reverse speeds set"
  277.  
  278. # SET INITIAL MOTOR SPEEDS
  279.  
  280. TurnSpeed = SpeedHigh         # Start @ SpeedHigh
  281. ForwardSpeedL = ForwardHighL  # Start @ ForwardHighL
  282. ForwardSpeedR = ForwardHighR  # Start @ ForwardHighR
  283. ReverseSpeedL = ReverseHighL  # Start @ ReverseHighL
  284. ReverseSpeedR = ReverseHighR  # Start @ ReverseHighR
  285. print "Initial speeds set"
  286.  
  287. print ""
  288.  
  289.  
  290. #======================================================================
  291. # SET UP LINE SENSORS
  292.  
  293. print "- - - - - START LINE SENSOR SETUP"
  294. time.sleep(0.5)
  295.  
  296. # SET GPIO FOR LINE FOLLOWING SENSORS
  297. lineRight = 11  #(BROWN)
  298. lineMiddle = 12 # (WHITE)
  299. lineLeft = 13  #(BLUE)
  300. print "Line sensor GPIO numbers set"
  301.  
  302. # SET UP DIGITAL LINE DETECTORS AS INPUTS
  303. GPIO.setup(lineRight,GPIO.IN)
  304. GPIO.setup(lineMiddle,GPIO.IN)
  305. GPIO.setup(lineLeft,GPIO.IN)
  306. print "Line sensor GPIO direction set (IN)"
  307.  
  308. print ""
  309.  
  310.  
  311. #======================================================================
  312. # SET COUNT AND INITIAL DISPLAY CHARACTER
  313.  
  314. print "- - - - - START INITIAL COUNT & DISPLAY CHARACTER SETUP"
  315. time.sleep(0.5)
  316.  
  317. # SET INITIAL COUNT FOR DISPLAY BOARD MENU CODE
  318. count = 0
  319. print "Count set to zero"
  320.  
  321. # SET AN UNDERSCORE IN THE MENU TO START WITH (MCP23017 COMMAND)
  322. bus.write_byte_data(addr, 0x13, 7)
  323. bus.write_byte_data(addr, 0x12, underscore)
  324. print "Initial display character set"
  325.  
  326. print ""
  327.  
  328.  
  329. #======================================================================
  330. # PRINT TO CONFIRM START UP PROCESSES COMPLETE
  331.  
  332. print "- - - - - ALL START UP PROCESSES COMPLETE"
  333. print "- - - - - - - - - - - - - - - - - - - - -"
  334. print ""
  335. time.sleep(0.5)
  336.  
  337.  
  338. #======================================================================
  339. #======================================================================
  340. #======================================================================
  341. # START MAIN PROGRAM
  342. #======================================================================
  343. #======================================================================
  344. #======================================================================
  345.  
  346.  
  347. print "- - - - - STARTING MAIN PROGRAM"
  348. print "- - - - - - - - - - - - - - - -"
  349. print ""
  350. time.sleep(0.5)
  351.  
  352.  
  353. #======================================================================
  354. # TRY BLOCK
  355. # This is the main program
  356. # When we exit this block, the finally/except block will run
  357.  
  358. try:
  359.     def menu(): # MENU CODE FOR 4TRONIX DISPLAY BOARD AND SWITCH
  360.    
  361.         global count # IMPORTS 'COUNT' BUT LETS US EDIT IT IN A MODULE
  362.         print "Menu start"
  363.        
  364.         while 1: # RUN FOREVER UNTIL WE MANUALLY EXIT THIS MODULE
  365.        
  366.             time.sleep(0.01)
  367.            
  368.             if GPIO.input(switch1): # IF SWITCH 1 IS CLICKED
  369.                 time.sleep(0.8)
  370.                 count = count +1 # ADD 1 TO COUNT
  371.                
  372.                 if count == 1: # IF 'COUNT' IS 1
  373.                     print "count is 1"
  374.                     bus.write_byte_data(addr, 0x13, 7)
  375.                     bus.write_byte_data(addr, 0x12, N1) # DISPLAY 1
  376.                    
  377.                 if count == 2: # IF 'COUNT' IS 2
  378.                     print "count is 2"
  379.                     bus.write_byte_data(addr, 0x13, 7)
  380.                     bus.write_byte_data(addr, 0x12, N2) # DISPLAY 2
  381.                    
  382.                 if count == 3: # IF 'COUNT' IS 3
  383.                     print "count is 3"
  384.                     bus.write_byte_data(addr, 0x13, 7)
  385.                     bus.write_byte_data(addr, 0x12, N3) # DISPLAY 3
  386.                    
  387.                 if count == 4: # IF 'COUNT' IS 4
  388.                     print "count is 4"
  389.                     bus.write_byte_data(addr, 0x13, 7)
  390.                     bus.write_byte_data(addr, 0x12, N4) # DISPLAY 4
  391.                    
  392.                 if count == 5: # IF 'COUNT' IS 5
  393.                     print "count is 5"
  394.                     bus.write_byte_data(addr, 0x13, 7)
  395.                     bus.write_byte_data(addr, 0x12, N5) # DISPLAY 5
  396.                    
  397.                 if count == 6: # IF 'COUNT' IS 6
  398.                     print "count is 6"
  399.                     bus.write_byte_data(addr, 0x13, 7)
  400.                     bus.write_byte_data(addr, 0x12, N6) # DISPLAY 6
  401.                    
  402.                 if count == 7: # IF 'COUNT' IS 7
  403.                     print "count is 7"
  404.                     bus.write_byte_data(addr, 0x13, 7)
  405.                     bus.write_byte_data(addr, 0x12, N7) # DISPLAY 7
  406.                    
  407.                 if count == 8: # IF 'COUNT' IS 8
  408.                     count = 1 # RETURN COUNT TO 1 TO RESET MENU
  409.                     print "...returning count to 1"
  410.                     bus.write_byte_data(addr, 0x13, 7)
  411.                     bus.write_byte_data(addr, 0x12, N1) # DISPLAY 1
  412.  
  413.                
  414.             if GPIO.input(switch2): # IF SWITCH 2 IS CLICKED
  415.                 time.sleep(0.8)
  416.                
  417.                 if count == 1: # IF 'COUNT' IS 1
  418.                     bus.write_byte_data(addr, 0x13, 0xff)
  419.                     time.sleep(3)
  420.                     count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
  421.                     generalcontrol() # GO TO THE GENERALCONTROL MODULE
  422.  
  423.                 if count == 2: # IF 'COUNT' IS 2
  424.                     bus.write_byte_data(addr, 0x13, 0xff)
  425.                     time.sleep(3)
  426.                     count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
  427.                     proximity() # GO TO THE PROXIMITY MODULE
  428.  
  429.                 if count == 3: # IF 'COUNT' IS 3
  430.                     bus.write_byte_data(addr, 0x13, 0xff)
  431.                     time.sleep(3)
  432.                     count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
  433.                     threepoint() # GO TO THE THREEPOINT MODULE
  434.  
  435.                 if count == 4: # IF 'COUNT' IS 4
  436.                     bus.write_byte_data(addr, 0x13, 0xff)
  437.                     time.sleep(3)
  438.                     count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
  439.                     linefollow() # GO TO THE LINEFOLLOW MODULE
  440.  
  441.                 if count == 5: # IF 'COUNT' IS 5
  442.                     bus.write_byte_data(addr, 0x13, 0xff)
  443.                     time.sleep(3)
  444.                     count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
  445.                     skittles()  # GO TO THE SKITTLES MODULE
  446.  
  447.                 if count == 6: # IF 'COUNT' IS 6
  448.                     bus.write_byte_data(addr, 0x13, 0xff)
  449.                     time.sleep(3)
  450.                     count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
  451.                     endscript() # GO TO THE ENDSCRIPT MODULE
  452.  
  453.                 if count == 7: # IF 'COUNT' IS 7
  454.                     bus.write_byte_data(addr, 0x13, 0xff)
  455.                     time.sleep(3)
  456.                     count = 0 # RESET COUNT SO MENU RETURNS TO 1 NEXT TIME
  457.                     haltcommand() # GO TO THE HALTCOMMAND MODULE
  458.  
  459.                    
  460.     #======================================================================
  461.     # DEFINE GENERAL CONTROL MODULE
  462.     # We have separate speed sets for forward and back due to inaccuracies
  463.    
  464.     def generalcontrol(): ### PROGRAM 1 ###
  465.        
  466.         # IMPORT GLOBAL VARIABLES SO WE CAN CHANGE THEM IN A MODULE
  467.         global TurnSpeed
  468.         global ForwardSpeedL
  469.         global ForwardSpeedR
  470.         global ReverseSpeedL
  471.         global ReverseSpeedR
  472.        
  473.         while 1: # RUN FOREVER UNTIL WE MANUALLY EXIT THIS MODULE
  474.        
  475.             for event in pygame.event.get(): # LOOK FOR A PYGAME EVENT
  476.                 print 'General Control PyGame Event'
  477.                
  478.                 #########################################
  479.                 # IF A KEY IS BEING PRESSED...          #
  480.                 #########################################
  481.                
  482.                 if event.type == pygame.KEYDOWN: # IF A KEY IS PRESSED DOWN
  483.  
  484.                
  485.                     #======================================================
  486.                     # KEY DEBUG - PRINTS THE KEY DETECTED IN THE TERMINAL (Optional)
  487.                
  488.                     #newkey = ""
  489.                     #newkey = pygame.key.name(event.key)
  490.                     #print newkey
  491.                    
  492.                    
  493.                     #======================================================
  494.                     # BASIC MOVEMENT (FWD, REV, RIGHT, LEFT)
  495.                
  496.                     # FORWARD (UP KEY)
  497.                     if event.key == pygame.K_UP: # UP KEY PRESSED
  498.                         Forward(ForwardSpeedL, ForwardSpeedR) # FORWARD AT SET SPEED
  499.                         print 'Forward speed', ForwardSpeedL, ForwardSpeedR # PRINT SPEED
  500.                        
  501.                     # REVERSE (DOWN KEY)
  502.                     if event.key == pygame.K_DOWN: # DOWN KEY PRESSED
  503.                         Reverse(ReverseSpeedL, ReverseSpeedR) # REVERSE AT SET SPEED
  504.                         print 'Reverse speed:', ReverseSpeedL, ReverseSpeedR # PRINT SPEED
  505.        
  506.                     # RIGHT (RIGHT KEY)
  507.                     if event.key == pygame.K_RIGHT: # RIGHT KEY PRESSED
  508.                         Right(TurnSpeed) # SPIN RIGHT AT SET SPEED
  509.                         print 'Spin right speed:', TurnSpeed # PRINT SPEED
  510.        
  511.                     # LEFT (LEFT KEY)
  512.                     if event.key == pygame.K_LEFT: # LEFT KEY PRESSED
  513.                         Left(TurnSpeed) # SPIN LEFT AT SET SPEED
  514.                         print 'Spin left speed', TurnSpeed # PRINT SPEED
  515.                            
  516.                            
  517.                     #======================================================
  518.                     # TURN THE FORWARD/REVERSE SPEED UP
  519.                     # Looks at left AND right speed and changes both at the same time
  520.                     # This maintains PWM left/right balancing to handle inaccuracies
  521.  
  522.                     if event.key == pygame.K_1: # 1 KEY PRESSED
  523.                    
  524.                         # IF SPEED IS LOW - SET TO MEDIUM
  525.                         if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
  526.                             print 'Increasing to medium speed'
  527.                             print 'Forward:', ForwardMediumL, ForwardMediumR
  528.                             print 'Reverse:', ReverseMediumL, ReverseMediumR
  529.                             ForwardSpeedL = ForwardMediumL
  530.                             ForwardSpeedR = ForwardMediumR                          
  531.                             ReverseSpeedL = ReverseMediumL
  532.                             ReverseSpeedR = ReverseMediumR
  533.                            
  534.  
  535.                         # IF SPEED IS MEDIUM - SET TO HIGH
  536.                         elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
  537.                             print 'Increasing to high speed'
  538.                             print 'Forward:', ForwardHighL, ForwardHighR
  539.                             print 'Reverse:', ReverseHighL, ReverseHighR
  540.                             ForwardSpeedL = ForwardHighL
  541.                             ForwardSpeedR = ForwardHighR
  542.                             ReverseSpeedL = ReverseHighL
  543.                             ReverseSpeedR = ReverseHighR
  544.                
  545.                         # IF SPEED ALREADY HIGHEST - DO NOTHING
  546.                         elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
  547.                             print 'Highest speed reached!'
  548.                            
  549.                            
  550.                     #======================================================
  551.                     # TURN FORWARD/REVERSE SPEED DOWN
  552.                     # Looks at left AND right speed and changes both at the same time
  553.                     # This maintains PWM left/right balancing to handle inaccuracies
  554.                            
  555.                     if event.key == pygame.K_2: # 2 KEY PRESSED
  556.                            
  557.                         # IF SPEED ALREADY LOWEST - DO NOTHING
  558.                         if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
  559.                             print 'Lowest speed reached'
  560.  
  561.                         # IF SPEED IS MEDIUM - SET TO LOW
  562.                         elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
  563.                             print 'Dropping to low speed'
  564.                             print 'Forward:', ForwardLowL, ForwardLowR
  565.                             print 'Reverse:', ReverseLowL, ReverseLowR
  566.                             ForwardSpeedL = ForwardLowL
  567.                             ForwardSpeedR = ForwardLowR
  568.                             ReverseSpeedL = ReverseLowL
  569.                             ReverseSpeedR = ReverseLowR
  570.                
  571.                         # IF SPEED IS HIGH - SET TO MEDIUM
  572.                         elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
  573.                             print 'Dropping to medium speed'
  574.                             print 'Forward:', ForwardMediumL, ForwardMediumR
  575.                             print 'Reverse:', ReverseMediumL, ReverseMediumR
  576.                             ForwardSpeedL = ForwardMediumL
  577.                             ForwardSpeedR = ForwardMediumR
  578.                             ReverseSpeedL = ReverseMediumL
  579.                             ReverseSpeedR = ReverseMediumR
  580.                            
  581.                            
  582.                     #======================================================
  583.                     # TURN THE TURNING SPEED UP
  584.                        
  585.                     if event.key == pygame.K_3: # 3 KEY PRESSED
  586.        
  587.                         # IF SPEED IS LOW - SET TO MEDIUM
  588.                         if TurnSpeed == SpeedLow:
  589.                             print 'Increasing to medium turning speed. Speed:', SpeedMedium
  590.                             TurnSpeed = SpeedMedium
  591.                    
  592.                         # IF SPEED IS MEDIUM - SET TO HIGH
  593.                         elif TurnSpeed == SpeedMedium:
  594.                             print 'Increasing to high turning speed. Speed:', SpeedHigh
  595.                             TurnSpeed = SpeedHigh
  596.                
  597.                         # IF SPEED ALREADY HIGHEST - DO NOTHING
  598.                         elif TurnSpeed == SpeedHigh:
  599.                             print 'Highest turning speed reached'
  600.                        
  601.                        
  602.                     #======================================================
  603.                     # TURN THE TURNING SPEED DOWN
  604.                        
  605.                     if event.key == pygame.K_4: # 4 KEY PRESSED
  606.        
  607.                         # IF SPEED ALREADY LOWEST - DO NOTHING
  608.                         if TurnSpeed == SpeedLow:
  609.                             print 'Lowest turning speed reached'
  610.                
  611.                         # IF SPEED IS MEDIUM - SET TO LOW
  612.                         elif TurnSpeed == SpeedMedium:
  613.                             print 'Dropping to low turning speed. Speed:', SpeedLow
  614.                             TurnSpeed = SpeedLow
  615.                            
  616.                         # IF SPEED IS HIGH - SET TO MEDIUM
  617.                         elif TurnSpeed == SpeedHigh:
  618.                             print 'Dropping to medium turning speed. Speed:', SpeedMedium
  619.                             TurnSpeed = SpeedMedium
  620.    
  621.                
  622.                         #======================================================
  623.                         # EXIT THE PROGRAM AND BACK TO MENU
  624.  
  625.                         if event.key == pygame.K_ESCAPE: # ESCAPE KEY PRESSED
  626.                            
  627.                             # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
  628.                             bus.write_byte_data(addr, 0x13, 7)
  629.                             bus.write_byte_data(addr, 0x12, underscore)
  630.                             time.sleep(0.5)
  631.                            
  632.                             # RESET GLOBAL SPEEDS FOR OTHER PROGRAMS THAT HAVE FIXED SPEED
  633.                             TurnSpeed = SpeedHigh
  634.                             ForwardSpeedL = ForwardHighL
  635.                             ForwardSpeedR = ForwardHighR
  636.                             ReverseSpeedL = ReverseHighL
  637.                             ReverseSpeedR = ReverseHighR
  638.  
  639.                             print "Returning to main menu"
  640.                             time.sleep(0.5)
  641.                            
  642.                             # GO BACK TO MAIN MENU
  643.                             menu()
  644.                        
  645.                        
  646.                 #########################################
  647.                 # IF A KEY IS NOT BEING PRESSED...      #
  648.                 #########################################
  649.        
  650.                 elif event.type == pygame.KEYUP: # IF A KEY IS RELEASED
  651.                
  652.                     # STOP ALL MOVEMENT (WHEN NO KEY PRESSED)
  653.  
  654.                    
  655.                     if event.key == pygame.K_UP: # UP KEY RELEASED
  656.                         stop() # STOP MOTORS
  657.                         print 'STOP - No key press'
  658.                        
  659.                     if event.key == pygame.K_DOWN: # DOWN KEY RELEASED
  660.                         stop() # STOP MOTORS
  661.                         print 'STOP - No key press'
  662.                        
  663.                     if event.key == pygame.K_RIGHT: # RIGHT KEY RELEASED
  664.                         stop() # STOP MOTORS
  665.                         print 'STOP - No key press'
  666.                        
  667.                     if event.key == pygame.K_LEFT: # LEFT KEY RELEASED
  668.                         stop() # STOP MOTORS
  669.                         print 'STOP - No key press'
  670.                        
  671.                     if event.key == pygame.K_1: # 1 KEY RELEASED
  672.                         stop() # STOP MOTORS
  673.                         print 'STOP - No key press'
  674.                        
  675.                     if event.key == pygame.K_2: # 2 KEY RELEASED
  676.                         stop() # STOP MOTORS
  677.                         print 'STOP - No key press'
  678.                        
  679.                     if event.key == pygame.K_3: # 3 KEY RELEASED
  680.                         stop() # STOP MOTORS
  681.                         print 'STOP - No key press'
  682.                        
  683.                     if event.key == pygame.K_4: # 4 KEY RELEASED
  684.                         stop() # STOP MOTORS
  685.                         print 'STOP - No key press'
  686.                        
  687.                     if event.key == pygame.K_ESCAPE: # ESCAPE KEY RELEASED
  688.                         stop() # STOP MOTORS
  689.                         print 'STOP - No key press'
  690.                        
  691.  
  692.     #======================================================================
  693.     # DEFINE PROXIMITY MODULE
  694.  
  695.     def proximity(): ### PROGRAM 2 ###
  696.        
  697.         countme = 0
  698.        
  699.         while True:
  700.          
  701.             # GET DISTANCE
  702.             dist = getDistance()
  703.  
  704.             # PRINT DISTANCE
  705.             print "------------------------------"
  706.             print "DISTANCE = ", int(dist)
  707.             print "------------------------------"
  708.            
  709.             # DISTANCE MOVEMENT CONTROL
  710.             if dist >= 150: # IF DISTANCE EQUAL TO OR GREATER THAN 150
  711.                 #print "Distance: ", int(dist)
  712.                 #print "Distance greater than 150"
  713.                 Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  714.                 time.sleep(1)
  715.                 #stop()
  716.        
  717.             elif 51 <= dist <= 150: # IF DISTANCE BETWEEN 50 AND 150
  718.                 #print "Distance: ", int(dist)
  719.                 #print "Distance 51 to 150"
  720.                 Forward(ForwardMediumL, ForwardMediumR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  721.                 time.sleep(0.3)
  722.                 #stop()
  723.            
  724.             elif 17 <= dist <= 50: # IF DISTANCE BETWEEN 15 AND 50
  725.                 #print "Distance: ", int(dist)
  726.                 #print "Distance 17 to 50"
  727.                 Forward(ForwardLowL, ForwardLowR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  728.                 time.sleep(0.01)
  729.                 #stop()
  730.            
  731.             elif 10 <= dist <= 16: # IF DISTANCE BETWEEN 8 AND 15
  732.                 #print "Distance: ", int(dist)
  733.                 #print "Distance is 10 to 16"
  734.                 Forward(ForwardLowL, ForwardLowR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  735.                 time.sleep(0.001)
  736.                 #stop()
  737.  
  738.             elif 0 <= dist <= 9 and countme <= 7: # IF DISTANCE BETWEEN 0 AND 9
  739.                 print "0-8", countme
  740.                 stop() # STOP MOVING
  741.                 countme = countme+1
  742.                 time.sleep(0.5)
  743.                
  744.             elif 0 <= dist <= 9 and countme == 8: # IF DISTANCE BETWEEN 0 AND 9
  745.                 print "countme 5..."
  746.                 stop() # STOP MOVING
  747.                 #print "PROGRAM COMPLETE - KILLING PROGRAM"
  748.                 time.sleep(0.5)
  749.                
  750.                 # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
  751.                 bus.write_byte_data(addr, 0x13, 7)
  752.                 bus.write_byte_data(addr, 0x12, underscore)
  753.                 time.sleep(0.5)
  754.                
  755.                 print "Returning to main menu"
  756.                 time.sleep(0.5)
  757.                
  758.                 # RETURN TO MAIN MENU
  759.                 menu()
  760.  
  761.                
  762.     #======================================================================
  763.     # DEFINE THREE POINT TURN MODULE   
  764.  
  765.     def threepoint(): ### PROGRAM 3 ###
  766.    
  767.         Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  768.         time.sleep(3.5)
  769.        
  770.         Left(SpeedMedium) # SPIN LEFT AT SET SPEED
  771.         time.sleep(0.6)
  772.        
  773.         Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  774.         time.sleep(0.8)
  775.        
  776.         Reverse(ReverseHighL, ReverseHighR) # REVERSE FOR THE TIME IN TIME.SLEEP BELOW
  777.         time.sleep(1.7)
  778.        
  779.         Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  780.         time.sleep(0.8)
  781.        
  782.         Left(SpeedMedium) # SPIN LEFT AT SET SPEED
  783.         time.sleep(0.5)
  784.        
  785.         Forward(ForwardHighL, ForwardHighR) # MOVE FORWARD FOR THE TIME IN TIME.SLEEP BELOW
  786.         time.sleep(3.5)
  787.        
  788.         # STOP THE ROBOT
  789.         stop()
  790.        
  791.         # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
  792.         bus.write_byte_data(addr, 0x13, 7)
  793.         bus.write_byte_data(addr, 0x12, underscore)
  794.         time.sleep(0.5)
  795.        
  796.         print "Returning to main menu"
  797.         time.sleep(0.5)
  798.        
  799.         # RETURN TO MAIN MENU
  800.         menu()
  801.  
  802.        
  803.     #======================================================================
  804.     # DEFINE LINE FOLLOWER MODULE
  805.  
  806.     def linefollow(): ### PROGRAM 4 ###
  807.    
  808.     ## Line sensor: http://4tronix.co.uk/store/index.php?rt=product/product&product_id=144
  809.     ## Logic:
  810.     ##     When seeing white (off of the line) = Low logic
  811.     ##     When seeing black (on the line) = High logic
  812.    
  813.         print "LineFollow script started"
  814.         while True:
  815.             print "while started"
  816.            
  817.             # IF SWITCH PRESSED
  818.             if GPIO.input(switch1):
  819.                 print "linefollow script exit"
  820.                 stop()
  821.                 time.sleep(0.5)
  822.                 bus.write_byte_data(addr, 0x13, 7)
  823.                 bus.write_byte_data(addr, 0x12, underscore)
  824.                 menu()
  825.            
  826.             # X-0 or X-X-0
  827.             elif (GPIO.input(lineLeft) == 1 and GPIO.input(lineRight) == 0) or (GPIO.input(lineLeft) == 1 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 0) :
  828.                 print "X-0-0"
  829.                 LastDir = 1
  830.                 Left(40) # SPIN LEFT AT LOW SPEED SETTING
  831.                 time.sleep(0.2)
  832.                
  833.             # 0-X or 0-X-X
  834.             elif (GPIO.input(lineLeft) == 0 and GPIO.input(lineRight) == 1) or (GPIO.input(lineLeft) == 0 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 1):
  835.                 print "0-0-X"
  836.                 LastDir = 3
  837.                 Right(40) # SPIN RIGHT AT LOW SPEED SETTING
  838.                 time.sleep(0.2)
  839.                
  840.             # 0-X-0
  841.             elif GPIO.input(lineLeft) == 0 and GPIO.input(lineMiddle) == 1 and GPIO.input(lineRight) == 0:
  842.                 print "0-X-0"
  843.                 Forward (30,30) # FORWARD AT LOW SPEED SETTING
  844.                 time.sleep(0.1)
  845.  
  846.  
  847.     #======================================================================
  848.     # DEFINE SKITTLES MODULE
  849.     # Will always be same speed as default set initially before try block
  850.     # Could add speed control but seems excessive
  851.  
  852.     def skittles(): ### PROGRAM 5 ###
  853.    
  854.         global ForwardSpeedL
  855.         global ForwardSpeedR
  856.         global ReverseSpeedL
  857.         global ReverseSpeedR
  858.         global TurnSpeed
  859.    
  860.         while True:
  861.        
  862.             for event in pygame.event.get():
  863.                 print 'General Control PyGame Event'
  864.                
  865.                 #########################################
  866.                 # IF A KEY IS BEING PRESSED...          #
  867.                 #########################################
  868.                
  869.                 if event.type == pygame.KEYDOWN: # IF A KEY IS BEING PRESSED
  870.        
  871.                     #======================================================
  872.                     # BASIC MOVEMENT (FWD, REV, RIGHT, LEFT)
  873.                        
  874.                     # FORWARD (UP KEY)
  875.                     if event.key == pygame.K_UP: # UP KEY PRESSED
  876.                         Forward(ForwardSpeedL, ForwardSpeedR) # FORWARD AT SET SPEED
  877.                         print 'Forward speed', ForwardSpeedL, ForwardSpeedR # PRINT SPEED
  878.                                
  879.                     # REVERSE (DOWN KEY)
  880.                     if event.key == pygame.K_DOWN: # DOWN KEY PRESSED
  881.                         Reverse(ReverseSpeedL, ReverseSpeedR) # REVERSE AT SET SPEED
  882.                         print 'Reverse speed:', ReverseSpeedL, ReverseSpeedR # PRINT SPEED
  883.                
  884.                     # RIGHT (RIGHT KEY)
  885.                     if event.key == pygame.K_RIGHT: # RIGHT KEY PRESSED
  886.                         Right(TurnSpeed) # SPIN RIGHT AT SET SPEED
  887.                         print 'Spin right speed:', TurnSpeed # PRINT SPEED
  888.                
  889.                     # LEFT (LEFT KEY)
  890.                     if event.key == pygame.K_LEFT: # LEFT KEY PRESSED
  891.                         Left(TurnSpeed) # SPIN LEFT AT SET SPEED
  892.                         print 'Spin left speed', TurnSpeed # PRINT SPEED
  893.                        
  894.                     #======================================================
  895.                     # SERVO PADDLE CONTROL
  896.            
  897.                     # FIRE PADDLE
  898.                     if event.key == pygame.K_f: # F KEY PRESSED
  899.                         print "Firing"
  900.                        
  901.                         # FIRE PADDLE BY MOVING SERVOS
  902.                         servo1.ChangeDutyCycle(10) #180
  903.                         servo2.ChangeDutyCycle(5) #0
  904.                        
  905.                         # WAIT 3 SECONDS
  906.                         time.sleep(1)
  907.            
  908.                         # AUTOMATICALLY RESET PADDLE AFTER FIRING
  909.                         print "Resetting"
  910.                         servo1.ChangeDutyCycle(4) #0
  911.                         servo2.ChangeDutyCycle(11) #180
  912.                        
  913.                        
  914.                     #======================================================
  915.                     # TURN THE FORWARD/REVERSE SPEED UP
  916.                     # Looks at left AND right speed and changes both at the same time
  917.                     # This maintains PWM left/right balancing to handle inaccuracies
  918.  
  919.                     if event.key == pygame.K_1: # 1 KEY PRESSED
  920.                    
  921.                         # IF SPEED IS LOW - SET TO MEDIUM
  922.                         if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
  923.                             print 'Increasing to medium speed'
  924.                             print 'Forward:', ForwardMediumL, ForwardMediumR
  925.                             print 'Reverse:', ReverseMediumL, ReverseMediumR
  926.                             ForwardSpeedL = ForwardMediumL
  927.                             ForwardSpeedR = ForwardMediumR                          
  928.                             ReverseSpeedL = ReverseMediumL
  929.                             ReverseSpeedR = ReverseMediumR
  930.                            
  931.  
  932.                         # IF SPEED IS MEDIUM - SET TO HIGH
  933.                         elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
  934.                             print 'Increasing to high speed'
  935.                             print 'Forward:', ForwardHighL, ForwardHighR
  936.                             print 'Reverse:', ReverseHighL, ReverseHighR
  937.                             ForwardSpeedL = ForwardHighL
  938.                             ForwardSpeedR = ForwardHighR
  939.                             ReverseSpeedL = ReverseHighL
  940.                             ReverseSpeedR = ReverseHighR
  941.                
  942.                         # IF SPEED ALREADY HIGHEST - DO NOTHING
  943.                         elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
  944.                             print 'Highest speed reached!'
  945.                            
  946.                            
  947.                     #======================================================
  948.                     # TURN FORWARD/REVERSE SPEED DOWN
  949.                     # Looks at left AND right speed and changes both at the same time
  950.                     # This maintains PWM left/right balancing to handle inaccuracies
  951.                            
  952.                     if event.key == pygame.K_2: # 2 KEY PRESSED
  953.                            
  954.                         # IF SPEED ALREADY LOWEST - DO NOTHING
  955.                         if (ForwardSpeedL == ForwardLowL and ForwardSpeedR == ForwardLowR and ReverseSpeedL == ReverseLowL and ReverseSpeedR == ReverseLowR):
  956.                             print 'Lowest speed reached'
  957.  
  958.                         # IF SPEED IS MEDIUM - SET TO LOW
  959.                         elif (ForwardSpeedL == ForwardMediumL and ForwardSpeedR == ForwardMediumR and ReverseSpeedL == ReverseMediumL and ReverseSpeedR == ReverseMediumR):
  960.                             print 'Dropping to low speed'
  961.                             print 'Forward:', ForwardLowL, ForwardLowR
  962.                             print 'Reverse:', ReverseLowL, ReverseLowR
  963.                             ForwardSpeedL = ForwardLowL
  964.                             ForwardSpeedR = ForwardLowR
  965.                             ReverseSpeedL = ReverseLowL
  966.                             ReverseSpeedR = ReverseLowR
  967.                
  968.                         # IF SPEED IS HIGH - SET TO MEDIUM
  969.                         elif (ForwardSpeedL == ForwardHighL and ForwardSpeedR == ForwardHighR and ReverseSpeedL == ReverseHighL and ReverseSpeedR == ReverseHighR):
  970.                             print 'Dropping to medium speed'
  971.                             print 'Forward:', ForwardMediumL, ForwardMediumR
  972.                             print 'Reverse:', ReverseMediumL, ReverseMediumR
  973.                             ForwardSpeedL = ForwardMediumL
  974.                             ForwardSpeedR = ForwardMediumR
  975.                             ReverseSpeedL = ReverseMediumL
  976.                             ReverseSpeedR = ReverseMediumR
  977.                            
  978.                     #======================================================
  979.                     # TURN THE TURNING SPEED UP
  980.                        
  981.                     if event.key == pygame.K_3: # 3 KEY PRESSED
  982.        
  983.                         # IF SPEED IS LOW - SET TO MEDIUM
  984.                         if TurnSpeed == SpeedLow:
  985.                             print 'Increasing to medium turning speed. Speed:', SpeedMedium
  986.                             TurnSpeed = SpeedMedium
  987.                    
  988.                         # IF SPEED IS MEDIUM - SET TO HIGH
  989.                         elif TurnSpeed == SpeedMedium:
  990.                             print 'Increasing to high turning speed. Speed:', SpeedHigh
  991.                             TurnSpeed = SpeedHigh
  992.                
  993.                         # IF SPEED ALREADY HIGHEST - DO NOTHING
  994.                         elif TurnSpeed == SpeedHigh:
  995.                             print 'Highest turning speed reached'
  996.                        
  997.                        
  998.                     #======================================================
  999.                     # TURN THE TURNING SPEED DOWN
  1000.                        
  1001.                     if event.key == pygame.K_4: # 4 KEY PRESSED
  1002.        
  1003.                         # IF SPEED ALREADY LOWEST - DO NOTHING
  1004.                         if TurnSpeed == SpeedLow:
  1005.                             print 'Lowest turning speed reached'
  1006.                
  1007.                         # IF SPEED IS MEDIUM - SET TO LOW
  1008.                         elif TurnSpeed == SpeedMedium:
  1009.                             print 'Dropping to low turning speed. Speed:', SpeedLow
  1010.                             TurnSpeed = SpeedLow
  1011.                            
  1012.                         # IF SPEED IS HIGH - SET TO MEDIUM
  1013.                         elif TurnSpeed == SpeedHigh:
  1014.                             print 'Dropping to medium turning speed. Speed:', SpeedMedium
  1015.                             TurnSpeed = SpeedMedium
  1016.                        
  1017.                     #======================================================
  1018.                     # EXIT THE PROGRAM AND BACK TO MENU
  1019.                        
  1020.                     if event.key == pygame.K_ESCAPE: # ESCAPE KEY PRESSED
  1021.                         print "Returning to main menu"
  1022.                         time.sleep(0.5)
  1023.                        
  1024.                         # SET SEGMENT BACK TO UNDERSCORE FOR THE MAIN MENU
  1025.                         bus.write_byte_data(addr, 0x13, 7)
  1026.                         bus.write_byte_data(addr, 0x12, underscore)
  1027.                         time.sleep(0.5)
  1028.                        
  1029.                         # RESET GLOBAL SPEEDS FOR OTHER PROGRAMS THAT HAVE FIXED SPEED
  1030.                         TurnSpeed = SpeedHigh
  1031.                         ForwardSpeedL = ForwardHighL
  1032.                         ForwardSpeedR = ForwardHighR
  1033.                         ReverseSpeedL = ReverseHighL
  1034.                         ReverseSpeedR = ReverseHighR
  1035.  
  1036.                         print "Returning to main menu"
  1037.                         time.sleep(0.5)
  1038.                        
  1039.                         # GO BACK TO MAIN MENU
  1040.                         menu()
  1041.                        
  1042.  
  1043.                 #########################################
  1044.                 # IF A KEY IS NOT BEING PRESSED...      #
  1045.                 #########################################
  1046.                
  1047.                 # STOP ALL MOVEMENT (WHEN NO KEY PRESSED) ################################################ can we just add stop() under the keyup?
  1048.                                                           ################################################ or use OR statements?
  1049.        
  1050.                 elif event.type == pygame.KEYUP: # IF A KEY IS RELEASED
  1051.                        
  1052.                     if event.key == pygame.K_UP: # UP KEY RELEASED
  1053.                         stop() # STOP MOTORS
  1054.                         print 'STOP - No key press'
  1055.                            
  1056.                     if event.key == pygame.K_DOWN: # DOWN KEY RELEASED
  1057.                         stop() # STOP MOTORS
  1058.                         print 'STOP - No key press'
  1059.                            
  1060.                     if event.key == pygame.K_RIGHT: # RIGHT KEY RELEASED
  1061.                         stop() # STOP MOTORS
  1062.                         print 'STOP - No key press'
  1063.            
  1064.                     if event.key == pygame.K_LEFT: # LEFT KEY RELEASED
  1065.                         stop() # STOP MOTORS
  1066.                         print 'STOP - No key press'
  1067.        
  1068.                     if event.key == pygame.K_f: # K KEY RELEASED
  1069.                         stop() # STOP MOTORS
  1070.                         print 'STOP - No key press'
  1071.                        
  1072.                     if event.key == pygame.K_1: # 1 KEY RELEASED
  1073.                         stop() # STOP MOTORS
  1074.                         print 'STOP - No key press'
  1075.        
  1076.                     if event.key == pygame.K_2: # 2 KEY RELEASED
  1077.                         stop() # STOP MOTORS
  1078.                         print 'STOP - No key press'
  1079.                        
  1080.                     if event.key == pygame.K_3: # 3 KEY RELEASED
  1081.                         stop() # STOP MOTORS
  1082.                         print 'STOP - No key press'
  1083.        
  1084.                     if event.key == pygame.K_4: # 4 KEY RELEASED
  1085.                         stop() # STOP MOTORS
  1086.                         print 'STOP - No key press'
  1087.                        
  1088.                     if event.key == pygame.K_ESCAPE: # ESCAPE KEY RELEASED
  1089.                         stop() # STOP MOTORS
  1090.                         print 'STOP - No key press'
  1091.    
  1092.        
  1093.     #======================================================================
  1094.     # DEFINE CLEAN EXIT MENU OPTION (DISPLAY OFF AND GPIO CLEANUP)
  1095.                    
  1096.     def endscript(): ### PROGRAM 6 ###
  1097.    
  1098.         quit() # WILL RUN THE 'FINALLY' BLOCK AS WELL
  1099.        
  1100.        
  1101.     #======================================================================
  1102.     # DEFINE HALT COMMAND (SHUT DOWN)
  1103.                    
  1104.     def haltcommand(): ### PROGRAM 7 ###
  1105.         print "Shutting down system"
  1106.         time.sleep(1)
  1107.        
  1108.         # TURN OFF 4TRONIX DISPLAY BOARD
  1109.         bus.write_byte_data(addr, 0x13, 0xff)
  1110.         time.sleep(0.5)
  1111.        
  1112.         # TURN OFF MOTORS
  1113.         print "MOTORS OFF"
  1114.         stop()
  1115.         time.sleep(0.5)
  1116.        
  1117.         # CLEAN UP GPIOS
  1118.         GPIO.cleanup()
  1119.         time.sleep(0.5)
  1120.        
  1121.         # QUIT PYGAME
  1122.         pygame.quit()
  1123.         time.sleep(0.5)
  1124.        
  1125.         # SHUT DOWN
  1126.         os.system("sudo halt") # After this it will try and run the finally block but will already be shutting down
  1127.        
  1128.        
  1129.     #======================================================================
  1130.     # DEFINE MOTOR FUNCTIONS (PWM SETTINGS)
  1131.  
  1132.     # STOP ALL MOTORS
  1133.     def stop():
  1134.         p.ChangeDutyCycle(0)
  1135.         q.ChangeDutyCycle(0)
  1136.         a.ChangeDutyCycle(0)
  1137.         b.ChangeDutyCycle(0)
  1138.  
  1139.     # LEFT(SPEED): SETS MOTORS TO TURN OPPOSITE DIRECTIONS AT SPEED. 0 <= SPEED <= 100
  1140.     def Left(speed):
  1141.         p.ChangeDutyCycle(0)
  1142.         q.ChangeDutyCycle(speed)
  1143.         a.ChangeDutyCycle(speed)
  1144.         b.ChangeDutyCycle(0)
  1145.        
  1146.     # RIGHT(SPEED): SETS MOTORS TO TURN OPPOSITE DIRECTIONS AT SPEED. 0 <= SPEED <= 100
  1147.     def Right(speed):
  1148.         p.ChangeDutyCycle(speed)
  1149.         q.ChangeDutyCycle(0)
  1150.         a.ChangeDutyCycle(0)
  1151.         b.ChangeDutyCycle(speed)
  1152.        
  1153.     # FORWARD(LEFTSPEED, RIGHTSPEED): MOVES FORWARDS IN AN ARC BY SETTING DIFFERENT SPEEDS. 0 <= LEFTSPEED,RIGHTSPEED <= 100
  1154.     def Forward(leftSpeed, rightSpeed):
  1155.         p.ChangeDutyCycle(leftSpeed)
  1156.         q.ChangeDutyCycle(0)
  1157.         a.ChangeDutyCycle(rightSpeed)
  1158.         b.ChangeDutyCycle(0)
  1159.        
  1160.     # REVERSE(LEFTSPEED, RIGHTSPEED): MOVES BACKWARDS IN AN ARC BY SETTING DIFFERENT SPEEDS. 0 <= LEFTSPEED,RIGHTSPEED <= 100
  1161.     def Reverse(leftSpeed, rightSpeed):
  1162.         p.ChangeDutyCycle(0)
  1163.         q.ChangeDutyCycle(leftSpeed)
  1164.         a.ChangeDutyCycle(0)
  1165.         b.ChangeDutyCycle(rightSpeed)
  1166.        
  1167.  
  1168.     #======================================================================
  1169.     # DEFINE SR-04 FUNCTIONS
  1170.  
  1171.     # GETDISTANCE(). RETURNS THE DISTANCE IN CM TO THE NEAREST OBJECT
  1172.     def getDistance():
  1173.         GPIO.setup(sonar, GPIO.OUT)
  1174.         # SEND 10US PULSE TO TRIGGER
  1175.         GPIO.output(sonar, True)
  1176.         time.sleep(0.00001)
  1177.         GPIO.output(sonar, False)
  1178.         start = time.time()
  1179.         count=time.time()
  1180.         GPIO.setup(sonar,GPIO.IN)
  1181.        
  1182.         while GPIO.input(sonar)==0 and time.time()-count<0.1:
  1183.             start = time.time()
  1184.         count=time.time()
  1185.         stop=count
  1186.        
  1187.         while GPIO.input(sonar)==1 and time.time()-count<0.1:
  1188.             stop = time.time()
  1189.            
  1190.         # CALCULATE PULSE LENGTH
  1191.         elapsed = stop-start
  1192.        
  1193.         # DISTANCE PULSE TRAVELLED IN THAT TIME IS TIME
  1194.         # MULTIPLIED BY THE SPEED OF SOUND (CM/S)
  1195.         distance = elapsed * 34000
  1196.        
  1197.         # THAT WAS THE DISTANCE THERE AND BACK SO HALVE THE VALUE
  1198.         distance = distance / 2
  1199.         return distance            
  1200.        
  1201.  
  1202.     #======================================================================
  1203.     # !! IMPORTANT !! MAKE THE SCRIPT RUN 'MENU' AT THE START !!
  1204.     # Without this, noting will happen!
  1205.    
  1206.     menu()
  1207.  
  1208. #======================================================================
  1209. # FINALLY/EXCEPT BLOCK
  1210. # This will run if there is an error or we choose to exit the program
  1211. # We use Finally in the end code, but have Except ready for debuging
  1212.    
  1213. #finally:
  1214. except KeyboardInterrupt: # USE THIS OPTION FOR DEBUGGING
  1215.  
  1216.     print "EXIT SCRIPT"
  1217.     time.sleep(1)
  1218.    
  1219.     # TURN OFF 4TRONIX DISPLAY
  1220.     print "TURNING OFF DISPLAY"
  1221.     time.sleep(0.5)
  1222.     bus.write_byte_data(addr, 0x13, 0xff) # Set all of bank 1 to High (Off)
  1223.    
  1224.     # TURN OFF MOTORS
  1225.     print "MOTORS OFF"
  1226.     stop()
  1227.     time.sleep(0.5)
  1228.    
  1229.     # CLEAN UP GPIOS
  1230.     print "PERFORMING GPIO CLEANUP"
  1231.     time.sleep(0.5)
  1232.     GPIO.cleanup()  
  1233.    
  1234.     # EXIT PYGAME
  1235.     print "EXIT PYGAME"
  1236.     time.sleep(0.5)
  1237.     pygame.quit()
  1238.    
  1239.     # EXIT PROGRAM
  1240.     print "--- EXIT NOW ---"
  1241.     time.sleep(0.5)
  1242.     quit()
Advertisement
RAW Paste Data Copied
Advertisement