' mars_rover.bas - rev 4  08/20/04  Author: kerwin lumpkins
' This program runs on a DARC board in the mars rover robot.  It watches for
' serial input as a 2-3 byte command, decodes the command and then drives
' the appropriate output.  Runs in continuous loop to get command then service.

' Supported commands in this rev of code are:
'mfx/mbx - move forward/backward hex x units. Each unit is 5 cm distance.
'mrx/mlx - move right/left. Neutral turn about 10 degrees per x unit.
'trx/tlx - turret left hex x units (rotates the turret about 10 degrees)
'cd1/2/3 - tilt camera down. 1 gives about 30 degrees down, 2 gives 60 and 3 is 90 degrees down.
'cu1/2/3 - Like the cd command but this gives control to tilt up 30, 60 and 90 degrees.
'cc - camera center. Takes camera back to center vertical position.
'gs - get sonar range, prints range in cm
'gix - get IR range data, x specs 1-fl, 2-fr, 3-bl, 4-br

' The do loop watches for input on UART RX line and uses nested if statements
' to decide which command is being invoked, then which subcommand, and then
' what if any magnitude command is given.  It then calls the approriate
' subroutine to carry out the command (motor control or sensor driver).  In
' the case of a sensor get command, it will feedback data from the sensor.
'--------------------------------------------------------------

' 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

' ******* declaration section. Declare subroutines, variables, constants
' servo and motor control subroutines declared
Declare Sub Servo4_init
Declare Sub Servo4_set(byval Setting As Byte)
Declare Sub Pwm_mtr12_init()
Declare Sub Pwm_mtr1_set(byval Hi As Byte , Byval Lo As Byte)
Declare Sub Pwm_mtr2_set(byval Hi As Byte , Byval Lo As Byte)
Declare Sub Pwm_mtr3_init
Declare Sub Pwm_mtr3_set(byval Setting As Byte)

' higher level DC motor control routines for control of DMC board
Declare Sub Forward()
Declare Sub Backward()
Declare Sub Rightturn()
Declare Sub Leftturn()
Declare Sub Stopmotors()
Declare Sub Turretright()
Declare Sub Turretleft()
Declare Sub Turretstop()

' items for the ADC module
Declare Sub Init_adc
Declare Function Get_darc_adc(byval Channel As Byte) As Integer
Dim Adc_value As Integer
Dim Channel As Byte

' variables for IR sensor module output
Dim Flraw As Integer , Frraw As Integer , Blraw As Integer , Brraw As Integer
Dim Fl As Byte , Fr As Byte , Bl As Byte , Br As Byte
Dim Perim As Byte


' delay constants used to calculate how long to run motors
Const Forwarddelay = 300
Const Backwarddelay = 300
Const Turndelay = 300
Const Rotatedelay = 50
Dim Delay As Word

' pwm function settings for different speeds
Const Fast1 = &H02
Const Fast2 = &HD0
Const Slow1 = &H01
Const Slow2 = &H32

' serial input command items
Dim S As String * 10
Dim L As Byte
Dim Cmd1str As String * 5 , Cmd2str As String * 5 , Cmd3str As String * 5
Dim Cmd3val As Byte

' port c has dc motor dir control lines
Ddrc = &B11111100
Ddra.0 = 1

Dirleft1 Alias Portc.7
Dirleft2 Alias Portc.6
Dirright1 Alias Portc.5
Dirright2 Alias Portc.4
Dirturret1 Alias Portc.3
Dirturret2 Alias Portc.2

' SHarp GP2D12 IR ranging sensors are on port A as Analog voltages
' below defines what channel is tied for which sensor (front left,back left,etc)
Const Irfl = 7
Const Irfr = 6
Const Irbl = 4
Const Irbr = 3

' sonar sensor related subs and variables
Declare Sub Srf04_init
Declare Sub Srf04_get_distance
Dim Temp As Bit
Dim Starttime As Integer , Endtime As Integer , Sonartime As Integer
Dim Dist As Integer

