program SoccerProgram2
'*******************************************************************************
dim ch0,ch1,ch2,ch3,ch4 as word         'i have the second to fastest
dim direction,value as word             'gear ratio, change delays accoringly
dim rightbutt,leftbutt as word
dim MatRead1,MatRead2,MatDiff as word
dim LSharp,RSharp as word
dim Butts as Integer
dim Seed,WallState as byte
dim WALL_THRESHOLD, IR_MAX_THRESHOLD, MAT_DIFF, SENSOR_OFFSET as word
dim MAT_BLACK_THRESHOLD,MAT_WHITE_THRESHOLD, IR_MIN_THRESHOLD as word
'*******************************************************************************
sub procedure Forward       'insert your forward, T_Left, T_Right, and reverse
   PORTB = %00011101        'routines, these MAY BE DIFFERENT from yours
end sub
'*******************************************************************************
sub procedure Reverse
   PORTB = %00101011
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 Walls                               'ignore this for now
   RSharp = adc_read(3)
   LSharp = adc_read(4)
   if RSharp > WALL_THRESHOLD then
      WallState.0 = 1
   else
      WallState.0 = 0
   end if
   if LSharp > WALL_THRESHOLD then
      WallState.1 = 1
   else
      WallState.1 = 0
   end if
   select case (WallState)
      case 1
         T_Left
         delay_ms(250)
      case 2
         T_Right
         delay_ms(250)
      case 3
         reverse
         delay_ms(500)
         T_Left
         delay_ms(250)
   end select
end sub
'*******************************************************************************
sub procedure GetValues

   PORTC = %00000000
   ch0 = adc_read(0)

   PORTC = %00000001
   ch1 = adc_read(0)

   PORTC = %00000010
   ch2 = adc_read(0)

   PORTC = %00000100
   ch3 = adc_read(0)

   PORTC = %00000101
   ch4 = adc_read(0)

end sub
'*******************************************************************************
sub procedure CheckButts                                'igonre this for now
   leftbutt = adc_read(1)
   rightbutt = adc_read(2)
   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
   MatRead1 = adc_read(2)
   Forward
   Delay_ms(250)
   MatRead2 = adc_read(2)
   If MatRead1 > MatRead2 Then
      MatDiff = MatRead1 - MatRead2
         If MatDiff > MAT_BLACK_THRESHOLD Then
            T_Right
            Delay_ms(500)
         End If
   End If
end sub
'*******************************************************************************
sub procedure ToWhite
   MatRead1 = adc_read(2)
   Forward
   Delay_ms(250)
   MatRead2 = adc_read(2)
   If MatRead1 > MatRead2 Then
      MatDiff = MatRead1 - MatRead2
         If MatDiff > MAT_WHITE_THRESHOLD Then
            T_Left
            Delay_ms(500)
         End If
   End If
end sub
'*******************************************************************************
Sub Procedure hunt
   select case (seed)
         case 0,1,2,3,4,6,7,8,9,40,41,42,43,44,46,47,48,49
            forward
            delay_ms(150)
         case 5,45
            forward
            delay_ms(150)
            halt
            delay_ms(100)
         case 10,11,12,13,14,16,17,18,19,30,31,32,33,34,36,37,38,39
            T_Right
            delay_ms(25)
         case 50,51,52,53,54,56,57,58,59,70,71,72,73,74,76,77,78,79
            T_Right
            delay_ms(25)
         case 15,35,55,75
            T_Right
            delay_ms(25)
            halt
            delay_ms(100)
         case 20,21,22,23,24,26,27,28,29,60,61,62,63,64,66,67,68,69
            Forward
            delay_ms(75)
         case 25, 65
            Forward
            delay_ms(75)
            halt
            delay_ms(100)
         end select
      seed = seed + 1
      if seed = 80 then
         seed = 0
      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                                    'for me my sensors go    0
   select case(direction)                      '                   1         4
      case 0                                   '
         if value < IR_MAX_THRESHOLD then      '
            Forward                            '                     2     3
               if value < IR_MIN_THRESHOLD then'change the turn directions accordingly
                  ToBlack
                  'ToWhite
                  walls
                  'checkbutts
               end if
         end if
      case 1
         if value < IR_MAX_THRESHOLD then
            T_left
         end if
      case 2
         if value < IR_MAX_THRESHOLD then
            T_left
         end if
      case 3
         if value < IR_MAX_THRESHOLD then
            T_right
         end if
      case 4
         if value < IR_MAX_THRESHOLD then
            T_right
         end if
   end select
   if value > IR_MAX_THRESHOLD then
      'hunt
      halt
   end if
end sub
'*******************************************************************************
main:
   TRISA = %00011111
   TRISB = %00000000
   TRISC = %00000000
   ADCON1 = %10000000
   seed = 0
   WALL_THRESHOLD = 300   'currently inconsequential
   SENSOR_OFFSET = 75     'currently inconsequential
   MAT_DIFF = 50          'currently inconsequential
   MAT_BLACK_THRESHOLD = 25
   MAT_WHITE_THRESHOLD = -25     'to determine relative direction
   IR_MAX_THRESHOLD = 500 'to differentiate between the ball and ambient light
   IR_MIN_THRESHOLD = 50  'to determine whether the ball is captured
   forward
   delay_ms(750)
while true
   direction = 0
   value = 0
   GetValues
   ChaseBall
wend
end.