'--------------------------------------------------------------
' file: darc_bumpngo - this file is meant to be ran on the DARC board
' It sets up a simple read of a switch that is
' connected as shown below to port A7
'
'                  +5
'                 ----
'                  |
'                  |
'                  \
'                  / 10K
'                  \
'                  /
'                  |            /
'                  |           /
'   A7 0----------------------0     0----
'                                       |
'                              switch   |
'                                      ---
'                                      \ / Gnd
'

' This program reads the state of pin A7.  When the switch is open
' A7 will be pulled up to 5V (HI) and the forward section of the
' code is jumped to by goto statement.  in Forward the servos are
' are turned so that the bot goes forward.  When the switch is closed
' (bump sensor) then the bot will go into "avoid" where it reverses
' the motors, then spins to the right and starts forward again.
'--------------------------------------------------------------

' 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 = &HA3

$crystal = 8000000

' these declares are for the servo init and run subroutines
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

'Main
' init the servos
Call Servos12_init

'set port a for all outputs but A7 which is input
Ddra = &B01111111
' port c is outputs on upper 4 bits (where LEDs are), input on lower 4
Ddrc = &B11110000


'drive C6 LED on for 5 seconds to indicate that code is loaded and running
Portc = &B01000000
Wait 5
' then turn off all LEDs
Portc = &B00000000
Wait 1


Do
Loop1:
  If Pina.7 = 1 Then Goto Forward Else Goto Avoid
Loop



Forward:
   ' LED on C7 is on while in forward mode
   Portc = &B10000000
   Call Servo1_set(0 , 60)
   Call Servo2_set(0 , 20)
   Goto Loop1

Avoid:
   ' LED C4 is on while in avoid mode
   Portc = &B00010000
   ' set for backward
   Call Servo1_set(0 , 20)
   Call Servo2_set(0 , 60)
   Wait 3

   ' set for  right turn
   Call Servo1_set(0 , 60)
   Call Servo2_set(0 , 60)
   Wait 1


   ' then turn off the servos
   Call Servo1_set(&H03 , &HFF)
   Call Servo2_set(&H03 , &HFF)
   Goto Loop1

End


' the rest is servo init and set value subs
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)