'
' A basic little robot program.
'
'***************************************************************
' Declaration and Variables Section
'***************************************************************
include "regs11.lib"

declare SensorInput	' Variable that holds input sensor data
declare BehaveState	' Variable that holds the previous behavior
declare TurnDelayCount	' Variable for timing turns
'
' I like to name constants relative to their function. For example
' the following constants are used with the SensorInput variable,
' so I put the variable name in the constant name. This makes it
' easier to find functions that are affecting and affected by
' the value or the sensor. I also relate the function of the
' value in the name, as well as the group. For example, you
' can see which bits belong to the sensor variable, which
' are bumper related, and what their functions are. This is a good
' way to keep track of things.

const  Sensor_Port			= portc
const  Sensor_Port_DDR			= ddrc
const  Sensor_Bumper_Left		= %00000001
const  Sensor_Bumper_Right 		= %00000010
const  Sensor_Bumper_Back 		= %00000100
const  Sensor_Bumper_Mask		= %00000111
const  Sensor_Mask			= %00000111
const  Sensor_DDRC_Value		= Sensor_Mask XOR $FF

const  Motor_Port	= portb
const  Motors_Forward  	= %10100000
const  Motors_Backward 	= %01010000
const  Motors_Left	= %10010000
const  Motors_Right	= %01100000
const  Motors_Stop	= %00000000

const  Behave_Forward		= 1
const  Behave_Back_Left		= 2
const  Behave_Back_Right	= 3
const  Behave_Turn_Left		= 4
const  Behave_Turn_Right	= 5
const  Behave_Stop		= 6

const  Turn_Delay_Counter_Init  = 300
'***************************************************************
' Subroutines and Functions section
'***************************************************************

'
' Move_Forward sets both drive motors into forward motion
' Again, I like to categorize functions into groups, and
' make them explicit in the name.
'
Motors_Move_Forward:
        pokeb Motor_Port, Motors_Forward
        return

Motors_Move_Backward:
        pokeb Motor_Port, Motors_Backward
        return

Motors_Turn_Left:
        pokeb Motor_Port, Motors_Left
        return

Motors_Turn_Right:
        pokeb Motor_Port, Motors_Right
        return

Motors_Move_Stop:
        pokeb Motor_Port, Motors_Stop
        return

StartTurnDelay:
	TurnDelayCount = Turn_Delay_Counter_Init
	return
StopTurnDelay:
	TurnDelayCount = 0
	return
UpdateTurnDelay:
	if TurnDelayCount <> 0
		TurnDelayCount = TurnDelayCount - 1
        endif
	return
'
' Initialize_Robot is called once when the robot starts. It will
' put the robot into a known state.
'
Initialize_Robot:
      '
      ' Setup the serial port for 9600 baud
      '
      pokeb   baud, $30
      pokeb   sccr2, $0c

      ' Set the data direction register for the sensor inputs

      pokeb   Sensor_Port_DDR, Sensor_DDRC_Value

      ' Set the motors to the off position
      gosub Motors_Move_Stop

      TurnDelayCount = 0
      BehaveState = Behave_Forward
      SensorInput = 0
      return

'
' Read sensors and put the values into a variable.
' This routine includes any 'processing' that needs
' to be done to the sensor data
'
Sensor_Read:

	' Read the sensor values
	SensorInput = peekb(Sensor_Port)

	' Mask off any output pins
	SensorInput = SensorInput AND Sensor_Mask

	' Input switches are active low. Flip the sense of
	' the bits so they are active high in the variable
	' This allows them to be tested correctly in the
	' rest of the program
	SensorInput = SensorInput XOR Sensor_Bumper_Mask

	return

'
' Behave is the routine that determines the robots behaviour. This
' routine will evaluate the sensors, determine the correct action,
' and make the robot do what it is supposed to
'
Behave:

    if (SensorInput AND Sensor_Bumper_Left) <> 0
	' We are hitting something on the front left.
	' Back off of it
	gosub Motors_Move_Backward
        gosub StopTurnDelay
        BehaveState = Behave_Back_Left

    elseif (SensorInput AND Sensor_Bumper_Right) <> 0
        ' We are hitting something on the front right.
        ' Back off of it
        gosub Motors_Move_Backward
        gosub StopTurnDelay
        BehaveState = Behave_Back_Right

    elseif (SensorInput AND Sensor_Bumper_Back) <> 0
        ' We just backed into something, go forward
        gosub Motors_Move_Forward
        gosub StopTurnDelay
        BehaveState = Behave_Forward
    else
        ' The bumper switches are clear. Therefore, we
        ' set our state based on the behaviour information
        select (BehaveState)
            case Behave_Back_Left
		' We were backing because of something on left
		' Turn to the right
		gosub Motors_Turn_Right
		BehaveState = Behave_Turn_Right
		gosub StartTurnDelay

            endcase
            case Behave_Back_Right
		' We were backing because of something on right
		' Turn to the left
		gosub Motors_Turn_Left
		BehaveState = Behave_Turn_Left
		gosub StartTurnDelay
	    endcase
	    '
	    ' The default action for the case statement.
	    '
	    ' If the TurnDelayCount is Zero, then we can
	    ' go forward. Otherwise, we will continue to
	    ' turn in the TurnDelayCount direction.
	    if TurnDelayCount = 0
    		gosub Motors_Move_Forward
		BehaveState = Behave_Forward
	    endif

	endselect
	
    endif
    return

'***************************************************************
'
' Main is the starting point for the program. It will handle calling
' the appropriate initialization routine, print out an indication that
' it is alive, then go into the Infinite Loop
'
'***************************************************************
Main:

      ' Initialize the robot first thing

      gosub Initialize_Robot

      ' Give some indication that the robot is starting.

      print "Robot program has started"

      '
      ' Being the infinite loop. Notice there is no way out of this loop
      '
      do
	    gosub Sensor_Read
	    gosub Behave

	    '
	    ' Do any housekeeping chores
	    '
	    gosub UpdateTurnDelay

      loop

      '
      ' Code after the final loop should never get executed, and therefore
      ' shouldn't exist. If you are paranoid, then you should put in some
      ' code to stop the robot in its tracks, and shutdown.
      '

end