import pigpio
from time import sleep
import cv2
from PiRapidStream import PiRapidStream
import serial

print("THIS IS A DEMONSTRATION PROGRAMM OF THE TU DARMSTADT\'S PIPETTING MACHINE.\nUSE IT FOR DEMONSTRATION PURPOSES ONLY.\nTHANKS TO THE WHOLE, AWESOME TEAM\n.")

#Start of the script. Enable Serial Connection

try:
	ser = serial.Serial('/dev/ttyACM0', 115200, timeout = 1)
except serial.serialutil.SerialException:
	print("No valid serial connection on port ttyACM0. Try again!")
	exit()

sleep(1)

print(ser.readall())
ser.write("G28\n")
print(ser.readline())

#Set up the necessary Pins for the Communciation with everything important

freq=1000
dt=200
blue_enable=21
tisch=25
ir_pwm=24
blue_pwm=23
fokus=18

gp=pigpio.pi()

gp.set_mode(blue_enable,pigpio.OUTPUT)
gp.set_mode(tisch,pigpio.OUTPUT)
gp.set_mode(blue_pwm,pigpio.OUTPUT)
gp.set_PWM_range(blue_pwm,4000)
gp.set_mode(ir_pwm,pigpio.OUTPUT)
gp.set_PWM_range(ir_pwm,4000)

gp.set_PWM_frequency(blue_pwm, 4000)
gp.set_PWM_frequency(ir_pwm, 4000)

vs = PiRapidStream(resolution=(320,240),fps=30,form='yuv',exp=40000,gain=1.15)
vs.startcapt()

sleep(10)

ser.write("G1 X 100 Y 100 F 4000")
print(ser.readline());

sleep(4)

print("Now I will start to measure something.")

#Here starts the actual routine. I do not know what we will do here.

gp.write(tisch, 1)
gp.write(blue_enable, 1)

for x in range(0,4000, 50):
	gp.set_PWM_dutycycle(ir_pwm, 4000-x)
	gp.set_PWM_dutycycle(blue_pwm, 4000-x)
	sleep(0.5)
	print(x)

#Close Everything that you have possibly opened
gp.set_mode(blue_enable,pigpio. INPUT)
gp.set_mode(blue_pwm, pigpio.INPUT)
gp.set_mode(ir_pwm, pigpio.INPUT)
gp.set_mode(tisch, pigpio.INPUT)

gp.stop()
ser.close()
vs.stopcapt()

print("We made it this. Thank you for you Attentation!")

