134 lines
3.9 KiB
Python
134 lines
3.9 KiB
Python
# Creates instructions for the robot to move 360 degrees around, make photos of the ball-plate and write the ball_colors to the database
|
|
|
|
import MySQLdb
|
|
import random
|
|
import serial
|
|
import time
|
|
from color_detection_v4 import DetectColor
|
|
from struct import pack
|
|
|
|
def sendBin( dec ):
|
|
x = pack('i', dec)
|
|
ser.write(x[1])
|
|
ser.write(x[0])
|
|
|
|
#ser = serial.Serial('/dev/ttyACM0', 9600) # Change name if necessary. Find out name with ls /dev/tty* (1x without, 1x with USB connected)
|
|
ser = serial.Serial(port='/dev/ttyACM0', # Change name if necessary. Find out name with ls /dev/tty* (1x without, 1x with USB connected)
|
|
baudrate=9600,
|
|
parity=serial.PARITY_NONE,
|
|
stopbits=serial.STOPBITS_ONE,
|
|
bytesize=serial.EIGHTBITS,
|
|
timeout=1)
|
|
time.sleep(2) # Arduino resets, so wait for some time
|
|
|
|
db = MySQLdb.connect("localhost","root","root","rpr_robot" ) # Open database connection
|
|
cursor = db.cursor() # prepare a cursor object using cursor() method
|
|
|
|
# Get the number of rows in the database for the loop
|
|
cursor.execute('select * from ball_positions')
|
|
cursor.fetchall()
|
|
num_entries = cursor.rowcount - 1
|
|
|
|
# Define the possible colors of the balls
|
|
colors = ["blue", "red", "yellow", "cyan", "green"]
|
|
is_empty = 0;
|
|
|
|
# Create instructions to turn around
|
|
signature = chr(0b01010010) # from raspberry
|
|
priority0 = chr(0b00000000) # add to end of queue
|
|
priority1 = chr(0b00000001)
|
|
moveJ = chr(0b00000001) # binary 1
|
|
moveA = chr(0b00000100) # binary 4
|
|
pause = chr(0b00000111) # binary 7
|
|
dontChange = chr(0b01111111) + chr(0b11111111)
|
|
eot = chr(0b11111110) + chr(0b11111110) # end of transmission
|
|
magOff = chr(0b00000000)
|
|
magOn = chr(0b00000001)
|
|
angle = (0, 45, 90, 135, 180, 225, 270, 315)
|
|
|
|
# Predefine messages
|
|
msgPause = signature + priority0 + pause
|
|
msgTurn = signature + priority0 + moveA
|
|
|
|
# Initialize color detection
|
|
ballColors = DetectColor()
|
|
|
|
# TODO: Check if robot is in save mode or move it to!
|
|
|
|
|
|
# Set startposition
|
|
ser.write(msgTurn)
|
|
sendBin(0) # Keep R1 as it is
|
|
sendBin(110) # Put P to this height
|
|
sendBin(90) # Put R2 to this angle
|
|
ser.write(eot)
|
|
print("TO START")
|
|
|
|
time.sleep(10)
|
|
|
|
for i in angle:
|
|
ser.write(msgTurn)
|
|
sendBin(i) # Turn R1 to actual angle
|
|
ser.write(dontChange) # Keep P as it is
|
|
ser.write(dontChange) # Keep R2 as it is
|
|
ser.write(eot)
|
|
print("TO "+str(i))
|
|
|
|
ser.write(msgPause)
|
|
sendBin(2000) # Send Wait for 2 seconds
|
|
ser.write(eot)
|
|
print("pause at "+str(i))
|
|
|
|
# TODO Wait time or wait for "I'm there" from robot
|
|
time.sleep(3)
|
|
i = i + 90
|
|
if i >= 360:
|
|
i = i - 360
|
|
ballColors.startDetection(i)
|
|
# TODO Wait time
|
|
time.sleep(3)
|
|
|
|
ser.write(msgTurn)
|
|
sendBin(0) # Keep R1 as it is
|
|
sendBin(110) # Put P to this height
|
|
sendBin(90) # Put R2 to this angle
|
|
ser.write(eot)
|
|
print("TO START")
|
|
|
|
# Go through array and write detected positions to database
|
|
for i in range(0, num_entries + 1):
|
|
# Ball-Position 29 must be empty at any time!
|
|
if i == 29 or ballColors.Matrix[i][1] == -1:
|
|
is_empty = 1
|
|
color = "invalid"
|
|
else:
|
|
is_empty = 0
|
|
# Set random color -> Just for offline-programming!
|
|
#color = random.choice(colors);
|
|
color = colors[int(ballColors.Matrix[i][1])];
|
|
|
|
|
|
# Prepare SQL query to UPDATE required records
|
|
sql = "UPDATE ball_positions SET pos_isEmpty = %i, ball_color = '%s', ball_targetPos = 0, ball_prio = -1 WHERE pos_id = %i" % (is_empty, color, i)
|
|
try:
|
|
cursor.execute(sql) # Execute the SQL command
|
|
db.commit() # Commit your changes in the database
|
|
except:
|
|
print "Error: unable to update records"
|
|
db.rollback() # Rollback in case there is any error
|
|
|
|
db.close() # disconnect from server
|
|
|
|
|
|
# Only for debugging
|
|
while True:
|
|
print(ser.readline())
|
|
|
|
file = open("testfile.txt", "w")
|
|
file.write(ballColors.Matrix);
|
|
file.close
|
|
#print ballColors.Matrix
|
|
|
|
|
|
ser.close() # close serial connection
|