

Stop                                '

' ╔════════════════════════════════════════════╗ █   █▀▀█ █▀▀█ █▀▀█
' ║ LOOP                                       ║ █   █  █ █  █ █▄▄█
' ╚════════════════════════════════════════════╝ █▄▄ █▄▄█ █▄▄█ █
Label Loop
	'AudioInput
	ShowMotorFPS
	Encoders
	FiveButtons_A
	FiveButtons_B
	'If SensorsStatus  = "SensorsON"
	'	Sensors
	'EndIf
	
Goto Loop

' ╔════════════════════════════════════════════╗
' ║ ENABLE/DISABLE BUTTONS                     ║
' ╚════════════════════════════════════════════╝
Label EnableAllButtons
	For v1 = 1 To 48
		Button v1 Enabled
	Next
Return

Label DisableAllButtons
	For v1 = 1 To 48
		Button v1 Disabled
	Next
Return

' ╔════════════════════════════════════════════╗
' ║  WAIT COBOT READY                          ║
' ╚════════════════════════════════════════════╝
Label WaitCobotReady
	Variable Numeric WCR1
	For WCR1 = 1 To Infinity
		Wait Seconds 0.05
		If SlotText(0) = "Ready"
			Exit
		EndIf
	Next
Return





' =============================================================
'  SEND COMMAND (default timeout = 0.5 Sec)
' =============================================================
Label SendCommand			'(Destination Slot)(Command String)
	Variable Numeric SC1
	Variable String  SC2
	Variable Numeric SC3 = 0.7 '<-- The timeout in seconds was 0.5
	Variable Numeric SC4
	SlotText SC1 = SC2
	For SC4 = 0.1 To SC3 Step 0.1
		If SlotText(SC1) = ""
			Exit
		EndIf
		Wait Seconds 0.1
	Next
Return

'╔═════════════════╗
'║COMMANDS TO COBOT║
'╚═════════════════╝
' █▀▀▀ █▀▀█ █▄ ▄█ █▄ ▄█   ▀▀█▀▀ █▀▀█   █▀▀▀ █▀▀█ █▀█  █▀▀█ ▀▀█▀▀
' █    █  █ █ ▀ █ █ ▀ █     █   █  █   █    █  █ █▀▀█ █  █   █
' █▄▄▄ █▄▄█ █   █ █   █ ▄   █   █▄▄█   █▄▄▄ █▄▄█ █▄▄█ █▄▄█   █

Label ExecCobot
	SendCommand(Slot_CommandsToCobot)("EnableRepeat")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label StopCobot
	SendCommand(Slot_CommandsToCobot)("StopExecution")
Return

Label ArrowUpCobot
	SendCommand(Slot_CommandsToCobot)("ArrowUp")
Return

Label ArrowDownCobot
	SendCommand(Slot_CommandsToCobot)("ArrowDown")
Return

Label ExecuteCobotFromSelectedLine
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label AddOrCorrectPosition
	SendCommand(Slot_CommandsToCobot)("AddOrCorrectPosition")
Return

Label DeleteSelectedLine
	SendCommand(Slot_CommandsToCobot)("DeleteSelectedLine")
Return

Label AddNewPosition
	SendCommand(Slot_CommandsToCobot)("AddNewPosition")
Return

Label EnableRepeat
	SendCommand(Slot_CommandsToCobot)("EnableRepeat")
Return

Label DisableRepeat
	SendCommand(Slot_CommandsToCobot)("DisableRepeat")
Return


' ╔══════════════════════════════════════════╗ █▀▀▀ █  █ █▀█
' ║  S U B R O U T I N E   A R E A           ║ ▀▀▀█ █  █ █▀▀█
' ╚══════════════════════════════════════════╝ ▄▄▄█ █▄▄█ █▄▄█

'Label SensorsSet
'	If SensorsStatus  = "SensorsOFF"
'		Button SensorsSet Text "Sensors ON"
'		Button SensorsSet Color Flashing_9 Lime Snow
'		SensorsStatus  = "SensorsON"
'	Else
'		Button SensorsSet Text "Sensors OFF"
'		Button SensorsSet Color DarkSalmon
'		SensorsStatus  = "SensorsOFF"
'	EndIf
'Return

Label EnableHomeButtons
	For v1 = 17 To 22
		Button v1 Enabled
	Next
Return

Label DisableHomeButtons
	For v1 = 17 To 22
		Button v1 Disabled
	Next
Return