' head lights port definition
Leds Alias Porta.0


' ***************************************************************
' *************** start of the program code *********************
'Main
   ' init the DC motor controller lines
   Dirleft1 = 0
   Dirleft2 = 0
   Dirright1 = 0
   Dirright2 = 0
   Dirturret1 = 0
   Dirturret2 = 0

      ' init the timer for servo and pwm control
   Call Servo4_init
   Call Pwm_mtr12_init
   Call Pwm_mtr3_init
   Call Init_adc
   Call Srf04_init

   ' wait a bit before starting
   Wait 3

Do
   S = "0"
L = 0
Input S
L = Len(s)
'Print "A3rx: " ; S
'Print "length : " ; L

Cmd1str = Mid(s , 2 , 1)
Cmd2str = Mid(s , 3 , 1)
Cmd3str = Mid(s , 4 , 1 )
Cmd3val = Hexval(cmd3str)

'Print ; Cmd1str ;
'Print " " ; Cmd2str ;
'Print " " ; Cmd3str

' this is for the move "m" command
If Cmd1str = "m" Then
     If Cmd2str = "f" Then
        Call Forward()
        Delay = Cmd3val * Forwarddelay
        Waitms Delay
        Call Stopmotors

     Elseif Cmd2str = "b" Then
        Call Backward()
        Delay = Cmd3val * Backwarddelay
        Waitms Delay
        Call Stopmotors

     Elseif Cmd2str = "r" Then
        Call Rightturn()
        Delay = Cmd3val * Turndelay
        Waitms Delay
        Call Stopmotors

     Elseif Cmd2str = "l" Then
        Call Leftturn()
        Delay = Cmd3val * Turndelay
        Waitms Delay
        Call Stopmotors

     End If

End If
' that's it for move command "m"


' items for turret "t" commands
If Cmd1str = "t" Then
     If Cmd2str = "r" Then
        Call Turretright()
        Delay = Cmd3val * Rotatedelay
        Waitms Delay
        Call Turretstop()


     Elseif Cmd2str = "l" Then
        Call Turretleft()                                   'start motor
        Delay = Cmd3val * Rotatedelay
        Waitms Delay
        Call Turretstop()

     End If

End If
' that's it for turret command "t"


' items for camera tilt "c" commands
If Cmd1str = "c" Then
     If Cmd2str = "c" Then
        Call Servo4_set(&H08)                               ' center the camera tilt assembly

     Elseif Cmd2str = "d" Then
        If Cmd3val = 1 Then
           Call Servo4_set(&H0a)
        Elseif Cmd3val = 2 Then
           Call Servo4_set(&H0c)
        Elseif Cmd3val = 3 Then
           Call Servo4_set(&H0f)
        End If

     Elseif Cmd2str = "u" Then
        If Cmd3val = 1 Then
           Call Servo4_set(&H06)
        Elseif Cmd3val = 2 Then
           Call Servo4_set(&H04)
        Elseif Cmd3val = 3 Then
           Call Servo4_set(&H02)
        End If                                              'tilt up

     End If

End If
' that's it for camera tilt command "c"

' items for leds (lights) "l"
If Cmd1str = "l" Then
     If Cmd2str = "o" Then Leds = 1 Else Leds = 0

End If
' that's it for leds command "l"


' items for get sensor data
If Cmd1str = "g" Then
   If Cmd2str = "i" Then
        If Cmd3val = Irfl Then
           Adc_value = Get_darc_adc(irfl)
        Elseif Cmd3val = Irfr Then
           Adc_value = Get_darc_adc(irfr)
        Elseif Cmd3val = Irbl Then
           Adc_value = Get_darc_adc(irbl)
        Elseif Cmd3val = Irbr Then
           Adc_value = Get_darc_adc(irbr)
        End If

        Print "ADC for IR#" ; Cmd3val ; ": " ; Adc_value

    Elseif Cmd2str = "s" Then
        Call Srf04_get_distance()
        Print "Sonar range: " ; Dist

    End If
