GoPiGo/Software/Scratch/GoPiGoScratch.py at CDev · ExplodingCow/GoPiGo

491 lines (444 loc) · 12.7 KB
#!/usr/bin/python
###############################################################################################################                                                               
# This library is for using the GoPiGo with Scratch
# http://www.dexterindustries.com/GoPiGo/                                                                
# ------------------------------------------------
# Author     Date      		Comments
# Karan      28 July 14  	Initial Authoring                                                            
# These files have been made available online through a Creative Commons Attribution-ShareAlike 3.0  license.
# (http://creativecommons.org/licenses/by-sa/3.0/)           
# Based on the BrickPi Scratch Library written by Jaikrishna
# The Python program acts as the Bridge between Scratch & GoPiGo and must be running for the Scratch program to run.
##############################################################################################################
 GoPiGo for the Raspberry Pi: an open source robotics platform for the Raspberry Pi.
 Copyright (C) 2015  Dexter Industries
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/gpl-3.0.txt>.
import scratch,sys,threading,math
from gopigo import *
en_line_sensor=1
en_ir_sensor=0
if en_line_sensor:
	import line_sensor as l
#360 roation is ~64 encoder pulses
#or 5 deg/pulse
#Deg:Pulse Ratio
DPR = 360.0/64
WHEEL_RAD = 3.25 # Wheels are ~6.5 cm diameter. 
CHASS_WID = 13.5 # Chassis is ~13.5 cm wide.
## This should probably be moved into a gopigo python module.
def cm2pulse(dist):
    Calculate the number of pulses to move the chassis dist cm.
    pulses = dist * [pulses/revolution]/[dist/revolution]
    wheel_circ = 2*math.pi*WHEEL_RAD # [cm/rev] cm traveled per revolution of wheel
    print 'WHEEL_RAD',WHEEL_RAD
    revs = dist/wheel_circ
    print 'revs',revs
    PPR = 18 # [p/rev] encoder Pulses Per wheel Revolution
    pulses = PPR*revs # [p] encoder pulses required to move dist cm.
    print 'pulses',pulses
    return pulses
fw_version=fw_ver()
print "GoPiGo Scratch: Current firmware version:",fw_ver()
if fw_version > 1.2:
	print "GoPiGo Scratch: Please Install the new firmware for the GoPiGo (v1.2+) to use GoPiGo with Scratch. \nPress enter to exit"
	raw_input()
	sys.exit()
    s = scratch.Scratch()
    if s.connected:
        print "GoPiGo Scratch: Connected to Scratch successfully"
    #sys.exit(0)
except scratch.ScratchError:
    print "GoPiGo Scratch: Scratch is either not opened or remote sensor connections aren't enabled"
    #sys.exit(0)
class myThread (threading.Thread):     
    def __init__(self, threadID, name, counter):
        threading.Thread.__init__(self)
        self.threadID = threadID
        self.name = name
        self.counter = counter
    def run(self):
        while running:
            time.sleep(.2)              # sleep for 200 ms
thread1 = myThread(1, "Thread-1", 1)        #Setup and start the thread
thread1.setDaemon(True)
    s.broadcast('READY')
except NameError:
	print "GoPiGo Scratch: Unable to Broadcast"