Label ShowMotorFPS
	Variable String MotorsAreEnabled
	Variable Numeric SMF1 = Slot(Slot_MotorsFPS)
	If SMF1 > 30
		If MotorsAreEnabled <> "TRUE"
			MotorsAreEnabled = "TRUE"
			Button MotorFPS Color GreenYellow
		EndIf
		Button MotorFPS Text "FPS " + Str(SMF1)
	Else
		If MotorsAreEnabled = "TRUE"
			MotorsAreEnabled = "FALSE"
			Button MotorFPS Color Flashing7 Yellow DarkOrange
			Button MotorFPS Text "NO MOTORS"
		EndIf
	EndIf
Return

Label MotorFPS
	If MotorsAreEnabled = "TRUE"
		SendCommand(Slot_CommandsToMotors)("DisableConnection")
	Else
		SendCommand(Slot_CommandsToMotors)("EnableConnection")
	EndIf
Return

' ╔═══════════════════════╗ █▀▀█   █▀█  █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ FIVE PUSH BUTTONS -A- ║ █▀▀█   █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚═══════════════════════╝ █  █   █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
Variable String PressedButtonA

Label FiveButtons_A
	Variable Numeric FBA0
	Variable Numeric FBA1
	Variable Numeric FBA2
	' ------------------------------ Read the value two times
	FBA1 = Slot(Slot_MultiAnaBtnA)
	
	' ------------------------------ If similar values
	If Abs(FBA2 - FBA1) < 10
		' -------------------------- If armed
		If FBA0 = 0
			FBA0 = 1
			Select True
				Case FBA1 > 500
					Print ""
					PressedButtonA = ""
				Case FBA1 > 250
					Print "red "
					PressedButtonA = "red"
					ExecCobot
				Case FBA1 > 120
					Print "white "
					PressedButtonA = "white"
					ExecuteCobotFromSelectedLine
				Case FBA1 > 56
					Print "arrowDOWN "
					PressedButtonA = "ArrowDOWN"
					ArrowDownCobot
				Case FBA1 > 15
					Print "arrowUP "
					PressedButtonA = "ArrowUP"
					ArrowUpCobot
				CaseElse
					Print "yellow "
					PressedButtonA = "yellow"
					AddOrCorrectPosition
			EndSelect
		EndIf
	EndIf
	FBA2 = FBA1
    ' ------------------------------ Rearm buttons
	If FBA1 > 500
		FBA0 = 0
	EndIf
Return

' ╔═══════════════════════╗ █▀█    █▀█  █  █ ▀▀█▀▀ ▀▀█▀▀ █▀▀█ █▄ █ ▄▀▀▀
' ║ FIVE PUSH BUTTONS -B- ║ █▀▀█   █▀▀█ █  █   █     █   █  █ █ ▀█ ▀▀▀▄
' ╚═══════════════════════╝ █▄▄█   █▄▄█ █▄▄█   █     █   █▄▄█ █  █ ▄▄▄▀
Variable String PressedButtonB

Label FiveButtons_B
	Variable Numeric FBB0
	Variable Numeric FBB1
	Variable Numeric FBB2
	' ------------------------------ Read the value two times
	FBB1 = Slot(Slot_MultiAnaBtnB)
	
	' ------------------------------ If similar values
	If Abs(FBB2 - FBB1) < 10
		' -------------------------- If armed
		If FBB0 = 0
			FBB0 = 1
			Select True
				Case FBB1 > 500
					'Print ""
					PressedButtonB = ""
				Case FBB1 > 250
					Print "yellowB "
					PressedButtonB = "yellowB"
					AddNewPosition
				Case FBB1 > 120
					Print "blackB "
					PressedButtonB = "blackB"
					DeleteSelectedLine
				Case FBB1 > 56
					Print "small "
					PressedButtonB = "small"
					'=== spare
				Case FBB1 > 15
					Print "bluBB "
					PressedButtonB = "bluB"
					DisableRepeat
				CaseElse
					Print "greenB "
					PressedButtonB = "greenB"
					'=== spare
			EndSelect
		EndIf
	EndIf
	FBB2 = FBB1
    ' ------------------------------ Rearm buttons
	If FBB1 > 500
		FBB0 = 0
	EndIf
Return






Label SetIOTSlotsToZero
	'Slot(Slot_EncoderSlow) = 0
	Slot(Slot_BtnYellow1)  = 0
	Slot(Slot_BtnBlue1)    = 0
	Slot(Slot_BtnGreen1)   = 0
	Slot(Slot_BtnWhite1)   = 0
	Slot(Slot_BtnRed1)     = 0
Return

