Little Sign Follower

edmunds

Senior Member
Dear all,

While I'm procrastinating with BNO055 intelligent IMU+sensor fusion processor because I know it will be difficult to solder :), I have been busy adding functions to line following. Meet the Sign Follower.

The car can now distinguish between 'just a line, business as usual', 'straight arrow', 'brake marker' and the stop line itself. Since I have not fixed the comms back to the main processor, I cannot stop the car at the stop line yet, but the task was to see if any patterns could be recognised, not acting on them at this point. A short video is available here.

The code is nothing special in the end, but my brain was exploding. After trying to design it on paper, failing, trying in the car, failing and that going on for a day, I decided to try flowcharts in programming editor. While it is certainly not the most stable or easy to use part of the package, it did the job! After some fiddling around with all the boxes and arrows my flowchart was converted to strange, but working code with some 20+ GOTOs. Working through it and replacing the calls with actual code from them and fixing a few remaining bugs, resulted in code that was both, readable and worked on sample data in the simulator on the first try.

I was worried about this taking a lot of time at first and given the struggle I have had with code becoming more complex and timing becoming more important, but also more difficult to judge accurately, I first ran this on a separate system with scope attached to it. Much to my relief, execution of the longest cycle possible took about 0.9ms with ~0.5ms being sample data read from the memory. +0.4ms for road sign recognition I can afford :). It is only one condition overhead for a normal line read, when no signs are expected. Since my sample data was a replica of data collected from the vehicle, it also worked on the actual vehicle on the first download.

Code:
#macro ReadRoadSigns()
	if no_line > 2 then            '... after sufficient gap
    gap_flag = 1                 'Set gap flag to 1, to release no_line
  endif
  no_line = 0                    'Release no_line, do not accumulate with line visible
  if gap_flag = 1 then           'If gap was registered
  	if line_width > 12 then      'If line all over the sensor, it is either a stop line ...
    	if arrow_body > 3 then     '... or if arrow body was recorded, maybe arrow head.
      	inc arrow_head           'If that is the case, start counting for arrow head.
      	goto done_signs:         'Read next sample.
      end if
      inc stop_line              'If not arrow, has to be stop line or emergency.
      if stop_line > 3 then      'If line has been all over the sensor for over 3 loops ...
        LineLost                 '... initialise emergency stop procedure for all white problem.
      	goto done_signs:         'Read next sample.
    	end if
    	goto done_signs:           'Read next sample.
    end if
    select case stop_line
    case 0                       'In case we are into arrows, not stop lines
      if arrow_head = 0 then     'In case we have not seen the head yet, continue counting body.
        inc arrow_body
      else                       'If there is a head, count head instead.
        inc arrow_head
      endif
    case > 1                     'If stop line has been registered at least for two loops (but less than 4)
      StopLineMark               'Notify the main processor by sending info on hserout
      stop_line = 0              'Release stop_line
      gap_flag = 0              'Release gap_flag
    	goto done_signs:           'Read next sample.
  	endselect
  	goto done_signs:             'Read next sample.
	end if
	goto done_signs:               'If there was no gap detected, nothing to do here, go to start
#endmacro

#macro ReadNoLine()
	if arrow_head > 3 then          'When first no_line iteration, check if there was no arrow, if was ...
    StraightArrowMark             '... notify the main processor by sending info on hserout
  end if
  gap_flag = 0                    'Restart all road marking related counters
	arrow_body = 0    
	arrow_head = 0    
  inc no_line                     'Increase no_line condition counter
  if no_line > 50 then
    LineLost                      '... initialise emergency stop procedure for all dark problem.
  endif
#endmacro

#macro GetLinePos(line_state)
'Determine the position of the line
  if line_state > 0 then                            'If there is any line at all ...
    line_width = NOB line_state                     '... find the number of set bits = line width ...
    line_edge = NCD line_state                      '... find the highest set bit = rightmost edge of the line ...
    line_centre = line_width / 2                    '... store half of the line in a new variable to save on number of operations
    select case line_edge                           'Cater for right and left side limits situations when not all of the line is seen ...
    case 1  : line_pos = line_centre                '... line dissapearing on the left side ...
    case 15 : line_pos = 15 - line_centre           '... line dissapearing on the right side ...
    else    : line_pos = line_edge - line_centre    '... both edges inside field of view
    endselect
    ReadRoadSigns                                   'Register road marking if any
  else
    ReadNoLine                                      'Take action for no_line situations
  endif
