def __init__(self): self.directionFacing = 'north' self.maxSpeed = 100 self.aStar = AStar() self.tcs = Adafruit_TCS34725.TCS34725() self.initialLeftCount = self.aStar.read_encoders()[0] self.initialRightCount = self.aStar.read_encoders()[1]
def __init__(self,
int_time_opt='50',
gain_opt='4',
led_ctrl_pin=26,
led_on=True):
# get the library constants for the gain and integration time from the option strings
self._int_time = get_tcs_int_time(int_time_opt)
self._gain = get_tcs_gain(gain_opt)
print 'Initializing TCS with integration time: %sms, gain: %sX' % (
int_time_opt, gain_opt)
print 'TCS LED control pin: %s, Initial State: %s' % (led_ctrl_pin,
led_on)
# You can also override the I2C device address and/or bus with parameters:
#self._tcs = Adafruit_TCS34725.TCS34725(integration_time, gain, address=0x30, busnum=2)
self._tcs = Adafruit_TCS34725.TCS34725(integration_time=self._int_time,
gain=self._gain)
# Disable interrupts (can enable them by passing true, see the set_interrupt_limits function too).
self._tcs.set_interrupt(False)
# set up the GPIO pin to control the LED on the TCS module and turn the LED on if specified
GPIO.setmode(GPIO.BCM)
self._led_ctrl_pin = led_ctrl_pin
if led_on:
initial_state = GPIO.HIGH
else:
initial_state = GPIO.LOW
GPIO.setup(self._led_ctrl_pin, GPIO.OUT, initial=initial_state)
def __init__(self):
#Get node name and vehicle name
self.node_name = rospy.get_name()
self.veh_name = self.node_name.split("/")[1]
##GPIO setup
#Choose BCM or BOARD numbering schemes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(18, GPIO.OUT)
#turn off LED
GPIO.output(18, GPIO.LOW)
#Set integrationtime and gain
self.tcs = Adafruit_TCS34725.TCS34725( \
integration_time=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_700MS, \
gain=Adafruit_TCS34725.TCS34725_GAIN_1X)
#Set parameter
self.readParamFromFile()
#Set local gain using yam
self.mult = self.setup_parameter("~mult", 1)
self.offset = self.setup_parameter("~offset", 1)
#ROS-Publications
self.msg_light_sensor = LightSensorM()
self.sensor_pub = rospy.Publisher('~sensor_data',
LightSensorM,
queue_size=1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.get_lux()
rate.sleep()
def readTCSAll():
avgr = 0
avgg = 0
avgb = 0
avgc = 0
count = 0
while (count < 130):
tcs = Adafruit_TCS34725.TCS34725()
r, g, b, c = tcs.get_raw_data()
avgr = avgr + r
avgg = avgg + g
avgb = avgb + b
avgc = avgc + c
count = count + 1
tcs.disable()
avgr = avgr / 130
avgg = avgg / 130
avgb = avgb / 130
avgc = avgc / 130
r = float(avgr)
g = float(avgg)
b = float(avgb)
c = float(avgc)
lux = float(Adafruit_TCS34725.calculate_lux(r, g, b))
color_temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
if color_temp is None:
color_temp = 0
color_temp = float(color_temp)
return r, g, b, c, lux, color_temp
def __init__(self, ip):
self.ip = ip
self.scale = nt.getTable('Scale')
self.switch = nt.getTable('Switch')
self.sensor = af.TCS34725()
self.sensor.set_interrupt(False)
def run(self):
tcs = Adafruit_TCS34725.TCS34725()
tcs.set_interrupt(False)
while True:
r, g, b, c = tcs.get_raw_data()
rgb_sensor.red = r
rgb_sensor.blue = b
rgb_sensor.green = g
def Capture(integrationtime=2.4, gain=1, output='all'): # Capture function for RGB sensor
try:
# Dictionary for possible integration time values:
dict = {} # creates a variable dictionary, for information about dictionaries: https://www.programiz.com/python-programming/dictionary
# Add all integration time values to dictionary
dict[2.4] = Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_2_4MS # (2.4ms, default)
dict[24] = Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_24MS
dict[50] = Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_50MS
dict[101] = Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_101MS
dict[154] = Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_154MS
dict[700] = Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_700MS
# Add possible gain values to Dictionary:
dict[1] = Adafruit_TCS34725.TCS34725_GAIN_1X
dict[4] = Adafruit_TCS34725.TCS34725_GAIN_4X
dict[16] = Adafruit_TCS34725.TCS34725_GAIN_16X
dict[60] = Adafruit_TCS34725.TCS34725_GAIN_60X
# Create a TCS34725 instance with default integration time (2.4ms) and gain (4x).
#tcs = Adafruit_TCS34725.TCS34725()
# You can also override the I2C device address and/or bus with parameters:
#tcs = Adafruit_TCS34725.TCS34725(address=0x30, busnum=2)
# Or you can change the integration time and/or gain:
tcs = Adafruit_TCS34725.TCS34725(integration_time=dict[integrationtime],
gain=dict[gain])
# Disable interrupts (can enable them by passing true, see the set_interrupt_limits function too).
tcs.set_interrupt(False)
# Read the R, G, B, C color data.
r, g, b, c = tcs.get_raw_data()
# Calculate color temperature using utility functions. You might also want to
# check out the colormath library for much more complete/accurate color functions.
color_temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
# Calculate lux with another utility function.
lux = Adafruit_TCS34725.calculate_lux(r, g, b)
if color_temp is None:
raise AttributeError('Too dark to determine color temperature!')
tcs.set_interrupt(True)
tcs.disable()
# Determines outputs depending on function parameters:
if output == 'all':
return r, g, b, c, lux, color_temp
elif output == 'rgbc':
return r, g, b, c
elif output == 'lux':
return lux
elif output == 'temp':
return color_temp
except KeyError:
raise ValueError('No such integration time or gain, refer to coliform-project guide in github for possible options.')
def __init__(self):
self.isLost = False
self.nextSearchDirection = 0 # (left: 0, right: 1)
self.veerSpeed = 30
self.maxSpeed = 70
self.aStar = AStar()
self.tcs = Adafruit_TCS34725.TCS34725()
self.initialLeftCount = self.aStar.read_encoders()[0]
self.initialRightCount = self.aStar.read_encoders()[1]
def __init__(self):
#Get node name and vehicle name
self.node_name = rospy.get_name()
self.veh_name = self.node_name.split("/")[1]
##GPIO setup
#Choose BCM or BOARD numbering schemes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(18, GPIO.OUT)
#turn off LED
GPIO.output(18, GPIO.LOW)
#Set integrationtime and gain
self.tcs = Adafruit_TCS34725.TCS34725( \
integration_time=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_700MS, \
gain=Adafruit_TCS34725.TCS34725_GAIN_1X)
# #Set parameter
# self.readParamFromFile()
# #Set local gain using yaml
# self.mult = self.setup_parameter("~mult",1)
# self.offset = self.setup_parameter("~offset", 1)
#ROS-Publications
self.msg_light_sensor = LightSensorM()
self.sensor_pub = rospy.Publisher('~sensor_data',
LightSensorM,
queue_size=1)
rate = rospy.Rate(10)
self.lux1 = []
self.lux2 = []
input("Are you ready for the first light evaluation (ENTER)?")
for count in range(53):
if count > 3:
self.lux1.append(self.get_lux())
rate.sleep()
val1 = int(input("How much was the light luminescence?"))
input("Are you ready for the next light evaluation (ENTER)?")
for count in range(53):
if count > 3:
self.lux2.append(self.get_lux())
rate.sleep()
val2 = int(input("How much was the light luminescence?"))
std1 = np.std(self.lux1)
med1 = np.median(self.lux1)
std2 = np.std(self.lux2)
med2 = np.median(self.lux2)
#make sure that the standard deviation is not to big
self.mult = (val2 - val1) / (med2 - med1)
self.offset = val1 - self.mult * med1
self.set_param()
return
def main():
kruispuntnr = 1
einde = False # True waarde nog te implementeren
GPIO.setmode(GPIO.BOARD)
# SPI-object maken voor afstandssensor
CE = 0 # Eerste kanaal op ADC selecteren
spi = spidev.SpiDev0(0, CE)
spi.max_speed_hz = 1000000
kanaal = 0 # Eerste analoge signaal
# Kleurensensor initialiseren
i2c = busio.I2C(5, 3)
kleurensensor = Adafruit_TCS34725.TCS34725(i2c)
# Server initialiseren
server = Server.Server()
last_error = 0 # Nodig voor de eerst keer volglijn uit te voeren
while not einde:
while True:
# Manuele override
message = server.listen()
if message:
last_error = 0
break
# Volg de lijn 10 keer
for i in range(10):
returnwaarde = lijninterpretatie()
if returnwaarde == "stopstreep":
stopMotor()
kruispunt(kruispuntnr, kleurensensor, kanaal)
kruispuntnr += 1
last_error = 0 #Herinitialisatie van lijnvolgalgoritme
else:
volglijn(returnwaarde)
# Na 10 maal lijn te volgen, check de sensoren
if adc.getAfstand(kanaal) < 15:
stopMotor()
while adc.getAfstand(kanaal) < 20:
pass
#Bericht uitlezen
while message != 'Ga door':
if message == 'rechtdoor':
forward(10)
time.sleep(0.0000001)
stopMotor()
message = server.listen()
elif message == 'links':
pass
message = server.listen()
raise Exception("Fout in de code.")
def read_color():
sensor = Adafruit_TCS34725.TCS34725()
sensor.set_interrupt(False)
red, green, blue, clear = sensor.get_raw_data()
sensor.set_interrupt(True)
sensor.disable()
print('RGBC read by sensor: {0}_{1}_{2}_{3}'.format(
red, green, blue, clear))
return '{0}_{1}_{2}_{3}'.format(red, green, blue, clear)
def __init__(self, mux, muxnum, servo, servonum, lednum):
self.mux = mux
self.muxnum = muxnum
self.servo = servo
self.servonum = servonum
self.mux.channel(self.muxnum)
self.sensor = Adafruit_TCS34725.TCS34725(address=0x29, busnum=1)
self.lednum = lednum
GPIO.setup(lednum, GPIO.OUT)
def read_left_color():
GPIO.output("P8_7", GPIO.HIGH)
sleep(0.1)
tcs = Adafruit_TCS34725.TCS34725()
r, g, b, c = tcs.get_raw_data()
for i in range(0, 5):
r_, g_, b_, c_ = tcs.get_raw_data()
r += r_
g += g_
b += b_
c += c_
GPIO.output("P8_7", GPIO.LOW)
print("Left: ", r / 5, g / 5, b / 5)
return r / 5, g / 5, b / 5, c / 5
class TestSensor(unittest.TestCase):
a_star = AStar()
tcs = Adafruit_TCS34725.TCS34725()
tcs.set_interrupt(False)
turnWheelSpeed=70
def testSensorColor(self):
self.assertTrue(getSensorColor(self.tcs)=='red' or getSensorColor(self.tcs)=='green' or getSensorColor(self.tcs)=='blue')
def testRecalibrateLeft(self):
calibrateLeft(self.a_star, self.tcs, 1000, self.turnWheelSpeed)
self.assertTrue(checkIfGreen(self.tcs))
def testRecalibrateRight(self):
calibrateRight(self.a_star, self.tcs, 1000, self.turnWheelSpeed)
self.assertTrue(checkIfGreen(self.tcs))
def __init__(self, node_name, disable_signals=False):
# initialize the DTROS parent class
super(LightSensorCalibrator, self).__init__(node_name=node_name)
self.veh_name = rospy.get_namespace().strip("/")
# GPIO setup
# Choose BCM or BOARD numbering schemes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(18, GPIO.OUT)
GPIO.output(18, GPIO.LOW)
# Set integrationtime and gain
self.tcs = Adafruit_TCS34725.TCS34725(
integration_time=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_700MS,
gain=Adafruit_TCS34725.TCS34725_GAIN_1X)
self.rate = rospy.Rate(10)
#paths
self.dirname = '/data/config/calibrations/light-sensor/'
self.filename = self.dirname + self.veh_name + ".yaml"
# look if we have already done a callibration
if os.path.isdir(self.dirname):
decission = input(
"calibration is already done. Do you want to make a newcallibration ? [Y/N]"
)
if (decission == "Y") or (
decission
== "y"): # versuche wie or wirklich aussehen muss.
shutil.rmtree(self.dirname)
else:
print(
"the old calibration is still valid and the node will shutdown because we already have a callibration file"
)
#make the folder for saving the callibration
# if decission was not y or Y we will get now an error and process shutdown
#vaiables
self.mult = 1
self.offset = 0
self.lux = 0
if not os.path.isdir(self.dirname):
#callibration
self.callibrator()
#set the parameter
self.set_param()
def get_data(self):
# tcs = Adafruit_TCS34725.TCS34725()
tcs = Adafruit_TCS34725.TCS34725(
address=0x29,
integration_time=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_2_4MS)
tcs.set_interrupt(False)
r, g, b, c = tcs.get_raw_data()
color_temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
lux = Adafruit_TCS34725.calculate_lux(r, g, b)
tcs.set_interrupt(True)
tcs.disable()
return lux
def init(self):
#Hardware configuration:
try:
# Create a TCS34725 instance
self.tcs = TCS34725.TCS34725(address=self.I2CAddr,
busnum=self.busnum,
integration_time=self.IntT,
gain=self.gain)
except Exception as e:
print("TCS34725Config: Error initialising device - exit")
print(str(e))
sys.exit(1)
# Disable interrupts (can enable them by passing true, see the set_interrupt_limits function too).
self.tcs.set_interrupt(False)
def CheckColor():
tcs = Adafruit_TCS34725.TCS34725()
tcs.set_interrupt(False)
r, g, b, c = tcs.get_raw_data()
color_temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
lux = Adafruit_TCS34725.calculate_lux(r, g, b)
if color_temp is None:
color_temp = 0
tcs.set_interrupt(True)
tcs.disable()
return r, g, b, c, lux, color_temp
def __init__(self):
self.STEPS_PER_REV = 3200
self.DISTANCE_PER_REV = 0.2827433388
self.LUX_CUTOFF = 250 #completely random
self.FLAME_LED = 18
self.START_LED = 23
self.ROTATION_KEY = 0.09
self.bno = BNO055.BNO055()
if not self.bno.begin():
raise RuntimeError('Uh oh spaghetti-o\'s my bno055 is on the fritz')
self.tcs = Adafruit_TCS34725.TCS34725()
self.tcs.set_interrupt(False)
GPIO.setmode(GPIO.BCM)
self.setupLeds()
self.SERIAL_PORT = serial.Serial('/dev/ttyUSB0', 9600, timeout=0)
def __init__(self, node_name):
# initialize the DTROS parent class
super(LightSensorNode, self).__init__(node_name=node_name)
# Add the node parameters to the parameters dictionary and load their default values
self.parameters['~default_mult'] = None
self.parameters['~default_offset']=None
self.parameters['~frequency']=None
self.updateParameters()
self.veh_name = rospy.get_namespace().strip("/")
# GPIO setup
# Choose BCM or BOARD numbering schemes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(18, GPIO.OUT)
GPIO.output(18, GPIO.LOW)
# Set integrationtime and gain
self.tcs = Adafruit_TCS34725.TCS34725(
integration_time=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_700MS,
gain=Adafruit_TCS34725.TCS34725_GAIN_1X)
#define file path
self.filepath = '/data/config/calibrations/light-sensor/' + self.veh_name + ".yaml"
#define parameter
self.mult = 0
self.offset = 0
self.default_mult=self.parameters['~default_mult'] #default value = 1
self.default_offset= self.parameters['~default_offset'] #default value = 0
#set parametervalue
self.readParamFromFile()
#define time between evaluations in sec.
self.timesensor = 1/self.parameters['~frequency'] #value = 1: every second one measurment
# ROS-Publications
self.msg_light_sensor = LightSensor()
self.sensor_pub = self.publisher('~sensor_data', LightSensor, queue_size=1)
while not rospy.is_shutdown():
self.get_lux()
rospy.sleep(rospy.Duration.from_sec(self.timesensor))
def __init__(self):
# Temp Sensor.
self._sensor_temp = MCP9808.MCP9808()
try:
self._sensor_temp.begin()
logging.info('Temperature sensor started.')
except Exception as err:
self._sensor_temp = None
msg = 'An error occured while setting up temperature sensor: {}'
logging.error(msg.format(str(err)))
# Color Sensor.
self._sensor_tcs = None
try:
self._sensor_tcs = Adafruit_TCS34725.TCS34725()
self._sensor_tcs.set_interrupt(True)
logging.info('Color sensor started.')
except Exception as err:
msg = 'An error occured while setting up color sensor: {}'
logging.error(msg.format(str(err)))
self._touch_sensor = MPR121.MPR121()
try:
self._touch_sensor.begin()
logging.info('Touch sensor started.')
except Exception as err:
self._touch_sensor = None
msg = 'An error occured while setting up touch sensor: {}'
logging.error(msg.format(str(err)))
self.temperature = 0
self.r, self.g, self.b, self.c = [0, 0, 0, 0]
self.color_temp = 0
self.lux = 0
self.touched_list = []
self._last_touched_data = 0
self._collect_touches = False
self.sensor_read_loop = tornado.ioloop.PeriodicCallback(
self._update, 1000)
self.sensor_read_loop.start()
import time
import os
import smbus
import Adafruit_TCS34725
tcs = Adafruit_TCS34725.TCS34725()
tcs.set_interrupt(False)
colorValues = {
'green': [[r for r in range(25, 51)], [g for g in range(70, 96)],
[b for b in range(45, 81)], [c for c in range(175, 201)],
[l for l in range(58, 90)]],
'brown': [[r for r in range(10, 36)], [g for g in range(10, 36)],
[b for b in range(10, 36)], [c for c in range(55, 81)],
[l for l in range(0, 26)]],
'black': [[r for r in range(20, 46)], [g for g in range(64, 90)],
[b for b in range(85, 111)], [c for c in range(190, 216)],
[l for l in range(25, 51)]],
'blue': [[r for r in range(7, 33)], [g for g in range(5, 31)],
[b for b in range(14, 40)], [c for c in range(5, 31)],
[l for l in range(0, 26)]],
'orange': [[r for r in range(80, 116)], [g for g in range(79, 115)],
[b for b in range(57, 83)], [c for c in range(175, 201)],
[l for l in range(25, 51)]],
'yellow': [[r for r in range(110, 136)], [g for g in range(115, 141)],
[b for b in range(45, 71)], [c for c in range(305, 331)],
[l for l in range(105, 131)]],
'darkgreen': [[r for r in range(-10, 11)], [g for g in range(-9, 10)],
[b for b in range(-10, 11)], [c for c in range(0, 21)],
[l for l in range(-9, 10)]],
'magenta': [[r for r in range(41, 62)], [g for g in range(25, 46)],
"""
Created on Fri Mar 19 11:34:49
2021
@author: otto.meerschman
"""
import time
import Adafruit_TCS34725
import RPi.GPIO as GPIO
import numpy as np
GPIO.setmode(GPIO.BOARD)
sensor = Adafruit_TCS34725.TCS34725()
print(sensor.get_gain())
sensor.set_gain(0x02)
print(sensor.get_gain())
print(sensor.get_integration_time())
sensor.set_integration_time(0xF6)
print(sensor.get_integration_time())
def detectiekleuren(sensor):
"""Leest de kleursensor uit en geeft een string terug naarmate de kleur groen of rood is.
Parameters
----------
sensor: TCS34725-object
de sensor die de data inleest
#tcs = Adafruit_TCS34725.TCS34725(address=0x30, busnum=2)
#tcs = Adafruit_TCS34725.TCS34725(
# integration_time=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_154MS,
# gain=Adafruit_TCS34725.TCS34725_GAIN_1X)
#mplex = multiplex()
#mplex.set_channel(0)
# Or you can change the integration time and/or gain:
tcs = Adafruit_TCS34725.TCS34725(
integration_time=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_154MS,
gain=Adafruit_TCS34725.TCS34725_GAIN_4X)
# Possible integration time values:
# - TCS34725_INTEGRATIONTIME_2_4MS (2.4ms, default)
# - TCS34725_INTEGRATIONTIME_24MS
# - TCS34725_INTEGRATIONTIME_50MS
# - TCS34725_INTEGRATIONTIME_101MS
# - TCS34725_INTEGRATIONTIME_154MS
# - TCS34725_INTEGRATIONTIME_700MS
# Possible gain values:
# - TCS34725_GAIN_1X
# - TCS34725_GAIN_4X
# - TCS34725_GAIN_16X
# - TCS34725_GAIN_60X
def main():
# override is en blijft False totdat er een bericht 'start' binnenkomt uit de manuele override
override = False
kruispuntnr = 1
einde = False # True waarde nog te implementeren
GPIO.setmode(GPIO.BOARD)
# SPI-object maken voor afstandssensor
CE = 0 # Eerste kanaal op ADC selecteren
spi = spidev.SpiDev(0, CE)
spi.max_speed_hz = 1000000
kanaal = 0 # Eerste analoge signaal
# Kleurensensor initialiseren
kleurensensor = Adafruit_TCS34725.TCS34725()
# Server initialiseren
server = Server()
last_error = 0 # Nodig voor de eerst keer volglijn uit te voeren
while not einde:
bericht = server.listen(timeout=0.250)
print(bericht)
mesg = str(bericht)
if mesg.find('start') >= 0:
kruispunt_reserve = kruispuntnr
override = True
# Manuele override
while override:
msg = str(server.listen(timeout=0.2))
print(msg)
if msg.find('vooruit') >= 0:
forward()
server.send('Vooruit.')
elif msg.find('achteruit') >= 0:
backwards()
server.send('Achteruit.')
elif msg.find('links') >= 0:
turnLeft()
server.send('Links.')
elif msg.find('rechts') >= 0:
turnRight()
server.send('Rechts.')
elif msg.find('stop') >= 0:
kruispunt_manueel = 0
print(msg[-3])
print(msg[-2])
try:
cijfer = int(msg[-3])
kruispunt_manueel += cijfer * 10
except:
pass
try:
cijfer = int(msg[-2])
kruispunt_manueel += cijfer
except:
pass
if kruispunt_manueel != 0:
kruispuntnr = kruispunt_manueel
else:
kruispuntnr = kruispunt_reserve
finaalBericht = str(kruispuntnr)
server.send('Kruispunt = ' + finaalBericht)
override = False
else:
server.send(str(kruispuntnr))
stopMotor()
# Volg de lijn 20 keer
for i in range(9):
#print("lijnvolg")
returnwaarde = lijninterpretatie()
if returnwaarde == "stopstreep":
print("kruispunt")
stopMotor()
time.sleep(0.5)
print(kruispuntnr)
kruispunt(kruispuntnr)
kruispuntnr += 1
server.send(str(kruispuntnr))
if kruispuntnr >= 26:
einde = True
break
last_error = 0 # Herinitialisatie van lijnvolgalgoritme
else:
volglijn(returnwaarde)
# Na 20 maal lijn te volgen, check de sensoren
if adc.getAfstand(kanaal) < 15:
print("afstandssensor")
stopMotor()
while adc.getAfstand(kanaal) < 20:
print('nog steeds')
pass
raise Exception("Fout in de code.")
def __init__(self):
self.sensor_temp = MCP9808.MCP9808() # Start Temp Sensor
self.sensor_temp.begin() # Initialize
# Color Sensor instance with default integration time (2.4ms) and gain (4x).
self.sensor_tcs = Adafruit_TCS34725.TCS34725()
def iothub_client_run():
try:
client = iothub_client_init()
if client.protocol == IoTHubTransportProvider.MQTT:
print ( "IoTHubClient is reporting state" )
reported_state = "{\"newState\":\"standBy\"}"
client.send_reported_state(reported_state, len(reported_state), send_reported_state_callback, SEND_REPORTED_STATE_CONTEXT)
while True:
print ( "IoTHubClient sending telemetry data" )
(airtemp,airpressure,airhumidity) = readBME280All(addr=DEVICE)
avgr = 0
avgg = 0
avgb = 0
avgc = 0
count = 0
while (count < 130):
tcs = Adafruit_TCS34725.TCS34725()
r, g, b, c = tcs.get_raw_data()
avgr = avgr + r
avgg = avgg + g
avgb = avgb + b
avgc = avgc + c
count = count + 1
tcs.disable()
avgr = avgr/130
avgg = avgg/130
avgb = avgb/130
avgc = avgc/130
print ( avgr )
r = float(avgr)
g = float(avgg)
b = float(avgb)
c = float(avgc)
lux = float(Adafruit_TCS34725.calculate_lux(r, g, b))
color_temp = Adafruit_TCS34725.calculate_color_temperature(r, g, b)
if color_temp is None:
color_temp = 0
color_temp = float(color_temp)
msg_txt_formatted = MSG_TXT % (airtemp, airpressure, airhumidity, r, g, b, lux, color_temp)
# messages can be encoded as string or bytearray
#if (message_counter & 1) == 1:
# message = IoTHubMessage(bytearray(msg_txt_formatted, 'utf8'))
#else:
message = IoTHubMessage(msg_txt_formatted)
# optional: assign properties
# prop_map = message.properties()
# prop_map.add("temperatureAlert", 'true' if temperature > 28 else 'false')
client.send_event_async(message, send_confirmation_callback, 1)
print ( "IoTHubClient.send_event_async accepted message 1 for transmission to IoT Hub." )
print ( msg_txt_formatted )
i=1
while i<=60:
status = client.get_send_status()
print ( "Send status: %s" % status )
i = i + 1
time.sleep (10)
except IoTHubError as iothub_error:
print ( "Unexpected error %s from IoTHub" % iothub_error )
return
except KeyboardInterrupt:
print ( "IoTHubClient sample stopped" )
print_last_message_time(client)
def checkIntersect(TR, alphabot, obstacle = False):
black_threshold = 500 #to determine if sensor is over black line
#adjust based on tests -- sensors values range from 0 - 1000
white_threshold = 200 #to determine if sensor is over white area
#adjust based on tests -- sensors values range from 0 - 1000
right_flag1 = False
right_flag2 = False
right_flag3 = False#for checking when right turn complete
left_flag1 = False
left_flag2 = False
left_flag3 = False #for checking when left turn complete
backward_flag = False # for checking when backward complete
maximum = 26
sleep_time = 0.5
turn_time = 0.025
adjust_time = 0.8
global random_path
sensor_values = TR.readCalibrated()
#if all sensors over black line, then agent at 4-way intersection ot T-intersection
if ((sensor_values[0] >= black_threshold) and (sensor_values[1] >= black_threshold) and
(sensor_values[2] >= black_threshold) and (sensor_values[3] >= black_threshold) and
(sensor_values[4] >= black_threshold)):
print("At 4-way or T-intersection!")
global index
print(index)
alphabot.stop()
time.sleep(sleep_time)
if (index < len(path)):
global dir_num
dir_num = path [index]
index = index + 1
# update location
global dir
global x1, y1
global x2, y2
x1 = x2
y1 = y2
print('at', x1, y1, dir)
if dir == "N":
if dir_num == 1:
x2 = x1 + 1
dir = "E"
elif dir_num == 2:
x2 = x1 - 1
dir = "W"
elif dir_num == 3:
y2 = y1 - 1
elif dir == "E":
if dir_num == 1:
y2 = y1 + 1
dir = "S"
elif dir_num == 2:
y2 = y1 - 1
dir = "N"
elif dir_num == 3:
x2 = x1 + 1
elif dir == "W":
if dir_num == 1:
y2 = y1 - 1
dir = "N"
elif dir_num == 2:
y2 = y1 + 1
dir = "S"
elif dir_num == 3:
x2 = x1 - 1
elif dir == "S":
if dir_num == 1:
x2 = x1 - 1
dir = "W"
elif dir_num == 2:
x2 = x1 + 1
dir = "E"
elif dir_num == 3:
y2 = y1 + 1
if (x2 < 0 or y2 < 0 or x2 > 2 or y2 > 2):
if (dir == "E"):
dir = "W"
x2 = x2 - 1
elif (dir == "W"):
dir = "E"
x2= x2 + 1
elif (dir == "S"):
dir = "N"
y2 = y2 - 1
elif (dir == "N"):
dir = "S"
y2 = y2 + 1
print('going to', x2, y2, dir)
if dir_num == 1: #right turn
print("right turn!")
conn.send(b'right turn')
alphabot.setPWMB(maximum)
alphabot.right()
t1 = time.time()
print(t1)
while True:
#check sensors to determine when turn complete
#agent sometimes overshoots intersection when stopping
#first check if right most sensor sees black
#then if right most sensor sees white after, the turn is complete
sensor_values = TR.readCalibrated()
if sensor_values[0] < white_threshold:
right_flag1 = True
if sensor_values[0] >= black_threshold:
right_flag2 = True
if sensor_values[4] >= black_threshold:
right_flag3 = True
if sensor_values[0] < white_threshold and right_flag1 and right_flag2 and right_flag3 and sensor_values[4] < white_threshold:
break
time.sleep(turn_time)
t2 = time.time()
if ((t2 -t1)>= adjust_time):
print(t2)
print("turn done!")
else:
time.sleep(adjust_time-(t2-t1))
print(adjust_time-(t2-t1))
print("turn done!")
# alphabot.stop()
# time.sleep(sleep_time)
return True
elif dir_num == 2: #left turn
print("left turn!")
conn.send(b'left turn')
alphabot.setPWMA(maximum)
alphabot.left()
t1 = time.time()
print(t1)
while True:
#check sensors to determine when turn complete
#agent sometimes overshoots intersection when stopping
#first check if left most sensor sees black
#then if left most sensor sees white after, the turn is complete
sensor_values = TR.readCalibrated()
if sensor_values[4] < white_threshold:
left_flag1 = True
if sensor_values[4] >= black_threshold:
left_flag2 = True
if sensor_values[0] >= black_threshold:
left_flag3 = True
if sensor_values[4] < white_threshold and left_flag1 and left_flag2 and left_flag3 and sensor_values[0] < white_threshold:
break
time.sleep(turn_time)
t2 = time.time()
if ((t2 -t1)>= adjust_time):
print(t2)
print("turn done!")
else:
time.sleep(adjust_time-(t2-t1))
print(adjust_time-(t2-t1))
print("turn done!")
# alphabot.stop()
# time.sleep(sleep_time)
return True
elif dir_num == 3: #straight
print("straight!")
conn.send(b'straight')
alphabot.setPWMA(maximum)
alphabot.setPWMB(maximum)
alphabot.forward()
while True:
#check sensors to determine when intersection passed
#if left and right most sensors see white, then intersection passed
sensor_values = TR.readCalibrated()
if ((sensor_values[0] < white_threshold) and
(sensor_values[4] < white_threshold)):
break
time.sleep(turn_time)
print("done!")
# alphabot.stop()
# time.sleep(sleep_time)
return True
elif dir_num == 0: #stop
alphabot.stop()
return True
elif dir_num == 4: #straight and backward
print("Arrived!")
conn.send(b'Arrived!')
alphabot.setPWMA(maximum)
alphabot.setPWMB(maximum)
alphabot.forward()
time.sleep(0.5)
# while True:
# sensor_values = TR.readCalibrated()
#
# if ((sensor_values[0] < white_threshold) and (sensor_values[1] < white_threshold) and
# (sensor_values[2] < white_threshold) and (sensor_values[3] < white_threshold) and
# (sensor_values[4] < white_threshold)):
# break
# time.sleep(turn_time)
alphabot.stop()
time.sleep(5)
alphabot.backward()
while True:
sensor_values = TR.readCalibrated()
if ((sensor_values[0] >= black_threshold) and (sensor_values[1] >= black_threshold) and
(sensor_values[2] >= black_threshold) and (sensor_values[3] >= black_threshold) and
(sensor_values[4] >= black_threshold)):
backward_flag = True
if ((sensor_values[0] < white_threshold) and
(sensor_values[4] < white_threshold)) and backward_flag:
break
time.sleep(turn_time)
print("done!")
alphabot.stop()
time.sleep(sleep_time)
return True
else:
print("random number error!")
alphabot.backward()
time.sleep(sleep_time)
alphabot.stop()
time.sleep(sleep_time)
return True
else:
# alphabot.stop()
# time.sleep(5)
if random_path :
#task finished, randomly pick direction at intersection
#if obstacle present, limit choices
if obstacle:
rand_num = random.randint(1,2)
conn.send(b'Obstacle!')
else:
rand_num = random.randint(1,3)
#if at 4 corners, limit choices
if (x2 == 0 and y2 == 2):
if dir == "W":
rand_num = 1
if dir == "S":
rand_num = 2
elif (x2 ==0 and y2 == 0):
if dir == "N":
rand_num = 1
if dir == "W":
rand_num = 2
elif (x2 == 2 and y2 == 0):
if dir == "N":
rand_num = 2
if dir == "E":
rand_num = 1
elif (x2 == 2 and y2 == 2):
if dir == "S":
rand_num = 1
if dir == "E":
rand_num = 2
path.append(rand_num)
# run the rand_num
dir_num = path [index]
index = index + 1
x1 = x2
y1 = y2
print('at', x1, y1, dir)
if dir == "N":
if dir_num == 1:
x2 = x1 + 1
dir = "E"
elif dir_num == 2:
x2 = x1 - 1
dir = "W"
elif dir_num == 3:
y2 = y1 - 1
elif dir == "E":
if dir_num == 1:
y2 = y1 + 1
dir = "S"
elif dir_num == 2:
y2 = y1 - 1
dir = "N"
elif dir_num == 3:
x2 = x1 + 1
elif dir == "W":
if dir_num == 1:
y2 = y1 - 1
dir = "N"
elif dir_num == 2:
y2 = y1 + 1
dir = "S"
elif dir_num == 3:
x2 = x1 - 1
elif dir == "S":
if dir_num == 1:
x2 = x1 - 1
dir = "W"
elif dir_num == 2:
x2 = x1 + 1
dir = "E"
elif dir_num == 3:
y2 = y1 + 1
if (x2 < 0 or y2 < 0 or x2 > 2 or y2 > 2):
if (dir == "E"):
dir = "W"
x2 = x2 - 1
elif (dir == "W"):
dir = "E"
x2= x2 + 1
elif (dir == "S"):
dir = "N"
y2 = y2 - 1
elif (dir == "N"):
dir = "S"
y2 = y2 + 1
print('going to', x2, y2, dir)
if dir_num == 1: #right turn
print("right turn!")
conn.send(b'right turn')
alphabot.setPWMB(maximum)
alphabot.right()
t1 = time.time()
print(t1)
while True:
#check sensors to determine when turn complete
#agent sometimes overshoots intersection when stopping
#first check if right most sensor sees black
#then if right most sensor sees white after, the turn is complete
sensor_values = TR.readCalibrated()
if sensor_values[0] < white_threshold:
right_flag1 = True
if sensor_values[0] >= black_threshold:
right_flag2 = True
if sensor_values[4] >= black_threshold:
right_flag3 = True
if sensor_values[0] < white_threshold and right_flag1 and right_flag2 and right_flag3 and sensor_values[4] < white_threshold:
break
time.sleep(turn_time)
t2 = time.time()
if ((t2 -t1)>= adjust_time):
print(t2)
print("turn done!")
else:
time.sleep(adjust_time-(t2-t1))
print(adjust_time-(t2-t1))
print("turn done!")
# alphabot.stop()
# time.sleep(sleep_time)
return True
elif dir_num == 2: #left turn
print("left turn!")
conn.send(b'left turn')
alphabot.setPWMA(maximum)
alphabot.left()
t1 = time.time()
print(t1)
while True:
#check sensors to determine when turn complete
#agent sometimes overshoots intersection when stopping
#first check if left most sensor sees black
#then if left most sensor sees white after, the turn is complete
sensor_values = TR.readCalibrated()
if sensor_values[4] < white_threshold:
left_flag1 = True
if sensor_values[4] >= black_threshold:
left_flag2 = True
if sensor_values[0] >= black_threshold:
left_flag3 = True
if sensor_values[4] < white_threshold and left_flag1 and left_flag2 and left_flag3 and sensor_values[0] < white_threshold:
break
time.sleep(turn_time)
t2 = time.time()
if ((t2 -t1)>= adjust_time):
print(t2)
print("turn done!")
else:
time.sleep(adjust_time-(t2-t1))
print(adjust_time-(t2-t1))
print("turn done!")
# alphabot.stop()
# time.sleep(sleep_time)
return True
elif dir_num == 3: #straight
print("straight!")
conn.send(b'straight')
alphabot.setPWMA(maximum)
alphabot.setPWMB(maximum)
alphabot.forward()
while True:
#check sensors to determine when intersection passed
#if left and right most sensors see white, then intersection passed
sensor_values = TR.readCalibrated()
if ((sensor_values[0] < white_threshold) and
(sensor_values[4] < white_threshold)):
break
time.sleep(turn_time)
print("done!")
# alphabot.stop()
# time.sleep(sleep_time)
return True
else:
alphabot.stop()
time.sleep(5)
tcs = Adafruit_TCS34725.TCS34725()
tcs.set_interrupt(False)
r, g, b, c = tcs.get_raw_data()
# Print out the values.
print('Color: red={0} green={1} blue={2} clear={3}'.format(r, g, b, c))
if ((r < 220) and (g > 390) and (b > 400)):
# Enable interrupts and put the chip back to low power sleep/disabled.
tcs.set_interrupt(True)
tcs.disable()
print('blue')
path.extend ([3,3,1,4,1,3,1,2])
conn.send(b'Going to Squarenut.')
elif ((r > 300) and (g < 220) and (b < 250)):
tcs.set_interrupt(True)
tcs.disable()
path.extend([2,1,3,4,1,1,3,3])
print('red')
conn.send(b'Going to Donut.')
# elif ((r > 500) and (g > 400) and (b > 180)):
# tcs.set_interrupt(True)
# tcs.disable()
# path.extend([2,4,1,1,1,3])
# print('yellow')
# conn.send(b'Going to Circle.')
else:
conn.send(b'Following random path.')
random_path = True
return True
#time.sleep(1)
#
# path.append(rand_num)
else:
return False
def __init__(self):
try:
self.sensor_tcs = Adafruit_TCS34725.TCS34725()
except:
print("faild to initialize color sensor")
#This is the Python program to detect color during the FRC 2018 competition using the Adafruit TCS 34725 color sensor and the raspberry pi 3
#import necessary dependencies
from networktables import NetworkTables as nt
import time
import smbus
import Adafruit_TCS34725 as af
import os
ip = "10.46.82.2"
nt.initialize(server=ip)
#Create an instance of the Color Sensor
sensor = af.TCS34725()
sensor.set_interrupt(False)
while (1):
r, g, b, c = sensor.get_raw_data()
time.sleep(0.5)
print("rgb=(" + str(r) + ", " + str(g) + ", " + str(b) + ")")
#print(r)
#print(g)
#print(b)
cls()
colorTemp = af.calculate_color_temperature(r, g, b)
lux = af.calculate_lux(r, g, b)
print(str(colorTemp) + "k")
print(str(lux) + " lux")