#include "NXTDefs.h"




//sensor set
#define kurosiro1 450
#define kurosiro2 450
#define kurosiro3 380
#define sirogin1 630
#define sirogin2 670
#define sirogin3 630




dseg segment
    theMode byte
    rsVal byte
    thePort byte
    pwr byte
    motorA byte  100
    motorC byte  100
    curveA byte  100
    curveC byte  100
    sensor_1vl sword
    sensor_2vl sword
    sensor_3vl sword
    time long[]
    timer long[]
dseg ends



subroutine FRONT
OnFwd(OUT_A,motorA)
OnFwd(OUT_C,motorC)
return
ends

subroutine BACK
OnRev(OUT_A,motorA)
OnRev(OUT_C,motorC)
return
ends

subroutine RIGHT
OnFwd(OUT_A,curveA)
OnRev(OUT_C,curveC)
return
ends

subroutine LEFT
OnRev(OUT_A,curveA)
OnFwd(OUT_C,curveC)
return
ends



thread main
dseg segment

dseg ends

SetSensorLowspeed(IN_4)

SetSensorType(IN_1,IN_TYPE_REFLECTION)
SetSensorType(IN_2,IN_TYPE_REFLECTION)
SetSensorType(IN_3,IN_TYPE_REFLECTION)

Loop:
ReadSensor(IN_1,sensor_1vl)
sub sensor_1vl, kurosiro1,sensor_1vl
ReadSensor(IN_2,sensor_2vl)
sub sensor_2vl, kurosiro2,sensor_2vl
ReadSensor(IN_3,sensor_3vl)
sub sensor_3vl, kurosiro3,sensor_3vl
NumOut(1,LCD_LINE1,sensor_1vl)
NumOut(1,LCD_LINE2,sensor_2vl)
NumOut(1,LCD_LINE3,sensor_3vl)

call FRONT

brtst GT,Loop,sensor_2vl

brtst GT,curvel,sensor_1vl
brtst GT,curver,sensor_3vl

 
jmp Loop
  
curvel:
call LEFT
jmp Loop

curver:
call RIGHT
jmp Loop
  

endt