' ╔══════════════════╗ █▀▀▀ █▀▀ █▀▀█ █▀▀█ █▀▀ █▀▀▀
' ║  S C O P E S     ║ ▀▀▀█ █   █  █ █▄▄█ █▀▀ ▀▀▀█
' ╚══════════════════╝ ▄▄▄█ █▄▄ █▄▄█ █    █▄▄ ▄▄▄█
Label CloseScopes
	Load CloseApps Theremino_SignalScope_M1.exe
Return

Label OpenScope_1
	Load Theremino_SignalScope_M1.exe
Return

' ╔═══════════════╗ █   █▀▀ █▀▄
' ║ LED COLORS    ║ █   █▀  █  █
' ╚═══════════════╝ █▄▄ █▄▄ █▄▄█
'Label Red_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 0
'Return
'
'Label Gre_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 0
'Return
'
'Label Blu_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 1000
'Return
'
'Label Yel_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 0
'Return
'
'Label Mag_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 1000
'Return
'
'Label Cya_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 1000
'Return
'
'Label Whi_8
'	Slot Slot_Red = 1000
'	Slot Slot_Gre = 1000
'	Slot Slot_Blu = 1000
'Return
'
'Label Off_8
'	Slot Slot_Red = 0
'	Slot Slot_Gre = 0
'	Slot Slot_Blu = 0
'Return
'
'Label RedBlink_8
For v1 = 1 To 10
'		Red_8
	Beep
'	Off_8
	Beep
Next
Return


Variable Numeric WF1
Label WiFiLive
	If WF1 > 30
		WF1 = 0
'		Slot(Slot_Gre) =  1000
	Else
		WF1 = WF1 + 1
'		Slot(Slot_Gre) =  0
	EndIf
Return

' ╔══════════════╗ █▀▀▀ █▀▀ █▀▀█ █   █ █▀▀█
' ║  S E R V O   ║ ▀▀▀█ █▀▀ █▄▄█ █   █ █  █
' ╚══════════════╝ ▄▄▄█ █▄▄ █ ▀▄  ▀▄▀  █▄▄█
Label ClearCommand
	SlotText(Slot_ExternalCommands) = ""
Return

Variable Numeric StartTime
Variable Numeric StartTimePartial
Variable String Time
Label PrintTime
	Time = "Partial time = "
	Time = Time + PadLeft(Format(ElapsedTime - StartTimePartial, "0.0"),4)
	Time = Time + "   Total time = "
	Time = Time + PadLeft(Format(ElapsedTime - StartTime, "0.0"),4)
	'Print Status + "  " + Time + CRLF
Return

' ╔══════════╗ ▄▀▀▀ █▀▀█ ▀▄ ▄▀
' ║  SAY     ║ ▀▀▀█ █▄▄█  ▀▄▀
' ╚══════════╝ ▄▄▄▀ █  █   █
Label Say
	TTS SelectVoice "ENGF"
	TTS Speak "Today is a beautiful day because we spend time together."
	Wait TTSready
	
	TTS SelectVoice "ITA FEM"
	TTS Speak "Oggi è una bella giornata perche passeremo del tempo insieme."
	Wait TTSready
	
	TTS SelectVoice "JPN"
	TTS Speak "私たちは一緒に時間を過ごすので、今日は美しい日です。"
	Wait TTSready
	
	TTS SelectVoice "ZHO"
	TTS Speak "今天是美好的一天，因为我们共度时光。"
	Wait TTSready
Return

' ╔═════════╗ █   █ █▀▀█ █ █▀▀▀ █▀▀▀
' ║ VOICE   ║  █ █  █  █ █ █    █▀▀
' ╚═════════╝   █   █▄▄█ █ █▄▄▄ █▄▄▄
Label Voice
	'Load images\LIST_VOICE_COMMAND.jpg
	ClearVoiceCommand
	If VoiceCommandsStatus = "active"
		VoiceCommandsStatus = ""
		Button Voice Color Blue
	Else
		VoiceCommandsStatus = "active"
		Button Voice Color Flashing7 Orange White
	EndIf
Return

Label AIGPT
	Load Theremino_GPT.exe
Return

Variable String Word
Variable String VoiceCommandsStatus
Label InterpretateVoiceCommands
	If VoiceCommandsStatus = "active"
		If SlotText(Slot_WordRecognized) <> ""
			Word = SlotText(Slot_WordRecognized)
			Print Word + " "
			Interpretate
			ClearVoiceCommand
		EndIf
	EndIf
Return