End If


Loop

End


' end of main control routine.  Everything below here are low level subroutines
' for device drivers


Sub Servo4_init:
' set bit 3 for output
Ddrb.3 = 1

' set timer 2 for pwm output, prescaler 1024
' these settings are for 8 MHz clock
Tccr0 = &H6D

'OCR2 is an 8 bit value that sets the on time or duty cycle.
'Pulse width = (setting + 1) * 130 us.  So a setting of
'0 will give a 130 us pulse.  Setting = 1 gives a 260 us
'pulse.  A setting of 10 will give about 1.5 ms pulse.
'The OFF setting is &HFF.

Ocr0 = &HFF
End Sub Servo4_init


Sub Servo4_set(setting As Byte)
'OCR2 is an 8 bit value that sets the on time or duty cycle.
'Pulse width = (setting + 1) * 130 us.  So a setting of
'0 will give a 130 us pulse.  Setting = 1 gives a 260 us
'pulse.  A setting of 10 will give about 1.5 ms pulse.
'The OFF setting is &HFF.

   Ocr0 = Setting
End Sub Servo4_set(setting As Byte)



Sub Pwm_mtr12_init():

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

' set timer 1 for pwm output, prescaler 64
' these settings are for 8 MHz clock which gives pwm clk = 125 KHz (8M/64)
Tccr1a = &HA3
Tccr1b = &H0B

'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.

'increment of setting will give (setting + 1)* 8 us pulse.  For a 30%
'duty cycle, .3 * 1020 = 307d = 133h.  That's (307+1) * 8 us = 2.45 ms pulse
'that repeats every 8.192 ms.

' Here the pwm signals are set for 0 "OFF" setting
' this actually gives a short pulse but so short that a motor won't energize
' since the duty cycle is less than 1%
Ocr1ah = &H0
Ocr1al = &H00

Ocr1bh = &H0
Ocr1bl = &H00

End Sub Pwm_mtr12_init