while True:
		m = s.receive()
		while m[0] == 'sensor-update' :
			m = s.receive()
		msg = m[1]
		if msg == 'SETUP' :
			print "GoPiGo Scratch: Setting up sensors done"
		elif msg == 'START' :
			running = True
			if thread1.is_alive() == False:
				thread1.start()
			print "GoPiGo Scratch: Service Started"
		# Stop the GoPiGo when "STOP" is received from scratch
		elif msg == 'STOP' :
			if en_gpg:
			if en_debug:
				print msg
		# Move the GoPiGo forward when "FORWARD" is received from scratch
		elif msg[:7]=="FORWARD":
			if en_gpg:
				if len(msg) > 7:
					dist = int(msg[7:])
					pulse = int(cm2pulse(dist))
					enc_tgt(1,1,pulse)
			if en_debug:
				print msg
		# Move the GoPiGo back when "BACKWARD" is received from scratch
		elif msg[:8]=="BACKWARD":
			if en_gpg:
				if len(msg) > 8:
					dist = int(msg[8:])
					pulse = int(cm2pulse(dist))
					enc_tgt(1,1,pulse)
			if en_debug:
				print msg
		# Turn the GoPiGo left when "LEFT" is received from scratch
		elif msg[:4]=="LEFT":
			if en_gpg:
				if len(msg) > 4:
					deg= int(msg[4:])
					pulse= int(deg/DPR)
					enc_tgt(0,1,pulse)
			if en_debug:
				print msg
		# Turn the GoPiGo right when "RIGHT" is received from scratch
		elif msg[:5]=="RIGHT":
			if en_gpg:
				if len(msg) > 5:
					deg= int(msg[5:])
					pulse= int(deg/DPR)
					enc_tgt(1,0,pulse)
				right()
			if en_debug:
				print msg
		# Turn the GoPiGo right when "RIGHT" is received from scratch
		elif msg[:5]=="SPEED":
			if en_gpg:
				speed= int(msg[5:])
				set_speed(speed)
			if en_debug:
				print msg
		# Increase the speed of GoPiGo when "INCREASE SPEED" is received from scratch
		elif msg=="INCREASE SPEED":
			if en_gpg:
				increase_speed()
			if en_debug:
				print msg
		# Decrease the speed of GoPiGo when "DECREASE SPEED" is received from scratch
		elif msg=="DECREASE SPEED":
			if en_gpg:
				decrease_speed()
			if en_debug:
				print msg
		# Turn On or Off the left LED
		elif msg[:4]=="LEDL":
			if en_debug:
				print msg
			l_led_pow=int(msg[4:])
			if en_gpg:
				if l_led_pow > 127:
					led_on(1)
					led_off(1)
		# Turn On or Off the Right LED
		elif msg[:4]=="LEDR":
			if en_debug:
				print msg
			r_led_pow=int(msg[4:])
			if en_gpg:
				if r_led_pow > 127:
					led_on(0)
					led_off(0)
		elif msg[:9]=="WHEEL ROT":
			if en_debug:
				print msg
			dist= int(msg[9:])
			if en_gpg:
				enc_tgt(1,1,dist)
		## Perhaps these should become
		## helper or convenience fcns
		## in the gopigo package.
		## and then below would just reference them.
		elif msg[:3]=="SER":
			if en_debug:
				print msg
			srv_pos=int(msg[3:])
			if en_gpg:
				if srv_pos > 180:
					srv_pos = 180
				elif srv_pos < 0:
					srv_pos = 0
				servo(srv_pos)
		# Get distance from the ultrasonic sensor connected to port A1
		elif msg=="GET_DIST":
			if en_debug:
				print "Received distance request."
				print msg
			dist= us_dist(15)
			if en_gpg:
				s.sensorupdate({'distance':dist})
		# Get value from the light sensor connected to Port A1
		elif msg=="LIGHT":
			# print "LIGHTS!"
			mode = "INPUT"
				a = pinMode(pin, mode)
				if en_debug:
					e = sys.exc_info()[1]
					print "Error reading light sensor: " + str(e)
			#time.sleep(0.1)
			light = analogRead(pin)
			if en_debug:
				print "Light Reading: " + str(light)
			if en_gpg:
				s.sensorupdate({'light':light})
		# Get value from the button connected to the port (A1 or D10) specified in the message
		elif msg[:6]=="BUTTON":
			print "BUTTON!",msg
				pin = int(msg[6:])
				mode = "OUTPUT"
				a = pinMode(pin, mode)
				if en_debug:
					e = sys.exc_info()[1]
					print "Error reading button: " + str(e)
			time.sleep(0.1)
			button = digitalRead(pin)
			if en_debug:
				print "Button Reading: " + str(button)
			if en_gpg:
				s.sensorupdate({'button':button})
		# Get the value from the sound sensor connected to port A1
		# elif msg=="SOUND":
			# # print "LIGHTS!"
			# pin = 1
				# sound = analogRead(pin)
			# except:
				# if en_debug:
					# e = sys.exc_info()[1]
					# print "Error reading sound sensor: " + str(e)
			# if en_debug:
				# print "Sound Sensor Reading: " + str(sound)
			# if en_gpg:
				# s.sensorupdate({'sound':sound})
		elif msg=="SOUND":
			print "Sound"
				len=100
				window_size=10
				for j in range(t*50):
					analog_read_value=analogRead(1)
					# Print non zero values
					if analog_read_value<>0:
						peak += analog_read_value
				avg = peak/(t*100)
				# print avg
				if en_debug:
					e = sys.exc_info()[1]
					print "Error reading sound sensor: " + str(e)
			if en_debug:
				print "Sound Sensor Reading: ",peak
			if en_gpg:
				s.sensorupdate({'sound':avg})
		# Make sound from the buzzer connected to the D10 port by giving the power value
		elif msg[:6]=="BUZZER":
			print msg
			pin = 10
				power = int(msg[6:])
				analogWrite(pin,power)
				if en_debug:
					e = sys.exc_info()[1]
					print "Error with buzzer: " + str(e)
		# Set the power in the LED (0-255) connected to port D10
		elif msg[:3]=="LED":
			if en_debug:
				print msg
			led_pow=int(msg[3:])
			if en_gpg:
				analogWrite(pin,led_pow)
		# Get the value from the motion sensor connected to Port D10
		elif msg=="MOTION":
			print "MOTION!"
				mode = "INPUT"
				a = pinMode(pin, mode)
				time.sleep(0.1)
				motion = digitalRead(pin)
				if en_debug:
					e = sys.exc_info()[1]
					print "Error reading motion sensor: " + str(e)
			if en_debug:
				print "Motion Reading: " + str(motion)
			if en_gpg:
				s.sensorupdate({'motion':motion})
		# Get the value from the IR remote when a button is pressed
		# IR Sensor goes on A1 Pin.
		elif msg=="IR":
			print "IR!"
			if en_ir_sensor==0:
				import lirc
				sockid = lirc.init("keyes", blocking = False)
				en_ir_sensor=1
				a= lirc.nextcode()  # press 1 
				if len(a) !=0:
					print a[0]
				if en_debug:
					e = sys.exc_info()[1]
					print "Error reading IR sensor: " + str(a)
			if en_debug:
				print "IR Reading: " + str(a[0])
			if en_gpg:
				s.sensorupdate({'ir':a[0]})
		# Get the value from the Dexter Industries line sensor
		elif msg=="LINE":
			if en_line_sensor:
				print "LINE!"
					line=l.line_position()
				except:
					if en_debug:
						e = sys.exc_info()[1]
						print "Error reading Line sensor: " + str(e)
				if en_debug:
					print "Line Sensor Readings: " + str(line)
				if en_gpg:
					s.sensorupdate({'line':line})
		elif msg=="SET_BLACK_LINE":
			if en_line_sensor:
				print "SET_BLACK_LINE!"
					l.set_black_line()
				except:
					if en_debug:
						e = sys.exc_info()[1]
						print "Error reading Line sensor: " + str(l.black_line)
				if en_debug:
					print "Black Line Sensor Readings: " + str(l.black_line)
				if en_gpg:
					s.sensorupdate({'black_line':l.black_line})
		elif msg=="SET_WHITE_LINE":
			if en_line_sensor:
				print "SET_WHITE_LINE!"
					l.set_white_line()
				except:
					if en_debug:
						e = sys.exc_info()[1]
						print "Error reading Line sensor: " + str(l.white_line)
				if en_debug:
					print "White Line Sensor Readings: " + str(l.white_line)
				if en_gpg:
					s.sensorupdate({'white_line':l.white_line})		
		elif msg=="READ_IR":
			print "READ_IR!" 
			if en_ir_sensor==0:
				import lirc
				sockid = lirc.init("keyes", blocking = False)
				en_ir_sensor=1
				read_ir= lirc.nextcode()  # press 1 
				if len(read_ir) !=0:
					print read_ir[0]
				if en_debug:
					e = sys.exc_info()[1]
					print "Error reading IR sensor: " + str(read_ir)
			if en_debug:
				print "IR Recv Reading: " + str(read_ir)
			if en_gpg:
				if len(read_ir) !=0:
					s.sensorupdate({'read_ir':read_ir[0]})		
					s.sensorupdate({'read_ir':""})
		elif msg=="TAKE_PICTURE":
			print "TAKE_PICTURE!" 
				from subprocess import call
				import datetime
				cmd_start="raspistill -o /home/pi/Desktop/img_"
				cmd_end=".jpg -w 640 -h 480 -t 1"
				dt=str(datetime.datetime.now())
				dt=dt.replace(' ','_',10)
				call ([cmd_start+dt+cmd_end], shell=True)
				print "Picture Taken"
				if en_debug:
					e = sys.exc_info()[1]
					print "Error taking picture"
				s.sensorupdate({'camera':"Error"})	
			s.sensorupdate({'camera':"Picture Taken"})	
			if en_debug:
				print "m",msg
				print "Wrong Command"
    except KeyboardInterrupt:
        running= False
        print "GoPiGo Scratch: Disconnected from Scratch"
        break
    except (scratch.scratch.ScratchConnectionError,NameError) as e:
		while True:
			#thread1.join(0)
			print "GoPiGo Scratch: Scratch connection error, Retrying"
			time.sleep(5)
				s = scratch.Scratch()
				s.broadcast('READY')
				print "GoPiGo Scratch: Connected to Scratch successfully"
			except scratch.ScratchError:
				print "GoPiGo Scratch: Scratch is either not opened or remote sensor connections aren't enabled\n..............................\n"
Provide feedback

Saved searches

Use saved searches to filter your results more quickly

FilesExpand file tree

GoPiGoScratch.py

Latest commit

History

GoPiGoScratch.py

File metadata and controls
