' sumo.bas     author: Kerwin Lumpkins, 11/6/04
' this is a simple program to run a mini sumo bot equipped with 2 servo
' motors in a differential (tank like) drive/steering system, and a Sharp
' Analog output IR ranging sensor such as the Sharp GP2D120 available from
' acroname.com and others, and an IR photoreflective sensor such as the
' Fairchild QRB1134 available from digikey.com.

' The program calls a subroutine  "adc_get_value" to read the analog voltage
' output from each of the sensors, storing the result in a variable called
' linevalue (for the white/black surface of the sumo ring) and rangevalue which
' holds the analog to digital converted 10 bit value of the range sensor.

' By experiment, I determined that a rangevalue of 300 would be pretty close
' range (4 inches or so), and 150 would be a long range (about 8 inches) but
' within detection range.

' When the IR photoreflective sensor is over the black surface, the linevalue
' will be about 700-900.  When on white, about 40 or so.  So I set a trigger
' value of 500 and call this a constant value called "onblack".  I also make
' constant definitions for the longrange and closerange values.

' The bot starts by initializing the motors and A/D converter module, then starts
' running the loop portion of the program.  Here it does the very simple
' behavior of "get range and line value.  Is line value less than the on black
' constant?  If yes, then bot is at the "border" so back up and turn right.  If
' line value is not less than onblack then check the rangevalue using the
' select/case statement which just checks the range and if rangevalue is less
' than longrange (remember that the value gets bigger as the range increases)
' then go into "seekhim" mode (turn right and start over).  If rangevalue is
' in the long to short range, drive forward (which will hopefully result in the
' opponent being pushed out of ring).

' This is a very simple program.  you can make the behavior more complex by
' adding to the existing "border" and "seekhim" routines, or by adding your
' own control routines.

' The servo init and set subroutines should be left alone, as will most likely
' forward/backward/right/left/stop subroutines.  Be aware that the direction
' of travel may be reversed depending on how you install the motors.  If so,
' just swap the labels "forward" and "backward", and also "jog_left" and
' "jog_right", fixing this problem in software rather than disassembling your
' robot.


' put in the cal value for your part here (marked on the 8535 case
' most are hex 9F, some 91's and some in between
Osccal = &HAF

$crystal = 8000000

'*****************************************************************
' Analog to Digital Conversion related items
' used for both the sharp IR range sensor and the Fairchild IR reflective sensor
Declare Sub Adc_init
Declare Function Adc_get_value(byval Channel As Byte) As Integer
Dim Adc_value As Integer
Dim Channel As Byte

'******************************************************************
'servo motor control routines
Declare Sub Servos12_init
Declare Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)
Declare Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)
Dim Servos12_active As Bit
Dim Setting As Byte

' these subroutines combine the servo1_set and servo2_set subs to get
' a combined drive forward, drive backward, etc function
Declare Sub Forward()
Declare Sub Backward()
Declare Sub Jog_left()
Declare Sub Jog_right()
Declare Sub Stopmotors()

' constants that are loaded into the servo1_set/servo2_set routines to
' accomplish left forward, right forward, etc (hence Lf, Rf, etc)
Const Lf = 60
Const Rf = 30
Const Lb = 30
Const Rb = 60

'******************************************************************
' subroutine for the seekhim behavior, short for seek him out and destroy him
Declare Sub Seekhim()


' constants for the channel into which the sensors are plugged
' rather than try to remember in the code below which channel has the IR range
' sensor plugged into it, I just give a name that I can remember to the number
' of the channel.  linesense is A/D channel 7, etc
Const Linesense = 7
Const Rangesense = 0

' variables to store the A/D values from the sensors
Dim Line_value As Integer
Dim Range_value As Integer

' again, a constant is declared with a name to represent the number.  easier to
' remember a name than a number.
Const Longrange = 150
Const Closerange = 300
Const Onblack = 500



' ***************************************************************
' init routine waits 5 seconds before doing anything so that the robot will
' sit and wait 5 seconds before moving when power is turned on, then
' initializes the motors and the A/D module
Init:
   Wait 5
   Call Servos12_init
   Call Adc_init