Sub Pwm_mtr1_set(byval Hi As Byte , Byval Lo 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.

'increment of setting will give (setting + 1)* 8 us pulse.  For a 30%
'duty cycle, .3 * 1020 = 307d = 133h.  That's (307+1) * 8 us = 2.45 ms pulse
'that repeats every 8.192 ms.

   Ocr1bh = Hi
   Ocr1bl = Lo

End Sub


Sub Pwm_mtr2_set(byval Hi As Byte , Byval Lo 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.

'increment of setting will give (setting + 1)* 8 us pulse.  For a 30%
'duty cycle, .3 * 1020 = 307d = 133h.  That's (307+1) * 8 us = 2.45 ms pulse
'that repeats every 8.192 ms.

   Ocr1ah = Hi
   Ocr1al = Lo

End Sub




Sub Pwm_mtr3_init:
' set bit 7 for output
Ddrd = Ddrd Or &H80

' set timer 2 for pwm output, prescaler 64
' these settings are for 8 MHz clock
Tccr2 = &H6C

'OCR2 is an 8 bit value that sets the on time or duty cycle.
'Pulse width = (setting + 1) * 130 us.  So a setting of
'0 will give a 130 us pulse.  Setting = 1 gives a 260 us
'pulse.  A setting of 10 will give about 1.5 ms pulse.
'The OFF setting is &HFF.

Ocr2 = &H00
End Sub Pwm_mtr3_init


Sub Pwm_mtr3_set(setting As Byte)
'OCR0 is an 8 bit register that sets the on time or duty
'cycle for servos 4. The setting is an 8 bit value.

   Ocr2 = Setting
End Sub Pwm_mtr3_set(setting As Byte)



' DC motor control functions for the mars rover using the DARC Motor Contorl
' board.  For this implentation there are 2 direction lines, Dir1 and Dir2 and
' each motor has 2 lines (so Dirleft1, Dirleft2, etc).  There are two dir lines
' used in order to take advantage of the quick stop feature.  If only one dir
' line was used the motors could be powered off but inertia would carry the bot
' forward a bit.  The pwm subroutines control the enable line for each motor and
' allow speed control of the dc motors.
Sub Forward()
  Dirleft1 = 1
  Dirleft2 = 0
  Dirright1 = 0
  Dirright2 = 1
  Call Pwm_mtr1_set(fast1 , Fast2)                          'start left motor
  Call Pwm_mtr2_set(fast1 , Fast2)                          'start right motorEnd Sub
End Sub

Sub Backward()
  Dirleft1 = 0
  Dirleft2 = 1
  Dirright1 = 1
  Dirright2 = 0
  Call Pwm_mtr1_set(fast1 , Fast2)                          'start left motor
  Call Pwm_mtr2_set(fast1 , Fast2)                          'start right motor
End Sub

Sub Rightturn()
   Dirleft1 = 1
  Dirleft2 = 0
  Dirright1 = 1
  Dirright2 = 0
  Call Pwm_mtr1_set(fast1 , Fast2)                          'start left motor
  Call Pwm_mtr2_set(fast1 , Fast2)                          'start right motor

End Sub


Sub Leftturn()
  Dirleft1 = 0
  Dirleft2 = 1
  Dirright1 = 0
  Dirright2 = 1
  Call Pwm_mtr1_set(fast1 , Fast2)                          'start left motor
  Call Pwm_mtr2_set(fast1 , Fast2)                          'start right motor

End Sub

Sub Stopmotors()
  ' do dir settings for fast stop on the L298
  Dirleft1 = 0
  Dirleft2 = 0
  Dirright1 = 0
  Dirright2 = 0

  Call Pwm_mtr1_set(fast1 , Fast2)                          'stop left motor
  Call Pwm_mtr2_set(fast1 , Fast2)                          'stop right motor
End Sub



' Turret motor contorl.  similar to the drive motor control but the pwm control
' is used more in order to ramp up the pwm signal to enable line.  this is done
' because if the turret motor is just pulsed with a full on signal it results in
' very jerky operation.  The ramp function gives smooth control of turret.
Sub Turretright()
   Dim Pwm3set As Byte
   Dirturret1 = 0
   Dirturret2 = 1
   For Pwm3set = 20 To 170 Step 10
     Call Pwm_mtr3_set(pwm3set)                             'start motor
     Waitms 50
   Next
End Sub

Sub Turretleft()
   Dirturret1 = 1
   Dirturret2 = 0
   For Pwm3set = 20 To 170 Step 10
     Call Pwm_mtr3_set(pwm3set)                             'start motor
     Waitms 50
   Next
End Sub

Sub Turretstop()
    Dirturret1 = 0
    Dirturret2 = 0
    Call Pwm_mtr3_set(0)                                    'stop motor
End Sub



' subroutine to initialize the adc module
Sub Init_adc
   ' 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 Init_adc


' function to get adc value and return it
Function Get_darc_adc(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

   Get_darc_adc = Adcval

End Function Get_darc_adc(byval Channel As Byte)


Sub Srf04_init
   Ddrb.2 = 0
   Ddra.6 = 1
   Portb.2 = 1                                              'set pull up on pin b2
   'If Servos12_active = 0 Then Call Servos12_init
End Sub Srf04_init

Sub Srf04_get_distance
   Porta.6 = 0
   Waitms 1
   Porta.6 = 1
   Waitms 1

   Porta.6 = 0
   Starttime = Counter1
   Waitus 210

   While Pinb.2 = 1
   Wend
   Endtime = Counter1
   Porta.6 = 0

   If Endtime > Starttime Then Sonartime = Endtime - Starttime
   If Starttime > Endtime Then
      Sonartime = &H3FF - Starttime
      Sonartime = Sonartime + Endtime
   End If

   Dist = Sonartime * 1.09
   Dist = Dist / 2

   If Dist < 30 Then Dist = Dist - 2
   Waitms 50
End Sub Srf04_get_range