Variable String ShowingType
Label Interpretate
	Select Word
		Case "greencar.pick"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_pick.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "windshield.side"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_front.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "see.backside"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_back.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
			
		Case "driver.prospect"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_left.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "right.outlook"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_right.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "bottom.view"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_underside.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "release"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence V_car_place.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
			'-------------------------------------------------------------------------
		Case "goto.square"
			If ShowingType = ""
'				Red_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_mechArm_square.txt  ")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
		Case "goto.home"
			If ShowingType = ""
'				Mag_8
				SendCommand(Slot_CommandsToCobot)("LoadSequence Voice_mechArm_home.txt")
				SendCommand(Slot_CommandsToCobot)("SelectLine 1")
				SendCommand(Slot_CommandsToCobot)("StartExecution")
			EndIf
			'-------------------------------------------------------------------------
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + Word + CRLF
			Beep
			ClearCommand
	EndSelect
Return

Label ClearVoiceCommand
	SlotText(Slot_WordRecognized) = ""
Return

'╔═══════════════════╗ █▀▀▀ █▀▀█ █▄ ▄█ █▄ ▄█   █▀▀ █▀▀█   █▀▀▀ █▀▀█ █▀█  █▀▀█ ▀▀█▀▀
'║COMMANDS FROM COBOT║ █    █  █ █ ▀ █ █ ▀ █   █▀  █▄▄█   █    █  █ █▀▀█ █  █   █
'╚═══════════════════╝ █▄▄▄ █▄▄█ █   █ █   █   █   █ ▀▄   █▄▄▄ █▄▄█ █▄▄█ █▄▄█   █
Label Event_ExternalCommands
	' ------------------------------------------------------ Do not print lines containing ''
	'Print " Exec: " + CommandText + CRLF
	' ------------------------------------------------------
	Select GetSeparatedString(CommandText, 0)
		' ---------------------------------------------- ACTIONS
		Case "Say"
			TTS Speak Mid(CommandText, 5)
			ClearCommand
		Case "SayW"
			TTS Speak Mid(CommandText, 5)
			Wait TTSready
			ClearCommand
			
		Case "Beep"
			Beep 880-100
			ClearCommand
		CaseElse
			Controls OpenTextBox
			Print CRLF + " Unrecognized command: " + CommandText + CRLF
			Beep
			ClearCommand
	EndSelect
Return


' ╔══════════════════════════════════╗ ▀▀█▀▀ █▀▀█ █ █▀▀█ █
' ║ T R I A L S                      ║   █   █▄▄▀ █ █▄▄█ █
' ╚══════════════════════════════════╝   █   █ ▀▄ █ █  █ █▄▄
Variable Numeric Loop_Count

Label Demoloop
	For Loop_Count = 1 To 10
		RaiseUp
		WaitCobotReady
		LieDown
		WaitCobotReady
	'	RaiseUp
	'	WaitCobotReady
	'	GoMax
	'	WaitCobotReady
	'	RaiseUp
	'	WaitCobotReady
	'	GoTiltLeft
	'	WaitCobotReady
	'	GoMin
	'	WaitCobotReady
	'	GoSeat
	'	WaitCobotReady
	'	LieDown
	'	WaitCobotReady
	'	GoTiltRight
	'	WaitCobotReady
	'	GoMax
	'	WaitCobotReady
	'	RaiseUp
	'	WaitCobotReady
	'	LieDown
	'	WaitCobotReady
	'	GoMax
	'	WaitCobotReady
	'	GoTiltRight
	'	WaitCobotReady
	'	GoMax
	Next
Return


Label LieDown
	
Return

Label RaiseUp
	
Return

Label GoMax
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Max.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoMin
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Min.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoSeat
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_Seat.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoTiltRight
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_TiltRight.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return

Label GoTiltLeft
	SendCommand(Slot_CommandsToCobot)("LoadSequence Go_TiltLeft.txt")
	SendCommand(Slot_CommandsToCobot)("SelectLine 1")
	SendCommand(Slot_CommandsToCobot)("StartExecution")
Return





' ╔════════════════════════╗ █▀▀▀ █▄ █ █▀▀▀ █▀▀█ █▀▀▄ █▀▀▀ █▀▀█ ▄▀▀▀
' ║ ENCODERS USB           ║ █▀▀  █ ▀█ █    █  █ █  █ █▀▀  █▄▄█ ▀▀▀█
' ╚════════════════════════╝ █▄▄▄ █  █ █▄▄▄ █▄▄█ █▄▄█ █▄▄▄ █ ▀▄ ▄▄▄▀
Variable Numeric EncoderSteps1
Variable Numeric OldEncoderSteps1 = Slot(Slot_Encoder1)
Variable Numeric Difference1
Variable Numeric EncoderSteps2
Variable Numeric OldEncoderSteps2 = Slot(Slot_Encoder2)
Variable Numeric Difference2
Variable Numeric EncoderSteps3
Variable Numeric OldEncoderSteps3 = Slot(Slot_Encoder3)
Variable Numeric Difference3