done_signs:
  select case line_pos                              'Maybe can be done faster, but takes us as it is
  case 1 to 7  : Eline = -8 + line_pos              'Set negative Eline from line_pos
  case 9 to 15 : Eline = line_pos - 8               'Set positive Eline from line_pos
  else         : ELine = 0                          'Set error to zero
  endselect
#endmacro

Have a good weekend everyone,

Edmunds
 
Last edited:

edmunds

Senior Member
Dear all,

I was thinking about asking it, but now, when there is a GOTO/no GOTO war out in the other thread - I will :).

I do not like the GOTOs in the code above, but I think part of the reason I had to ask flow chart for help is because I had completely eliminated the idea of exiting a loop with GOTO or any sort of jumping around to labels in the code from my algorithm calculator part of the brain.

Is there a way to avoid five GOTOs in the code above?


Thank you for your time,

Edmunds
 

BESQUEUT

Senior Member
Is there a way to avoid five GOTOs in the code above?
You can always write code without any goto.
When you write IF ... THEN always write ELSE before writing ENDIF
Code:
[color=Navy]#macro [/color][color=Black]ReadRoadSigns[/color][color=Blue]()
  if [/color][color=Black]no_line [/color][color=DarkCyan]> [/color][color=Navy]2 [/color][color=Blue]then            [/color][color=Green]'... after sufficient gap
    [/color][color=Black]gap_flag [/color][color=DarkCyan]= [/color][color=Navy]1                 [/color][color=Green]'Set gap flag to 1, to release no_line
  [/color][color=Blue]endif
  [/color][color=Black]no_line [/color][color=DarkCyan]= [/color][color=Navy]0                    [/color][color=Green]'Release no_line, do not accumulate with line visible
  
  [/color][color=Blue]if [/color][color=Black]gap_flag [/color][color=DarkCyan]= [/color][color=Navy]1 [/color][color=Blue]then           [/color][color=Green]'If gap was registered
      [/color][color=Blue]if [/color][color=Black]line_width [/color][color=DarkCyan]> [/color][color=Navy]12 [/color][color=Blue]then      [/color][color=Green]'If line all over the sensor, it is either a stop line ...
            [/color][color=Blue]if [/color][color=Black]arrow_body [/color][color=DarkCyan]> [/color][color=Navy]3 [/color][color=Blue]then     [/color][color=Green]'... or if arrow body was recorded, maybe arrow head.
                  [/color][color=Blue]inc [/color][color=Black]arrow_head           [/color][color=Green]'If that is the case, start counting for arrow head.
            [/color][color=Blue]else
                  inc [/color][color=Black]stop_line              [/color][color=Green]'If not arrow, has to be stop line or emergency.
                  [/color][color=Blue]if [/color][color=Black]stop_line [/color][color=DarkCyan]> [/color][color=Navy]3 [/color][color=Blue]then      [/color][color=Green]'If line has been all over the sensor for over 3 loops ...
                        [/color][color=Black]LineLost                 [/color][color=Green]'... initialise emergency stop procedure for all white problem.
                  [/color][color=Blue]endif
            endif


      else
        select case [/color][color=Black]stop_line
        [/color][color=Blue]case [/color][color=Navy]0                       [/color][color=Green]'In case we are into arrows, not stop lines
            [/color][color=Blue]if [/color][color=Black]arrow_head [/color][color=DarkCyan]= [/color][color=Navy]0 [/color][color=Blue]then     [/color][color=Green]'In case we have not seen the head yet, continue counting body.
                  [/color][color=Blue]inc [/color][color=Black]arrow_body
            [/color][color=Blue]else                       [/color][color=Green]'If there is a head, count head instead.
                  [/color][color=Blue]inc [/color][color=Black]arrow_head
            [/color][color=Blue]endif

        case [/color][color=DarkCyan]> [/color][color=Navy]1                     [/color][color=Green]'If stop line has been registered at least for two loops (but less than 4)
            [/color][color=Black]StopLineMark               [/color][color=Green]'Notify the main processor by sending info on hserout
            [/color][color=Black]stop_line [/color][color=DarkCyan]= [/color][color=Navy]0              [/color][color=Green]'Release stop_line
            [/color][color=Black]gap_flag [/color][color=DarkCyan]= [/color][color=Navy]0              [/color][color=Green]'Release gap_flag
        [/color][color=Blue]endselect
     endif
end if
goto [/color][color=Black]done_signs:               [/color][color=Green]'If there was no gap detected, nothing to do here, go to start[/color]
[color=Navy]#endmacro[/color]
For last one, as this is a macro, we can't know what you want to do. We need the whole code to understand.
 

stan74

Senior Member
If they all goto the same place put the code in a subroutine and use multiple returns just to make things complicated like forth.
 
Top