'Welcome to Small Basic!
'Below is a sample code to print 'Hello, World!' on the screen.
'Press Run(F5) for output.
Sub Main
  Program.Delay(4000)
  Speaker.Play(100, "Sounds/Mirkan bog")
  Motor.Start("A", 60)


  code=0
  x=1200
  blok = 0
  While "True"

    Program.Delay(1000)
    optika()
    If code=1 Then
      pomak_naprijed()
    Else
      Motor.Stop("BC", "True")
    EndIf



    ''dodir()
   'If blok = 1 Then
   'pomak_nazad()
   'okret_desno()
  'Else


    'pomak_naprijed()
  'EndIf
  EndWhile


EndSub

Sub pomak_naprijed
  Motor.StartSync("BC", 50, 50)
  Program.Delay(x)
  Motor.Stop("BC","True")
EndSub

Sub pomak_nazad
  Motor.StartSync("BC",-50,-50)
  Program.Delay(x)
  Motor.Stop("BC","True")
EndSub

Sub pomak_lijevo
  Motor.Start("C",50)
  Program.Delay(x)
  Motor.Stop("B","True")
EndSub

Sub pomak_desno
  Motor.Start("B",50)
  Program.Delay(x)
  Motor.Stop("C","True")
EndSub

Sub okret_lijevo
  Motor.StartSync("BC",-50, 50)
  Program.Delay(x)
  Motor.Stop("BC","True")
EndSub

Sub okret_desno
  Motor.StartSync("BC",50, -50)
  Program.Delay(x)
  Motor.Stop("BC","True")
EndSub

Sub optika

 Sensor.SetMode(4,1)

      LCD.StopUpdate()
      LCD.Clear()
      if Sensor.ReadPercent(4)<=50 Then
        code=0
      Else
        code=1
        EndIf

      Program.Delay(200)
EndSub

Sub dodir

  Sensor.SetMode(3,0)
  LCD.StopUpdate()
  LCD.Clear()
  If Sensor.ReadRawValue(3, 0) <= 25 Then
    blok = 1
    LCD.Text(1,15,55,2,"Stani lave!")
  Else


    blok = 0
    LCD.Text(1,15,55,2,Sensor.ReadRawValue(3, 0)+" mm ")
  EndIf
  LCD.Update()
EndSub


Main()
'optika()