# 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 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)
time.sleep(2) 					   # Arduino resets, so wait for some time

# 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(0b11111111) +  chr(0b11111111)          # end of transmission
magOff = chr(0b00000000)
magOn = chr(0b00000001)

# a, r, h
if 1 == 1:
    msg = signature + priority0 + moveJ
    ser.write(msg)
    sendBin(120)
    sendBin(100)
    sendBin(10)
    ser.write(eot)
    
    msg = signature + priority0 + pause
    ser.write(msg)
    sendBin(5000)
    ser.write(eot)
    
    msg = signature + priority0 + moveJ
    ser.write(msg)
    sendBin(80)
    sendBin(110)
    sendBin(20)
    ser.write(eot)

if 1 == 0:
    msg = signature + priority0 + moveA
    ser.write(msg)
    sendBin(50)
    sendBin(128)
    sendBin(90)
    msg = magOff + eot
    ser.write(msg)

    msg = signature + priority0 + moveA
    ser.write(msg)
    sendBin(30)
    sendBin(180)
    sendBin(100)
    msg = magOff + eot
    ser.write(msg)

    msg = signature + priority1 + pause
    ser.write(msg)
    sendBin(2000)
    ser.write(eot)

while True:
    print(ser.readline())

ser.close()		# close serial connection

# For debugging
#file = open("testfile.txt", "w")
#file.write("update")
#file.close