I have a gopigo attached with couple sensors (camera, GPS, air quality sensor).
ID: 3727458 • Letter: I
Question
I have a gopigo attached with couple sensors (camera, GPS, air quality sensor). I am trying to put all 4 code together to have one program(control, camera(live video stream), GPS(location) and air quality sensor. I want all of the 4 codes to run at the same time so that I can control the GOPIGO, view the video which is my eyes, know my location and analyze the condition of the environment with air quality sensor.
Control code in python
from gopigo import * #Has the basic functions for controlling the GoPiGo Robot
import sys #Used for closing the running program
print ("This is a basic example for the GoPiGo Robot control")
print ("Press: w: Move GoPiGo Robot forward a: Turn GoPiGo Robot left d: Turn GoPiGo Robot right s: Move GoPiGo Robot backward t: Increase speed g: Decrease speed x: Stop GoPiGo Robot z: Exit ")
while True:
print ("Enter the Command:")
a=raw_input() # Fetch the input from the terminal
if a=='w':
fwd() # Move forward
elif a=='a':
left() # Turn left
elif a=='d':
right() # Turn Right
elif a=='s':
bwd() # Move back
elif a=='x':
stop() # Stop
elif a=='t':
increase_speed() # Increase speed
elif a=='g':
decrease_speed() # Decrease speed
elif a=='z':
print ("Exiting") # Exit
sys.exit()
else:
print ("Wrong Command, Please Enter Again")
time.sleep(.1)
Camera video streaming code
# import the necessary packages
from picamera.array import PiRGBArray
from picamera import PiCamera
import time
import cv2
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(640, 480))
# allow the camera to warmup
time.sleep(0.1)
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
# show the frame
cv2.imshow("Frame", image)
key = cv2.waitKey(1) & 0xFF
# clear the stream in preparation for the next frame
rawCapture.truncate(0)
# if the `q` key was pressed, break from the loop
if key == ord("q"):
break
Air quality sensor code
import time
import gopigo
# Connect the Grove Air Quality Sensor to analog port A0
# SIG,NC,VCC,GND
air_sensor = 1
gopigo.pinMode(air_sensor,"INPUT")
while True:
try:
# Get sensor value
sensor_value = gopigo.analogRead(air_sensor)
if sensor_value > 500:
print ("Danger-High Pollution Air Quality")
elif sensor_value > 300:
print ("Low pollution")
else:
print ("Air fresh safe to go in with caution")
print ("sensor_value ="+ str(sensor_value))
time.sleep(.5)
except IOError:
print ("Error")
GPS sensor code
from gopigo import *
import serial, time
import smbus
import math
import RPi.GPIO as GPIO
import struct
import sys
import ir_receiver_check
if ir_receiver_check.check_ir():
print "Disable IR receiver before continuing"
exit()
ser = serial.Serial('/dev/ttyAMA0', 9600, timeout = 0) #Open the serial port at 9600 baud
ser.flush()
def readlineCR():
rv = ""
while True:
time.sleep(0.01) # This is the critical part. A small pause
# works really well here.
ch = ser.read()
rv += ch
if ch==' ' or ch=='':
return rv
while True:
#readlineCR()
x=readlineCR()
print x
Explanation / Answer
from __future__ import print_function
from __future__ import division
import sys
import time
import math
import struct
import subprocess
WHEEL_RAD=3.25
WHEEL_CIRC=2*math.pi*WHEEL_RAD
PPR = 18 # encoder Pulses Per Revolution
if sys.platform == 'uwp':
import winrt_smbus as smbus
bus = smbus.SMBus(1)
else:
import RPi.GPIO as GPIO
import smbus
# for RPI version 1, use "bus = smbus.SMBus(0)"
rev = GPIO.RPI_REVISION
if rev == 2 or rev == 3:
bus = smbus.SMBus(1)
else:
bus = smbus.SMBus(0)
# This is the address for the GoPiGo
address = 0x08
debug=0
#GoPiGo Commands
fwd_cmd =[119] #Move forward with PID
motor_fwd_cmd =[105] #Move forward without PID
bwd_cmd =[115] #Move back with PID
motor_bwd_cmd =[107] #Move back without PID
left_cmd =[97] #Turn Left by turning off one motor
left_rot_cmd =[98] #Rotate left by running both motors is opposite direction
right_cmd =[100] #Turn Right by turning off one motor
right_rot_cmd =[110] #Rotate Right by running both motors is opposite direction
stop_cmd =[120] #Stop the GoPiGo
ispd_cmd =[116] #Increase the speed by 10
dspd_cmd =[103] #Decrease the speed by 10
m1_cmd =[111] #Control motor1
m2_cmd =[112] #Control motor2
read_motor_speed_cmd=[114] #Get motor speed back
volt_cmd =[118] #Read the voltage of the batteries
us_cmd =[117] #Read the distance from the ultrasonic sensor
led_cmd =[108] #Turn On/Off the LED's
servo_cmd =[101] #Rotate the servo
enc_tgt_cmd =[50] #Set the encoder targeting
fw_ver_cmd =[20] #Read the firmware version
en_enc_cmd =[51] #Enable the encoders
dis_enc_cmd =[52] #Disable the encoders
read_enc_status_cmd =[53] #Read encoder status
en_servo_cmd =[61] #Enable the servo's
dis_servo_cmd =[60] #Disable the servo's
set_left_speed_cmd =[70] #Set the speed of the right motor
set_right_speed_cmd =[71] #Set the speed of the left motor
en_com_timeout_cmd =[80] #Enable communication timeout
dis_com_timeout_cmd =[81] #Disable communication timeout
timeout_status_cmd =[82] #Read the timeout status
enc_read_cmd =[53] #Read encoder values
trim_test_cmd =[30] #Test the trim values
trim_write_cmd =[31] #Write the trim values
trim_read_cmd =[32]
digital_write_cmd =[12] #Digital write on a port
digital_read_cmd =[13] #Digital read on a port
analog_read_cmd =[14] #Analog read on a port
analog_write_cmd =[15] #Analog read on a port
pin_mode_cmd =[16] #Set up the pin mode on a port
ir_read_cmd =[21]
ir_recv_pin_cmd =[22]
cpu_speed_cmd =[25]
#LED Pins
#MAKE COMPATIBLE WITH OLD FIRMWARE
# LED_L_PIN=17
# LED_R_PIN=16
#port definition
analogPort=15
digitalPort=10
#LED setup
LED_L=1
LED_R=0
# This allows us to be more specific about which commands contain unused bytes
unused = 0
v16_thresh=790
version = 0
'''
#Enable slow i2c (for better stability)
def en_slow_i2c():
#subprocess.call('sudo rmmod i2c_bcm2708',shell=True)
subprocess.call('sudo modprobe i2c_bcm2708 baudrate=70000',shell=True)
'''
#Write I2C block
def write_i2c_block(address,block):
try:
op=bus.write_i2c_block_data(address,1,block)
time.sleep(.005)
return op
except IOError:
if debug:
print ("IOError")
return -1
return 1
#Write a byte to the GoPiGo
def writeNumber(value):
try:
bus.write_byte(address, value)
time.sleep(.005)
except IOError:
if debug:
print ("IOError")
return -1
return 1
#Read a byte from the GoPiGo
def readByte():
try:
number = bus.read_byte(address)
time.sleep(.005)
except IOError:
if debug:
print ("IOError")
return -1
return number
#Control Motor 1
def motor1(direction,speed):
return write_i2c_block(address,m1_cmd+[direction,speed,0])
#Control Motor 2
def motor2(direction,speed):
return write_i2c_block(address,m2_cmd+[direction,speed,0])
#Move the GoPiGo forward
def fwd(dist=0): #distance is in cm
try:
if dist>0:
# this casting to int doesn't seem necessary
pulse=int(PPR*(dist//WHEEL_CIRC) )
enc_tgt(1,1,pulse)
except Exception as e:
print ("gopigo fwd: {}".format(e))
pass
return write_i2c_block(address,motor_fwd_cmd+[0,0,0])
# support more explicit spelling for forward function
forward=fwd
#Move the GoPiGo forward without PID
def motor_fwd():
return write_i2c_block(address,motor_fwd_cmd+[0,0,0])
#Move GoPiGo back
def bwd(dist=0):
try:
if dist>0:
# this casting to int doesn't seem necessary
pulse=int(PPR*(dist//WHEEL_CIRC) )
enc_tgt(1,1,pulse)
except Exception as e:
print ("gopigo bwd: {}".format(e))
pass
return write_i2c_block(address,motor_bwd_cmd+[0,0,0])
# support more explicit spelling for backward function
backward=bwd
#Move GoPiGo back without PID control
def motor_bwd():
return write_i2c_block(address,motor_bwd_cmd+[0,0,0])
#Turn GoPiGo Left slow (one motor off, better control)
def left():
return write_i2c_block(address,left_cmd+[0,0,0])
#Rotate GoPiGo left in same position (both motors moving in the opposite direction)
def left_rot():
return write_i2c_block(address,left_rot_cmd+[0,0,0])
#Turn GoPiGo right slow (one motor off, better control)
def right():
return write_i2c_block(address,right_cmd+[0,0,0])
#Rotate GoPiGo right in same position both motors moving in the opposite direction)
def right_rot():
return write_i2c_block(address,right_rot_cmd+[0,0,0])
DPR = 360.0/64
# turn x degrees to the right
def turn_right(degrees):
pulse = int(degrees//DPR)
enc_tgt(1,0,pulse)
right()
def turn_right_wait_for_completion(degrees):
'''
Same as turn_right() but blocking
'''
turn_right(degrees)
pulse = int(degrees//DPR)
while enc_read(0) < pulse:
pass
# turn x degrees to the left
def turn_left(degrees):
pulse = int(degrees//DPR)
enc_tgt(0,1,pulse)
left()
def turn_left_wait_for_completion(degrees):
'''
same as turn_left() but blocking.
'''
turn_left(degrees)
pulse = int(degrees//DPR)
while enc_read(1) < pulse:
pass
#Stop the GoPiGo
def stop():
return write_i2c_block(address,stop_cmd+[0,0,0])
#Increase the speed
def increase_speed():
return write_i2c_block(address,ispd_cmd+[0,0,0])
#Decrease the speed
def decrease_speed():
return write_i2c_block(address,dspd_cmd+[0,0,0])
#Trim test with the value specified
def trim_test(value):
if value>100:
value=100
elif value<-100:
value=-100
value+=100
write_i2c_block(address,trim_test_cmd+[value,0,0])
#Read the trim value in EEPROM if present else return -3
def trim_read():
write_i2c_block(address,trim_read_cmd+[0,0,0])
time.sleep(.08)
try:
b1=bus.read_byte(address)
b2=bus.read_byte(address)
except IOError:
return -1
if b1!=-1 and b2!=-1:
v=b1*256+b2
if v==255:
return -3
return v
else:
return -1
#Write the trim value to EEPROM, where -100=0 and 100=200
def trim_write(value):
if value>100:
value=100
elif value<-100:
value=-100
value+=100
write_i2c_block(address,trim_write_cmd+[value,0,0])
# Arduino Digital Read
def digitalRead(pin):
if pin ==10 or pin ==15 or pin ==0 or pin ==1:
write_i2c_block(address, digital_read_cmd + [pin, unused, unused])
time.sleep(.1)
n=bus.read_byte(address)
bus.read_byte(address) #Empty the buffer
return n
else:
return -2
# Arduino Digital Write
def digitalWrite(pin, value):
#if pin ==10 or pin ==0 or pin ==1 or pin==5 or pin ==16 or pin==17 :
if value==0 or value ==1:
write_i2c_block(address, digital_write_cmd + [pin, value, unused])
# time.sleep(.005) #Wait for 5 ms for the commands to complete
return 1
#else:
# return -2
# Setting Up Pin mode on Arduino
def pinMode(pin, mode):
# if pin ==10 or pin ==15 or pin ==0 or pin ==1:
if mode == "OUTPUT":
write_i2c_block(address, pin_mode_cmd + [pin, 1, unused])
elif mode == "INPUT":
write_i2c_block(address, pin_mode_cmd + [pin, 0, unused])
#time.sleep(.005) #Wait for 5 ms for the commands to complete
return 1
# else:
# return -2
# Read analog value from Pin
def analogRead(pin):
#if pin == 1 :
write_i2c_block(address, analog_read_cmd + [pin, unused, unused])
time.sleep(.007)
try:
b1=bus.read_byte(address)
b2=bus.read_byte(address)
except IOError:
return -1
return b1* 256 + b2
#else:
# return -2
# Write PWM
def analogWrite(pin, value):
if pin == 10 :
write_i2c_block(address, analog_write_cmd + [pin, value, unused])
return 1
else:
return -2
#Read voltage
# return: voltage in V
def volt():
write_i2c_block(address,volt_cmd+[0,0,0])
time.sleep(.1)
try:
b1=bus.read_byte(address)
b2=bus.read_byte(address)
except IOError:
return -1
if b1!=-1 and b2!=-1:
v=b1*256+b2
v=(5*float(v)/1024)/0.4
return round(v,2)
else:
return -1
#Read board revision
# return: voltage in V
def brd_rev():
write_i2c_block(address, analog_read_cmd + [7, unused, unused])
time.sleep(.1)
try:
b1=bus.read_byte(address)
b2=bus.read_byte(address)
except IOError:
return -1
return b1* 256 + b2
#Read ultrasonic sensor
# arg:
# pin -> Pin number on which the US sensor is connected
# return: distance in cm
def us_dist(pin):
write_i2c_block(address,us_cmd+[pin,0,0])
time.sleep(.08)
try:
b1=bus.read_byte(address)
b2=bus.read_byte(address)
except IOError:
return -1
if b1!=-1 and b2!=-1:
v=b1*256+b2
return v
else:
return -1
def corrected_us_dist(pin):
'''
based on lab experiments, the US sensor has to be corrected
with the following equation:
(x+4.41)/1.423
This seems to give the best results for the sensors on hand
'''
raw_data = float(us_dist(pin))
if raw_data > 0:
corrected_data = (raw_data + 4.41) / 1.423
else:
corrected_data = raw_data
# print(raw_data,corrected_data)
return int(corrected_data)
def read_motor_speed():
write_i2c_block(address,read_motor_speed_cmd+[unused,unused,unused])
try:
s1=bus.read_byte(address)
s2=bus.read_byte(address)
except IOError:
return [-1,-1]
return [s1,s2]
#Turn led on
# arg:
# l_id: 1 for left LED and 0 for right LED
def led_on(l_id):
if check_version() > 14:
r_led=16
l_led=17
else:
r_led=5
l_led=10
if l_id==LED_L or l_id==LED_R:
if l_id==LED_L:
pinMode(l_led,"OUTPUT")
digitalWrite(l_led,1)
elif l_id==LED_R:
pinMode(r_led,"OUTPUT")
digitalWrite(r_led,1)
return 1
else:
return -1
#Turn led off
# arg:
# l_id: 1 for left LED and 0 for right LED
def led_off(l_id):
if check_version()>14:
r_led=16
l_led=17
else:
r_led=5
l_led=10
if l_id==LED_L or l_id==LED_R:
if l_id==LED_L:
pinMode(l_led,"OUTPUT")
digitalWrite(l_led,0)
elif l_id==LED_R:
pinMode(r_led,"OUTPUT")
digitalWrite(r_led,0)
return 1
else:
return -1
#Set servo position
# arg:
# position: angle in degrees to set the servo at
def servo(position):
write_i2c_block(address,servo_cmd+[position,0,0])
#time.sleep(.05)
#Set encoder targeting on
#arg:
# m1: 0 to disable targeting for m1, 1 to enable it
# m2: 1 to disable targeting for m2, 1 to enable it
# target: number of encoder pulses to target (18 per revolution)
def enc_tgt(m1,m2,target):
# print("enc_tgt m1 {} m2 {} target {}".format(m1,m2,target))
if m1>1 or m1<0 or m2>1 or m2<0:
return -1
m_sel=m1*2+m2
write_i2c_block(address,enc_tgt_cmd+[m_sel,target//256,target%256])
return 1
#Read encoder value
# arg:
# motor -> 0 for motor1 and 1 for motor2
# return: distance in cm
def enc_read(motor):
write_i2c_block(address,enc_read_cmd+[motor,0,0])
time.sleep(.08)
try:
b1=bus.read_byte(address)
b2=bus.read_byte(address)
except IOError:
return -1
if b1!=-1 and b2!=-1:
v=b1*256+b2
return v
else:
return -1
#Returns the firmware version
def fw_ver():
write_i2c_block(address,fw_ver_cmd+[0,0,0])
time.sleep(.1)
try:
ver=bus.read_byte(address)
bus.read_byte(address) #Empty the buffer
except IOError:
return -1
return float(ver)/10
#Enable the encoders (enabled by default)
def enable_encoders():
return write_i2c_block(address,en_enc_cmd+[0,0,0])
#Disable the encoders (use this if you don't want to use the encoders)
def disable_encoders():
return write_i2c_block(address,dis_enc_cmd+[0,0,0])
#Enables the servo
def enable_servo():
return write_i2c_block(address,en_servo_cmd+[0,0,0])
#Disable the servo
def disable_servo():
return write_i2c_block(address,dis_servo_cmd+[0,0,0])
#Set speed of the left motor
# arg:
# speed-> 0-255
def set_left_speed(speed):
if speed >255:
speed =255
elif speed <0:
speed =0
return write_i2c_block(address,set_left_speed_cmd+[speed,0,0])
#Set speed of the right motor
# arg:
# speed-> 0-255
def set_right_speed(speed):
if speed >255:
speed =255
elif speed <0:
speed =0
return write_i2c_block(address,set_right_speed_cmd+[speed,0,0])
#Set speed of the both motors
# arg:
# speed-> 0-255
def set_speed(speed):
if speed >255:
speed =255
elif speed <0:
speed =0
set_left_speed(speed)
time.sleep(.1)
set_right_speed(speed)
#Enable communication time-out(stop the motors if no command received in the specified time-out)
# arg:
# timeout-> 0-65535 (timeout in ms)
def enable_com_timeout(timeout):
return write_i2c_block(address,en_com_timeout_cmd+[timeout//256,timeout%256,0])
#Disable communication time-out
def disable_com_timeout():
return write_i2c_block(address,dis_com_timeout_cmd+[0,0,0])
#Read the status register on the GoPiGo
# Gets a byte, b0-enc_status
# b1-timeout_status
# Return: list with l[0]-enc_status
# l[1]-timeout_status
def read_status():
st=bus.read_byte(address)
# Karan, can you double check this one?
st_reg=[st & (1 <<0),(st & (1 <<1))//2]
return st_reg
#Read encoder status
# return: 0 if encoder target is reached
def read_enc_status():
st=read_status()
return st[0]
#Read timeout status
# return: 0 if timeout is reached
def read_timeout_status():
st=read_status()
return st[1]
# Grove - Infrared Receiver- get the commands received from the Grove IR sensor
def ir_read_signal():
try:
write_i2c_block(address,ir_read_cmd+[unused,unused,unused])
time.sleep(.1)
data_back= bus.read_i2c_block_data(address, 1)[0:21]
if data_back[1]!=255:
return data_back
return [-1]*21
except IOError:
return [-1]*21
# Grove - Infrared Receiver- set the pin on which the Grove IR sensor is connected
def ir_recv_pin(pin):
write_i2c_block(address,ir_recv_pin_cmd+[pin,unused,unused])
def cpu_speed():
write_i2c_block(address,cpu_speed_cmd+[0,0,0])
time.sleep(.1)
try:
b1=bus.read_byte(address)
b2=bus.read_byte(address)
except IOError:
return -1
return b1
# Read the DHT sensor connected to the serial port
def dht(sensor_type=0):
try:
import Adafruit_DHT
if sensor_type==0: #blue sensor
sensor = Adafruit_DHT.DHT11
elif sensor_type==1: #white sensor
sensor = Adafruit_DHT.DHT22
pin = 15 #connected to the serial port on the GoPiGo, RX pin
humidity, temperature = Adafruit_DHT.read_retry(sensor, pin,retries=3,delay_seconds=.1)
if humidity is not None and temperature is not None:
return [temperature,humidity]
else:
return [-2.0,-2.0]
except RuntimeError:
return [-3.0,-3.0]
def check_version():
global version
if version == 0:
for i in range(10):
raw=analogRead(7)
if raw>v16_thresh:
version=16
else:
version=14
return version
Related Questions
Navigate
Integrity-first tutoring: explanations and feedback only — we do not complete graded work. Learn more.