'******************************************************************
' the main loop starts here
Get_range:
Do
   Line_value = Adc_get_value(linesense)
   Print "line:" ; Line_value
   Range_value = Adc_get_value(rangesense)
   Print "range:" ; Range_value

   If Line_value < Onblack Then Goto Border
   Select Case Range_value
      Case Is < Longrange : Call Seekhim
      Case Longrange To Closerange : Call Forward()
      Case Else : Call Forward()
   End Select

Loop


Border:
   Call Backward()
   Wait 1
   Call Jog_right()
   Waitms 300
   Call Stopmotors
   Goto Get_range

Sub Seekhim :
   Call Jog_right
   Waitms 100
   Call Stopmotors
   Goto Get_range
End Sub Seekhim

' the end statement is required for a program.
End




Sub Servos12_init:

' set bits 5 and 4 for output
Ddrd = Ddrd Or &H30

' set timer 1 for pwm output, prescaler 64
' these settings are for 1 MHz clock
Tccr1a = &HA3
Tccr1b = &H0C

'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

' Here the servos are init'd to be at an "OFF" setting
' hex 03FF gives a 100 % duty cycle pulse.
Ocr1ah = &H03
Ocr1al = &HFF

Ocr1bh = &H03
Ocr1bl = &HFF

Servos12_active = 1

End Sub Servos12_init


Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)
'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

   Ocr1bh = Hset
   Ocr1bl = Lset
End Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)


Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)
'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

   Ocr1ah = Hset
   Ocr1al = Lset
End Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)


Sub Backward():
   Print "f"
   Call Servo1_set(0 , Lf)
   Call Servo2_set(0 , Rf)
   'Waitms 100
   'Wait 1
   'Call Stopmotors()
   'waitms 20
End Sub Forward()

Sub Forward():
   Call Servo1_set(0 , Lb)
   Call Servo2_set(0 , Rb)
   'Wait 1
   'Call Stopmotors()
End Sub Forward()

Sub Jog_left():
   Print "l"
   Call Servo1_set(0 , Lb)
   Call Servo2_set(0 , Rf)
   'Waitms 100
   'Call Forward()
   'Call Stopmotors()
   'waitms 20
End Sub Jog_left()


Sub Jog_right():
   Print "r"
   Call Servo1_set(0 , Lf)
   Call Servo2_set(0 , Rb)
   'Waitms 100
   'Call Forward()
   'Call Stopmotors()
   'Waitms 20
End Sub Jog_right()

Sub Stopmotors():
   Call Servo1_set(3 , 255)
   Call Servo2_set(3 , 255)
End Sub Stopmotors()

' ******************************************************************
' ADC sub and functions
Sub Adc_init
   ' set for AVCC reference voltage,Right justified,channel 0
   Admux = &H40

   'set ad status and control register for conversion disabled, conversion
   'not started,auto trigger disabled,interrupts disabled,and divide
   'by 64 prescaler
   Adcsra = &H06

End Sub Adc_init

Function Adc_get_value(byval Channel As Byte) As Integer
   Dim Flag As Byte
   Dim Adcnotdone As Bit
   'init the admux for channel 0
   Admux = Admux And &HE0
   Admux = Admux + Channel

   'enable the adc
   Adcsra = &H86
   'start the conversion
   Adcsra = &HC6

   Adcnotdone = 1

   While Adcnotdone = 1
     Flag = Adcsra And &H40

     'wait until the adc interrupt flag is set
     If Flag = 0 Then Adcnotdone = 0
   Wend

   Dim Adcvalh As Integer
   Dim Adcvall As Integer
   Dim Adcval As Integer
   Adcvalh = 0
   Adcvall = 0
   Adcval = 0

   'get the 2 adc hi and lo bytes and combine into one integer value
   Adcvall = Adcl
   Adcvalh = Adch
   Shift Adcvalh , Left , 8
   Adcval = Adcvalh + Adcvall
   'disable conversion
   Adcsra = &H06
   Adc_get_value = Adcval
End Function Adc_get_value(byval Channel As Byte)