/*
/0LOGBOOK
0

*/
program test {
	
	#include <RCX2.h>
	#include <RCX2MLT.h>
	#include <RCX2Sounds.h>
	#include <RCX2Def.h>
	var vPolseMax = 0
	var vPolseMin = 0
	var vHightMax = 0
	var vHightMin = 0
	var vSwingMax = 0
	var vSwingMin = 0
	var vSwingLeft = 0
	var vSwing = 0
	var vPolseSet = 0
	var vHightSet = 0
	var vSwingSet = 0
	var vReady = 0
	var vPolseS2 = 0
	var vHightS2 = 0
	var vSwingS2 = 0
	var vAirOK = 0
	var vHandClosed = 0
	var vMove = 0
	event equal_vMoveEventEqual when vMove = 10
	
	sensor rotation1 on 1
	rotation1 is rotation as angle
	sensor rotation3 on 3
	rotation3 is rotation as angle
	sensor light2 on 2
	light2 is light as percent
	event lDark_light2EventLow when light2 < 41
	
	event equal_messageEventEqual when message = 1
	
	event equal_messageEventEqual0 when message = 9
	
	event equal_messageEventEqual1 when message = 10
	
	event equal_messageEventEqual2 when message = 11
	
	event equal_messageEventEqual3 when message = 3
	
	event equal_vMoveEventEqual0 when vMove = 0
	
	event equal_vAirOKEventEqual when vAirOK = 10
	
	macro GOSUBR36 {
		vMove = 10
		wait until equal_vMoveEventEqual0
	}
	macro MOTORCONTROL {
		vPolseS2 = vPolseSet
		vPolseS2 += 100
		vHightS2 = vHightSet
		vHightS2 += 250
		vReady = 0
		while vReady = 0 {
			vReady = 10
			if rotation1 is (vHightSet / 10)..(vHightS2 / 10){
				float [ A  ] 
			}
			else
			{
				vReady = 0
				if rotation1 > (vHightSet / 10){
					direction [  ] [ A ]
				}
				else
				{
					direction [ A  ] [ ]
				} 
				on [ A  ] 
			} 
			if vSwing = vSwingSet{
				float [ B  ] 
			}
			else
			{
				vReady = 0
				if vSwing > vSwingSet{
					vSwingLeft = 0
					direction [ B  ] [ ]
				}
				else
				{
					vSwingLeft = 10
					direction [  ] [ B ]
				} 
				on [ B  ] 
			} 
			if rotation3 is (vPolseSet / 10)..(vPolseS2 / 10){
				float [ C  ] 
			}
			else
			{
				vReady = 0
				if rotation3 > (vPolseSet / 10){
					direction [  ] [ C ]
				}
				else
				{
					direction [ C  ] [ ]
				} 
				on [ C  ] 
			} 
		}
	}
	
	main {
		ext InterfaceType "kFreestyle"
		rcx_ClearTimers
		bbs_GlobalReset([A B C])
		trigger equal_vMoveEventEqual0
		trigger equal_vAirOKEventEqual
		start variableWatcher0
		start LightWatcher1
		start IRWatcher2
		start IRWatcher3
		start IRWatcher4
		start IRWatcher5
		rcx_Priority( 8)
		trigger equal_vMoveEventEqual
		trigger lDark_light2EventLow
		rcx_ClearIRMessage
		trigger equal_messageEventEqual
		rcx_ClearIRMessage
		trigger equal_messageEventEqual0
		rcx_ClearIRMessage
		trigger equal_messageEventEqual1
		rcx_ClearIRMessage
		trigger equal_messageEventEqual2
		comment (0, 0 ) "During this initialisation the hand must be in TOP position#c vertically placed polse and the arm must point straight forward.\r\rFirst start MASTER then  SLAVE RCX to receive IR message 10 \r\r" 
		vAirOK = 0
		vHandClosed = 0
		vMove = 0
		direction [ A B C  ] [ ]
		vPolseMax = 17700
		vPolseMin = -6900
		vHightMax = 0
		vHightMin = 17400
		vSwingMax = 350
		vSwingMin = -350
		vSwingLeft = 10
		vSwing = 0
		rcx_Calibrate(4,5)
		clear Rotation1
		clear Rotation3
		display rotation1
		repeat {
			wait until equal_vAirOKEventEqual
			comment (0, 0 ) "Move to bottle" 
			vPolseSet = 0
			vHightSet = 12100
			vSwingSet = 0
			send 4
			comment (0, 0 ) "Jump to SubRoutine bypass created to minimize the number of steps in the RCX main routine.\r\rPrevent main routine to continue by waiting for subroutine to become ready (vMove=0)" 
			GOSUBR36
			comment (0, 0 ) "Fetch the bottle (close Hand)\r\rLift the bottle and start pooring" 
			send 7
			wait 50
			vPolseSet = 6500
			vHightSet = 4000
			GOSUBR36
			wait 200
			comment (0, 0 ) "Tilt the bottle a bit further to empty it completely" 
			vSwingSet = 10
			vPolseSet = 7300
			vHightSet = 3500
			GOSUBR36
			wait 200
			vPolseSet = 8000
			GOSUBR36
			comment (0, 0 ) "Stop the dripping from the bottle by turning the bottle neck up again." 
			vSwingSet = 0
			vPolseSet = 6000
			vHightSet = 5000
			GOSUBR36
			comment (0, 0 ) "Put empty bottle on table#c right side from the glass" 
			vPolseSet = -300
			vHightSet = 9000
			vSwingSet = -30
			wait 30
			GOSUBR36
			vHightSet = 11900
			GOSUBR36
			comment (0, 0 ) "Open hand" 
			send 8
			wait 100
			comment (0, 0 ) "Place hand against the Glass" 
			vHightSet = 1750
			GOSUBR36
			comment (0, 0 ) "Licht hier toe" 
			vHightSet = 11900
			vSwingSet = 50
			GOSUBR36
			comment (0, 0 ) "Close hand to fetch the filled glass of beer" 
			if vAirOK = 10{
			}
			else
			{
				sound 5
				wait until equal_vAirOKEventEqual
			} 
			send 7
			wait 50
			comment (0, 0 ) "Present this cool Glass of  Beer to the guest#c at the left" 
			vPolseSet = -1200
			vHightSet = 9000
			GOSUBR36
			vHightSet = 2700
			vSwingSet = 200
			GOSUBR36
			sound 6
			forever {
			}
		} until equal_messageEventEqual3
		vMove = 0
		wait 30
		comment (0, 0 ) "Wait for IR-Message 3.\r\rThis will open the Hand (release Glass)#c return to innitial arm position and stut down both RCX#ses" 
		send 3
		vPolseSet = 0
		vHightSet = vHightMax
		vSwingSet = 0
		GOSUBR36
		sound 1
		stop tasks
	}
	
	
	watcher variableWatcher0 monitor equal_vMoveEventEqual
	{
		rcx_Priority( 2 )
		MOTORCONTROL
		vMove = 0
	} restart on event
	watcher LightWatcher1 monitor lDark_light2EventLow
	{
		rcx_Priority( 4 )
		if vSwingLeft = 10{
			vSwing += 10
		}
		else
		{
			vSwing -= 10
		} 
	} restart on event
	watcher IRWatcher2 monitor equal_messageEventEqual
	{
		rcx_Priority( 6 )
		clear message
		sound 1
		if vMove = 10{
			MOTORCONTROL
			vMove = 0
		}
		else
		{
			vMove = 10
		} 
	} restart on event
	watcher IRWatcher3 monitor equal_messageEventEqual0
	{
		rcx_Priority( 6 )
		clear message
		vHandClosed = 10
	} restart on event
	watcher IRWatcher4 monitor equal_messageEventEqual1
	{
		rcx_Priority( 6 )
		clear message
		vAirOK = 10
	} restart on event
	watcher IRWatcher5 monitor equal_messageEventEqual2
	{
		rcx_Priority( 6 )
		clear message
		vAirOK = 0
	} restart on event
}