Really simple Artificial Intelligence (Part 2)

This is part two of really simple AI for Boe-bot in Spin.  In this example, Boe-bot is setup with a speaker (Pin 3) and two contact whiskers (Pins 6 and 9).

The code below gives boe-bot the goal of moving forward and several pre-programmed choices for dealing with obstacles.  Boe-bot tries different maneuvers when it encounters an obstacle until a maneuver is successful in navigating the obstacle.    

A few basic programming hygiene comments about the code:

• Name variables and constants.  Without names the code is less maintainable.  For example, WhiskerContact = 0 and NoWhiskerContact =1.  Without these names it would be very easy to reverse which value meant what!  To improve the below code, the values 0 and 255, which represent the minimum and maximum values that can be stored in a byte should be replaced with constants that reflect their purpose.
• Even better, give clear meaningful names.  Take constants C6, C7 and G6.  As the author I know that they correspond to musical note frequencies.  Unfortunately, that required context and increases the learning curve for others.  Better names would be noteC6, noteC7 and noteG6.
• Split code into small logical units.  This may seem obvious but is important as your program gets larger.  As a general rule, a function should be about 20 lines of code.  It will also help with reuse of code, as larger units of code are less likely to be reusable and much smaller units will not be too simple to capture meaningful functionality.    

Future ideas for this code.  While this code is functional, there are ways it could be enhanced.

• The PickManeuver function has simple logic, selecting the highest value.  Using a statistical model could improve the learning process.  This would also be useful if Boe-bot was in a changing environment and a previously successful maneuver was no longer successful.  
• Generate obstacle avoidance maneuvers at run time.  Currently these are hard coded to be successful options.   

OBJ

 system : "Propeller Board of Education"
 speaker : "PropBOE Square Wave"
 drive : "PropBOE-Bot Servo Drive"
 pst    : "Parallax Serial Terminal Plus"
 pin    : "Input Output Pins"   
 time : "Timing"  

CON
  C6 = 1046
  C7 = 2093
  G6 = 1568
  SpeakerPin = 3
  LeftWhisker = 6
  RightWhisker = 9
  LeftServoPin = 14
  RightServoPin = 15
  WhiskerContact = 0
  NoWhiskerContact = 1

DAT
  '                   left   right    time
  forward      long    100,    100,   1000
  backward     long   -100,   -100,    500
  pivot_R_45   long    100,   -100,    370   
  pivot_L_45   long   -100,    100,    370
  stop         long      0,      0,    500
  backupLeftLeft  long @backupLeftLeft, @backward, @pivot_L_45, @pivot_L_45, @stop, -1
  Right     long @Right, @pivot_R_45, @stop, -1
  Left      long @Left, @pivot_L_45, @stop, -1

VAR

  byte maneuverSuccessL_CntList[3]
  byte maneuverSuccessR_CntList[3]
  byte maneuverSuccessB_CntList[3]
  
PUB Main
  Startup
  LearnNavigation
  
PUB StartUp

  system.Clock(80_000_000)

  speaker.Out(SpeakerPin, 300, G6)
  speaker.Out(SpeakerPin, 300, G6)
  speaker.Out(SpeakerPin, 300, C6)

PUB LearnNavigation | maneuver, i
  pst.Str(String("Starting learn navigation program"))
  pst.NewLine

  bytefill(@maneuverSuccessL_CntList, 128, 3)
  bytefill(@maneuverSuccessR_CntList, 128, 3)
  bytefill(@maneuverSuccessB_CntList, 128, 3)
             
  repeat                                                                
      time.Pause(20)
      if pin.In(LeftWhisker) == WhiskerContact and pin.In(RightWhisker) == WhiskerContact
          maneuver := PickManeuver(@maneuverSuccessB_CntList)
          ExecManeuver(maneuver)
          if pin.In(LeftWhisker) == WhiskerContact and pin.In(RightWhisker) == WhiskerContact
            if(maneuverSuccessB_CntList[maneuver] > 0)
               maneuverSuccessB_CntList[maneuver]--
          else
            if(maneuverSuccessB_CntList[maneuver] < 255)
               maneuverSuccessB_CntList[maneuver]++
      elseif pin.In(LeftWhisker) == WhiskerContact
          maneuver := PickManeuver(@maneuverSuccessL_CntList)
          ExecManeuver(maneuver)
          if pin.In(LeftWhisker) == WhiskerContact
            if(maneuverSuccessL_CntList[maneuver] > 0)
               maneuverSuccessL_CntList[maneuver]--
          else
            if(maneuverSuccessL_CntList[maneuver] < 255)
               maneuverSuccessL_CntList[maneuver]++
      elseif pin.In(RightWhisker) == WhiskerContact
          maneuver := PickManeuver(@maneuverSuccessR_CntList)
          ExecManeuver(maneuver)
          if pin.In(RightWhisker) == WhiskerContact 
            if(maneuverSuccessR_CntList[maneuver] > 0)  
               maneuverSuccessR_CntList[maneuver]--
          else
            if(maneuverSuccessR_CntList[maneuver] < 255)
               maneuverSuccessR_CntList[maneuver]++
      else
        drive.wheels(100,100)  

PUB PickManeuver(maneuverSuccessList) : highScoreIndex | highScore, i          

        highScore := 0
        highScoreIndex := 0
        'use highest ranked option
        
        repeat i from 0 to 2
          if((byte[maneuverSuccessList][i]) > highScore)
              highScore := byte[maneuverSuccessList][i]
              highScoreIndex := i

        return highScoreIndex  

PUB ExecManeuver(maneuver)
  case maneuver
    0: drive.Sequence(@Left)
    1: drive.Sequence(@Right) 
    2: drive.Sequence(@backupLeftLeft)