Label ReadButtons
	Slot(Slot_ButtonFL) = 31   'gre
	Slot(Slot_ButtonFR) = 30   'yel
	Slot(Slot_ButtonRL) = 33   'why
	Slot(Slot_ButtonRR) = 32   'blu
Return

Label RecognizeEncoder
	Slot(0) = Recognize
Return

Label Encoders
	'------------------------------------------------ read encoder1
	EncoderSteps1 = Slot(Slot_Encoder1)
	Difference1 = EncoderSteps1 - OldEncoderSteps1
	OldEncoderSteps1 = EncoderSteps1
	If Difference1 > 32000
		Difference1 = Difference1 - 65536
	EndIf
	If Difference1 < -32000
		Difference1 = Difference1 + 65536
	EndIf
	Difference1 = Difference1 * 0.5
	'If Slot(Slot_EncoderSlow) < 500
	'	Difference1 = Difference1 * 0.1
	'EndIf
	If Slot(Slot_ButtonFL) < 500
		Slot(Slot_Servo_FL1) = Limit(Slot(Slot_Servo_FL1) + Difference1, 110, 899)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR1) = Limit(Slot(Slot_Servo_FR1) + Difference1, 110, 899)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL1) = Limit(Slot(Slot_Servo_RL1) + Difference1, 110, 899)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR1) = Limit(Slot(Slot_Servo_RR1) + Difference1, 110, 899)
	EndIf
    '------------------------------------------------ read encoder2
	EncoderSteps2 = Slot(Slot_Encoder2)
	Difference2 = EncoderSteps2 - OldEncoderSteps2
	OldEncoderSteps2 = EncoderSteps2
	If Difference2 > 32000
		Difference2 = Difference2 - 65536
	EndIf
	If Difference2 < -32000
		Difference2 = Difference2 + 65536
	EndIf
	Difference2 = Difference2 * 0.5
	'If Slot(Slot_EncoderSlow) < 500
	'	Difference2 = Difference2 * 0.1
	'EndIf
	If Slot(Slot_ButtonFL) < 500
		Slot(Slot_Servo_FL2) = Limit(Slot(Slot_Servo_FL2) + Difference2, 110, 999)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR2) = Limit(Slot(Slot_Servo_FR2) + Difference2, 110, 999)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL2) = Limit(Slot(Slot_Servo_RL2) + Difference2, 110, 999)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR2) = Limit(Slot(Slot_Servo_RR2) + Difference2, 110, 999)
	EndIf
    '------------------------------------------------ read encoder3
	EncoderSteps3 = Slot(Slot_Encoder3)
	Difference3 = EncoderSteps3 - OldEncoderSteps3
	OldEncoderSteps3 = EncoderSteps3
	If Difference3 > 32000
		Difference3 = Difference3 - 65536
	EndIf
	If Difference3 < -32000
		Difference3 = Difference3 + 65536
	EndIf
	Difference3 = Difference3 * 0.5
	'If Slot(Slot_EncoderSlow) < 500
	'	Difference3 = Difference3 * 0.1
	'EndIf
	If Slot(Slot_ButtonFL) < 500
		Slot(Slot_Servo_FL3) = Limit(Slot(Slot_Servo_FL3) + Difference3, 110, 999)
	EndIf
	If Slot(Slot_ButtonFR) < 500
		Slot(Slot_Servo_FR3) = Limit(Slot(Slot_Servo_FR3) + Difference3, 110, 999)
	EndIf
	If Slot(Slot_ButtonRL) < 500
		Slot(Slot_Servo_RL3) = Limit(Slot(Slot_Servo_RL3) + Difference3, 110, 999)
	EndIf
	If Slot(Slot_ButtonRR) < 500
		Slot(Slot_Servo_RR3) = Limit(Slot(Slot_Servo_RR3) + Difference3, 110, 999)
	EndIf
Return

Label ResetEncoders
	OldEncoderSteps1 = Slot(Slot_Encoder1)
	OldEncoderSteps2 = Slot(Slot_Encoder2)
	OldEncoderSteps3 = Slot(Slot_Encoder3)
Return


'WATCH Variable
