def get_error(self):
rgb.calculate_color_temperature(1, 2, 3)
sensor = self.car.line_detector.read_digital()
cnt, error = 0, 0
for i in range(0, 5):
if sensor[i] == 1:
error += (i - 2)
cnt += 1
if cnt != 0:
error /= cnt
error = error**2
return error, cnt
def __init__(self, carName):
try:
# ================================================================
# ULTRASONIC MODULE DRIVER INITIALIZE
# ================================================================
self.distance_detector = Supersonic_Sensor.Supersonic_Sensor(35)
# ================================================================
# TRACKING MODULE DRIVER INITIALIZE
# ================================================================
self.line_detector = Tracking_Sensor.SEN040134_Tracking([16, 18, 22, 40, 32])
# ================================================================
# RGB MODULE DRIVER INITIALIZE
# ================================================================
self.color_getter = RGB_Sensor.TCS34725()
if self.color_getter.get_exception_occur():
print("[ERRNO-101] There is a problem with RGB_Sensor(TCS34725)")
# ================================================================
# DISABLE RGB MODULE INTERRUPTION
# ================================================================
self.color_getter.set_interrupt(False)
# ================================================================
# PCA9685(PWM 16-ch Extension Board) MODULE WAKEUP
# ================================================================
self.carEngine = PWM_Controller.PWM()
self.carEngine.startup()
# ================================================================
# FRONT WHEEL DRIVER SETUP
# ================================================================
self.steering = front_wheels.Front_Wheels(db='config')
self.steering.ready()
# ================================================================
# REAR WHEEL DRIVER SETUP
# ================================================================
self.accelerator = rear_wheels.Rear_Wheels(db='config')
self.accelerator.ready()
# ================================================================
# SET LIMIT OF TURNING DEGREE
# ===============================================================
self.steering.turning_max = 35
# ================================================================
# SET FRONT WHEEL CENTOR ALLIGNMENT
# ================================================================
self.steering.center_alignment()
# ================================================================
# SET APPOINTED OF CAR NAME
# ================================================================
self.car_name = carName
except Exception as e:
print("CONTACT TO Kookmin Univ. Teaching Assistant")
print("Learn more : " + e)
def moduleInitialize(self):
try:
# ================================================================
# ULTRASONIC MODULE DRIVER INITIALIZE
# ================================================================
self.distance_detector = Ultrasonic_Sensor.Ultrasonic_Avoidance(35)
# ================================================================
# TRACKING MODULE DRIVER INITIALIZE
# ================================================================
self.line_detector = Tracking_Sensor.SEN040134_Tracking(
[16, 18, 22, 40, 32])
# ================================================================
# RGB MODULE DRIVER INITIALIZE
# ================================================================
self.color_getter = RGB_Sensor.TCS34725()
# ================================================================
# FRONT WHEEL DRIVER SETUP
# ================================================================
self.front_steering = front_wheels.Front_Wheels(db='config')
self.front_steering.ready()
# ================================================================
# REAR WHEEL DRIVER SETUP
# ================================================================
self.rear_wheels_drive = rear_wheels.Rear_Wheels(db='config')
self.rear_wheels_drive.ready()
# ================================================================
# SET LIMIT OF TURNING DEGREE
# ===============================================================
self.front_steering.turning_max = 35
# ================================================================
# SET FRONT WHEEL CENTOR ALLIGNMENT
# ================================================================
self.front_steering.turn_straight()
# ================================================================
# DISABLE RGB MODULE INTERRUPTION
# ================================================================
self.color_getter.set_interrupt(False)
except:
print("MODULE INITIALIZE ERROR")
print("CONTACT TO Kookmin Univ. Teaching Assistant")
# Simple demo of reading color data with the TCS34725 sensor. # Will read the color from the sensor and print it out along with lux and # color temperature. # Author: Tony DiCola # License: Public Domain import time # Import the TCS34725 module. from TCS34725 import TCS34725_RGB # Create a TCS34725 instance with default integration time (2.4ms) and gain (4x). import smbus tcs = TCS34725_RGB.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=Adafruit_TCS34725.TCS34725_INTEGRATIONTIME_700MS, # gain=Adafruit_TCS34725.TCS34725_GAIN_60X) # 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
def lineFollwer_main():
global turning_angle
off_track_count = 0
a_step = 5
b_step = 10
c_step = 30
d_step = 45
rear_wheels.forwardWithSpeed(forward_speed)
while True:
#SR02-Ultrasonic avoidance
uDistance = UA.get_distance()
if uDistance >= 0 and uDistance <= 2:
print("DISTANCE", uDistance)
rear_wheels.stop()
break
#TCS34725-RGB Module
cr, cg, cb, clr = RM.get_raw_data()
lux = TCS34725_RGB.calculate_lux(cr, cg, cb)
print(" RED : ", cr, " GREEN : ", cg, " BLUE : ", cb, " LUX : ", lux)
lt_status_now = LF.read_digital()
# Angle calculate
if lt_status_now == [0, 0, 1, 0, 0
] or lt_status_now == [0, 1, 1, 1, 0]:
step = 0
elif lt_status_now == [0, 1, 1, 0, 0
] or lt_status_now == [0, 0, 1, 1, 0]:
step = a_step
elif lt_status_now == [0, 1, 0, 0, 0
] or lt_status_now == [0, 0, 0, 1, 0]:
step = b_step
elif lt_status_now == [1, 1, 0, 0, 0
] or lt_status_now == [0, 0, 0, 1, 1]:
step = c_step
elif lt_status_now == [1, 0, 0, 0, 0
] or lt_status_now == [0, 0, 0, 0, 1]:
step = d_step
else:
step = 0
# Direction calculate
if lt_status_now == [0, 0, 1, 0, 0]:
off_track_count = 0
FR.turn(90)
# turn right
# elif lt_status_now in ([0, 1, 1, 0, 0], [0, 1, 0, 0, 0], [1, 1, 0, 0, 0], [1, 0, 0, 0, 0]):
elif sum(lt_status_now[:3]) > sum(lt_status_now[2:]):
off_track_count = 0
turning_angle = int(90 - step)
# turn left
# elif lt_status_now in ([0, 0, 1, 1, 0], [0, 0, 0, 1, 0], [0, 0, 0, 1, 1], [0, 0, 0, 0, 1]):
elif sum(lt_status_now[:3]) < sum(lt_status_now[2:]):
off_track_count = 0
turning_angle = int(90 + step)
elif lt_status_now == [0, 0, 0, 0, 0]:
# rgb sensor check
rear_wheels.forwardWithSpeed(forward_speed)
time.sleep(0.1)
rear_wheels.stop()
if LF.read_digital() == [0, 0, 0, 0, 0]:
#off_track_count += 1
#if off_track_count > max_off_track_count:
##tmp_angle = -(turning_angle - 90) + 90
# tmp_angle = (turning_angle - 90) / abs(90 - turning_angle)
# tmp_angle *= FR.turning_max
# rear_wheels.backwardWithSpeed(backward_speed)
# FR.turn(tmp_angle)
# LF.wait_tile_center()
# rear_wheels.stop()
# FR.turn(turning_angle)
# time.sleep(0.2)
# rear_wheels.forwardWithSpeed(forward_speed)
# time.sleep(0.2)
time.sleep(0.05)
rear_wheels.stop()
store_angle = turning_angle
turning_angle = 120 if turning_angle < 90 else 60
FR.turn(turning_angle)
time.sleep(0.1)
while sum(LF.read_digital()) <= 1:
rear_wheels.backwardWithSpeed(backward_speed)
time.sleep(0.1)
rear_wheels.stop()
turning_angle = store_angle
FR.turn(turning_angle)
time.sleep(0.1)
rear_wheels.forwardWithSpeed(forward_speed)
time.sleep(0.4)
else:
rear_wheels.forwardWithSpeed(forward_speed)
print("color")
else:
off_track_count = 0
FR.turn(turning_angle)
time.sleep(delay)
if __name__ == "__main__":
try:
# =======================================================================
# setup and initilaize the left motor and right motor
# =======================================================================
# ULTRASONIC MODULE DRIVER INITIALIZE
UA = SR02_Ultrasonic.Ultrasonic_Avoidance(35)
# TRACKING MODULE DRIVER INITIALIZE
LF = SEN040134_Tracking.SEN040134_Tracking([16, 18, 22, 40, 32])
# RGB MODULE DRIVER INITIALIZE
RM = TCS34725_RGB.TCS34725()
# FRONT WHEEL DRIVER SETUP
FR = front_wheels.Front_Wheels(db='config')
FR.ready()
# REAR WHEEL DRIVER SETUP
rear_wheels.setup(1)
# IS LINE FOLLOWER FR VAR SETUP
FR.turning_max = 35
# Front wheel Calibration
#setup()
# RGB Module interrupt SETUP
def __init__(self):
self.colour_detector = TCS34725_RGB.TCS34725()
self.red_range = [[100, 500], [0, 100], [0, 100]]
self.green_range = [[0, 100], [100, 500], [0, 100]]
self.blue_range = [[0, 100], [0, 100], [100, 500]]