Linw Follower with colour and intensity detection

Follower_colour_intensity.sb

Sensor.SetMode(1,4)
Sensor.SetMode(4,4)

Red_min[0] = 230
Red_min[1] = 40
Red_min[2] = 10
Red_max[0] = 350
Red_max[1] = 90
Red_max[2] = 70

White_min[0] = 130
White_min[1] = 130
White_min[2] = 130
White_max[0] = 300
White_max[1] = 300
White_max[2] = 300

Green_min[0] = 40
Green_min[1] = 140
Green_min[2] = 30
Green_max[0] = 90
Green_max[1] = 250
Green_max[2] = 100

Black_min[0] = 0
Black_min[1] = 0
Black_min[2] = 0
Black_max[0] = 240
Black_max[1] = 240
Black_max[2] = 240

Silver_min[0] = 300
Silver_min[1] = 300
Silver_min[2] = 300
Silver_max[0] = 1000
Silver_max[1] = 1000
Silver_max[2] = 1000

Sub dectect_colour
  sensor1 = Sensor.ReadRaw(1,3)
  Sensor4 = Sensor.ReadRaw(4,3) 
  
  sensor1_intensity = 0.3333 * sensor1[0] + 0.3333 * sensor1[1] + 0.3333 * sensor1[2]
  sensor4_intensity = 0.3333 * sensor4[0] + 0.3333 * sensor4[1] + 0.3333 * sensor4[2]
  
  sensor1_luminance = 0.2126 * sensor1[0] + 0.7152 * sensor1[1] + 0.0722 * sensor1[2]
  sensor4_luminance = 0.2126 * sensor4[0] + 0.7152 * sensor4[1] + 0.0722 * sensor4[2]
  
  If sensor1[0] > Red_min[0] And sensor1[0] < Red_max[0] And sensor1[1] > Red_min[1] And sensor1[1] < Red_max[1] And sensor1[2] > Red_min[2] And sensor1[2] < Red_max[2] then
    sensor1_colour = "Red"  
  ElseIf sensor1[0] > Green_min[0] And sensor1[0] < Green_max[0] And sensor1[1] > Green_min[1] And sensor1[1] < Green_max[1] And sensor1[2] > Green_min[2] And sensor1[2] < Green_max[2] then
    sensor1_colour = "Green" 
  ElseIf sensor1[0] > Black_min[0] And sensor1[0] < Black_max[0] And sensor1[1] > Black_min[1] And sensor1[1] < Black_max[1] And sensor1[2] > Black_min[2] And sensor1[2] < Black_max[2] then
    sensor1_colour = "Black"
  ElseIf sensor1[0] > White_min[0] And sensor1[0] < White_max[0] And sensor1[1] > White_min[1] And sensor1[1] < White_max[1] And sensor1[2] > White_min[2] And sensor1[2] < White_max[2] then
    sensor1_colour = "White" 
  ElseIf sensor1[0] > Silver_min[0] And sensor1[0] < Silver_max[0] And sensor1[1] > Silver_min[1] And sensor1[1] < Silver_max[1] And sensor1[2] > Silver_min[2] And sensor1[2] < Silver_max[2] then
    sensor1_colour = "Silver"
  Else
    sensor1_colour = "Unknown"
  EndIf  
  
  If sensor4[0] > Red_min[0] And sensor4[0] < Red_max[0] And sensor4[1] > Red_min[1] And sensor4[1] < Red_max[1] And sensor4[2] > Red_min[2] And sensor4[2] < Red_max[2] then
    sensor4_colour = "Red" 
  ElseIf sensor4[0] > Green_min[0] And sensor4[0] < Green_max[0] And sensor4[1] > Green_min[1] And sensor4[1] < Green_max[1] And sensor4[2] > Green_min[2] And sensor4[2] < Green_max[2] then
    sensor4_colour = "Green" 
  ElseIf sensor4[0] > Black_min[0] And sensor4[0] < Black_max[0] And sensor4[1] > Black_min[1] And sensor4[1] < Black_max[1] And sensor4[2] > Black_min[2] And sensor4[2] < Black_max[2] then
    sensor4_colour = "Black"
  ElseIf sensor4[0] > White_min[0] And sensor4[0] < White_max[0] And sensor4[1] > White_min[1] And sensor4[1] < White_max[1] And sensor4[2] > White_min[2] And sensor4[2] < White_max[2] then
    sensor4_colour = "White" 
  ElseIf sensor4[0] > Silver_min[0] And sensor4[0] < Silver_max[0] And sensor4[1] > Silver_min[1] And sensor4[1] < Silver_max[1] And sensor4[2] > Silver_min[2] And sensor4[2] < Silver_max[2] then
    sensor4_colour = "Silver"
  Else
    sensor4_colour = "Unknown"
  EndIf
  
EndSub


While "True"
  dectect_colour()
  If sensor1_colour = "Green" Then
    Motor.MoveSync("AD", 15, 45, 300,"true")    
  ElseIf sensor4_colour = "Green" then
    Motor.MoveSync("AD", 45, 15, 300,"true")   
  Elseif sensor1_colour = "Black" then
    LKp = 0.18
    error = 260- sensor1_intensity
    left_power = 31 - (error * LKp)
    Motor.StartSync("AD",left_power,31)
  Elseif sensor4_colour = "Black" then
    RKp = 0.19
    error = 260- sensor4_intensity
    right_power = 31 - (error * LKp)
    Motor.StartSync("AD",31,right_power)
  Elseif sensor1_colour = "Silver" or sensor4_colour = "Silver" then
    Motor.Stop("AD","true")
    Program.End()
  Else
    Motor.StartSync("AD",31,31)
  EndIf
Endwhile