program SoccerProgram3
'*******************************************************************************
dim ch0,ch1,ch2,ch3,ch4,ch5,ch6,ch7 as word
dim direction,value as word
dim rightbutt,leftbutt as word
dim MatRead1,MatRead2,MatDiff as word
dim LSharp,RSharp as word
dim Butts as Integer
dim WALL_THRESHOLD, IR_MAX_THRESHOLD, MAT_DIFF, SENSOR_OFFSET as word
'dim seed as byte
dim MAT_THRESHOLD,IR_MIN_THRESHOLD as word
'*******************************************************************************
sub procedure Forward
   PORTB = %00101011
end sub
'*******************************************************************************
sub procedure Reverse
   PORTB = %00011101
end sub
'*******************************************************************************
sub procedure T_Right
   PORTB = %00011011
end sub
'*******************************************************************************
sub procedure T_Left
   PORTB = %00101101
end sub
'*******************************************************************************
sub procedure Halt
   delay_ms(25)
   PORTB = 0
end sub
'*******************************************************************************
sub procedure Arc_Right
   PORTB = %00000011
end sub
'*******************************************************************************
sub procedure Arc_Left
   PORTB = %00101000
end sub
'*******************************************************************************
sub procedure Walls
   RSharp = adc_read(1)
   LSharp = adc_read(2)
   if RSharp > WALL_THRESHOLD then
      T_left
      delay_ms(250)
      Reverse
      delay_ms(250)
   end if
   if LSharp > WALL_THRESHOLD then
      T_right
      delay_ms(250)
      Reverse
      delay_ms(250)
   end if
end sub
'*******************************************************************************
sub procedure RoomScan
   RSharp = adc_read(1)
   LSharp = adc_read(2)
   if RSharp > 300 then
      Reverse
      delay_ms(250)
      T_left
      delay_ms(250)
   end if
   if LSharp > 300 then
      Reverse
      delay_ms(250)
      T_right
      delay_ms(500)
   end if
end sub
'*******************************************************************************
sub procedure GetValues

   PORTC = %00000000
   ch0 = adc_read(0)

   PORTC = %00000001
   ch1 = adc_read(0)

   PORTC = %00000010
   ch2 = adc_read(0)

   PORTC = %00000011
   ch3 = adc_read(0)

   PORTC = %00000100
   ch4 = adc_read(0)

   PORTC = %00000101
   ch5 = adc_read(0)

   'PORTC = %00000111
   'ch7 = adc_read(0)

end sub
'*******************************************************************************
sub procedure CheckButts
   leftbutt = ch7
   rightbutt = ch6
   rightbutt = rightbutt + SENSOR_OFFSET
   Butts = leftbutt - rightbutt
   If Butts > MAT_DIFF Then
      if rightbutt > leftbutt then
         T_right
         delay_ms(50)
      end if
      if leftbutt > rightbutt then
         T_left
         delay_ms(50)
      end if
   End If
   If Butts < -MAT_DIFF Then
      if rightbutt > leftbutt then
         T_right
         delay_ms(25)
      end if
      if leftbutt > rightbutt then
         T_left
         delay_ms(25)
      end if
   End If
end sub
'*******************************************************************************
sub procedure ToBlack
   PORTC = %00000110
   MatRead1 = adc_read(0)
   Forward
   Delay_ms(250)
   PORTC = %00000110
   MatRead2 = adc_read(0)
   If MatRead1 > MatRead2 Then
      MatDiff = MatRead1 - MatRead2
         If MatDiff > MAT_THRESHOLD Then
            Arc_Right
            Delay_ms(500)
         End If
   End If
end sub
'*******************************************************************************
sub procedure ToWhite
   PORTC = %00000110
   MatRead1 = adc_read(0)
   Forward
   Delay_ms(250)
   PORTC = %00000110
   MatRead2 = adc_read(0)
   If MatRead1 < MatRead2 Then
      MatDiff = MatRead2 - MatRead1
         If MatDiff > MAT_THRESHOLD Then
            Arc_Right
            Delay_ms(500)
         End If
   End If
end sub
'*******************************************************************************
sub procedure ChaseBall
   if ch0 < ch1 then
      value = ch0
      direction = 0
   end if
   if ch1 < ch0 then
      value = ch1
      direction = 1
   end if
   if ch2 < value then
      value = ch2
      direction = 2
   end if
   if ch3 < value then
      value = ch3
      direction = 3
   end if
   if ch4 < value then
      value = ch4
      direction = 4
   end if
   if ch5 < value then
      value = ch5
      direction = 5
   end if
   if value > IR_MAX_THRESHOLD then
      Forward
      RoomScan
   end if
   select case(direction)
      case 3
         if value < IR_MAX_THRESHOLD then
            Forward
               if value < IR_MIN_THRESHOLD then
                  Forward
                  ToBlack
                  walls
                  'checkbutts
               end if
         end if
      case 1
         if value < IR_MAX_THRESHOLD then
            T_right
         end if
      case 2
         if value < IR_MAX_THRESHOLD then
            T_right
         end if
      case 5
         if value < IR_MAX_THRESHOLD then
            T_left
         end if
      case 4
         if value < IR_MAX_THRESHOLD then
            T_left
         end if
      case 0
         if value < IR_MAX_THRESHOLD then
            T_left
         end if
   end select
end sub
'*******************************************************************************
main:
   TRISA = %00011111
   TRISB = %00000000
   TRISC = %00000000
   ADCON1 = %10000000
   WALL_THRESHOLD = 400
   SENSOR_OFFSET = 75
   MAT_DIFF = 50
   MAT_THRESHOLD = 25
   IR_MAX_THRESHOLD = 850
   IR_MIN_THRESHOLD = 75
   'seed = 0
   direction = 0
   value = 0
while true
   GetValues
   ChaseBall
wend
end.