import os, sys, io
import M5
from M5 import *
from hat import Servos8Hat
import time
from hardware import I2C
from hardware import Pin
from unit import ColorUnit
blue = None
col565Disp = None
colDisp = None
colCirc = None
message = None
battery = None
baseDisp = None
red = None
green = None
i2c0 = None
i2c1 = None
color_0 = None
hat_servos8_v11_0 = None
colourAct = None
col565 = None
iBlue = None
iGreen = None
colour = None
isRunning = None
baseVal = None
iRed = None
delayTime = None
colNum = None
noBall = None
# Resets the servos to a neutral position
def resetServos():
global colourAct, col565, iBlue, iGreen, colour, isRunning, baseVal, iRed, delayTime, colNum, noBall, blue, col565Disp, colDisp, colCirc, message, battery, baseDisp, red, green, i2c0, i2c1, color_0, hat_servos8_v11_0
hat_servos8_v11_0.power_on()
hat_servos8_v11_0.write_servo_angle(3, 0)
hat_servos8_v11_0.write_servo_angle(1, 82)
hat_servos8_v11_0.write_servo_angle(2, 90)
time.sleep_ms(200)
hat_servos8_v11_0.power_off()
# Using the colour sensor, finds the colour of the current marble.
# Adjust the colour based on default Red colour (baseVal) under current ambient light
# 0 - Red
# 1 - Yellow
# 2 - Green
# 3 - Blue
# 4 - No ball
def findColour():
global colourAct, col565, iBlue, iGreen, colour, isRunning, baseVal, iRed, delayTime, colNum, noBall, blue, col565Disp, colDisp, colCirc, message, battery, baseDisp, red, green, i2c0, i2c1, color_0, hat_servos8_v11_0
if col565 < 150 and iBlue >= 30:
return 3
if iGreen >= 20:
if colour < baseVal + 1:
return 2
if colour >= baseVal + 1:
return 1
else:
if colour > baseVal + 1:
return 0
return 4
# Sets the positions of the servos to direct the marbles to their correct slot
def setDial(colourAct):
global col565, iBlue, iGreen, colour, isRunning, baseVal, iRed, delayTime, colNum, noBall, blue, col565Disp, colDisp, colCirc, message, battery, baseDisp, red, green, i2c0, i2c1, color_0, hat_servos8_v11_0
if colourAct % 2 == 0:
hat_servos8_v11_0.write_servo_angle(1, 54)
else:
hat_servos8_v11_0.write_servo_angle(1, 119)
if colourAct < 2:
hat_servos8_v11_0.write_servo_angle(2, 50)
else:
hat_servos8_v11_0.write_servo_angle(2, 127)
# Release the ball by moving the gate back and forth
def releaseBall():
global colourAct, col565, iBlue, iGreen, colour, isRunning, baseVal, iRed, delayTime, colNum, noBall, blue, col565Disp, colDisp, colCirc, message, battery, baseDisp, red, green, i2c0, i2c1, color_0, hat_servos8_v11_0
hat_servos8_v11_0.write_servo_angle(3, 180)
time.sleep_ms(400)
hat_servos8_v11_0.write_servo_angle(3, 0)
def setup():
global blue, col565Disp, colDisp, colCirc, message, battery, baseDisp, red, green, i2c0, i2c1, color_0, hat_servos8_v11_0, colourAct, col565, iBlue, iGreen, colour, isRunning, baseVal, iRed, delayTime, colNum, noBall
# Set up the display variables, colour sensor and servo hat
M5.begin()
Widgets.setRotation(0)
Widgets.fillScreen(0x000000)
blue = Widgets.Label("Blue", 26, 90, 1.0, 0xffffff, 0x222222, Widgets.FONTS.DejaVu18)
col565Disp = Widgets.Label("Col565", 26, 170, 1.0, 0xffffff, 0x222222, Widgets.FONTS.DejaVu18)
colDisp = Widgets.Label("ColVal", 26, 142, 1.0, 0xffffff, 0x222222, Widgets.FONTS.DejaVu18)
colCirc = Widgets.Circle(51, 210, 15, 0xffffff, 0xffffff)
message = Widgets.Title("Starting Up...", 3, 0xffffff, 0x0000FF, Widgets.FONTS.DejaVu18)
battery = Widgets.Label("0%", 26, 117, 1.0, 0xffffff, 0x222222, Widgets.FONTS.DejaVu18)
baseDisp = Widgets.Label("Base", 78, 199, 1.0, 0xffffff, 0x222222, Widgets.FONTS.DejaVu18)
red = Widgets.Label("Red", 24, 33, 1.0, 0xffffff, 0x222222, Widgets.FONTS.DejaVu18)
green = Widgets.Label("Green", 24, 61, 1.0, 0xffffff, 0x222222, Widgets.FONTS.DejaVu18)
i2c0 = I2C(0, scl=Pin(33), sda=Pin(32), freq=100000)
i2c1 = I2C(1, scl=Pin(26), sda=Pin(0), freq=100000)
color_0 = ColorUnit(i2c0)
time.sleep_ms(100)
color_0.set_integration_time(154)
delayTime = 100
# Servo 1 controls the lower 2 paddles
# Servo 2 controls the upper paddle
# Servo 3 controls the marble release mechanism
hat_servos8_v11_0 = Servos8Hat(i2c1, 0x36)
# Sets the baseVal colour based on current lighting conditions
baseVal = (color_0.get_color()) / 100000 - 1.3
baseDisp.setText(str(baseVal))
# Set initial values and show current battery percentage
colNum = 4
resetServos()
battery.setText(str((str((Power.getBatteryLevel())) + str('%'))))
message.setText('Press B to start')
def loop():
global blue, col565Disp, colDisp, colCirc, message, battery, baseDisp, red, green, i2c0, i2c1, color_0, hat_servos8_v11_0, colourAct, col565, iBlue, iGreen, colour, isRunning, baseVal, iRed, delayTime, colNum, noBall
M5.update()
# Shut down the device
if BtnPWR.wasPressed():
Power.powerOff()
# Button B is used to stop or start the system
if BtnB.isPressed():
if isRunning:
isRunning = False
hat_servos8_v11_0.power_off()
message.setText('Press B to Start')
else:
isRunning = True
hat_servos8_v11_0.power_on()
message.setText('Working')
# Button A will stop the system
if BtnA.wasPressed():
isRunning = False
battery.setText(str((str((Power.getBatteryLevel())) + str('%'))))
resetServos()
message.setText('Press B to Start')
# If the system is running, read and display the current marble's colour values
if isRunning:
iRed = color_0.get_color_r()
iGreen = color_0.get_color_g()
iBlue = color_0.get_color_b()
colCirc.setColor(color=(iRed << 16) | (iGreen << 8) | iBlue, fill_c=(iRed << 16) | (iGreen << 8) | iBlue)
red.setText(str(iRed))
green.setText(str(iGreen))
blue.setText(str(iBlue))
colour = (color_0.get_color()) / 100000
col565 = color_0.get_color565()
colNum = findColour()
colDisp.setText(str(colour))
col565Disp.setText(str(col565))
# If a marble is detected, set the dial (servo) positions and release the marble
if colNum < 4:
noBall = False
setDial(colNum)
time.sleep_ms(delayTime)
releaseBall()
time.sleep_ms(200)
else:
# If no ball is detected, reset the servos and pause the system
if not noBall:
noBall = True
resetServos()
isRunning = False
message.setText('Press B to Start')
if __name__ == '__main__':
try:
setup()
while True:
loop()
except (Exception, KeyboardInterrupt) as e:
try:
from utility import print_error_msg
print_error_msg(e)
except ImportError:
print("please update to latest firmware")
Comments