stan cartwright - 2020-06-19

I wanted to try using V53L0X laser range finder but looked complicated.
I wanted to use 28byj-48 stepper motors but they need to be pulsed at 2ms for max torque/speed.
I sorted it for mega328. https://www.youtube.com/watch?v=Y_ai0tc_rek
Lots of code for your own projects like using V53L0X. using stepper motors driven by interrupt, using one interrupt for 2 events like running the motors and servo. Hope there's something useful.

;fullwave stepper V53L0X robot
#chip mega328p,16
#option Explicit

dir portc.0 out ;servo
dir portd.0 out:dir portd.1 out:dir portd.2 out:dir portd.3 out ;left motor
dir portd.4 out:dir portd.5 out:dir portd.6 out:dir portd.7 out ;right motor

;set up V53L0X
#define HI2C_DATA PORTC.5 ;sda portc.4 for nano portc.5 for uno
#define HI2C_CLOCK PORTC.4 ;scl portc.5 for nano portc.4 for uno
#define HI2C_BAUD_RATE 400
HI2CMode Master
;
;v53l0x software restart
HI2CStart
HI2CSend(0x52)
HI2CSend(0x89)
HI2CSend(0x01)
HI2CStop
wait 200 ms
;
dim l_mot_pos,r_mot_pos as byte
dim l_mot_dir,r_mot_dir as byte
dim servopos as Word
dim servodir,distance_lo,distance_hi,mindist,last_turn as Byte
;
#define distance distance_hi*256 + distance_lo ;v53l0x distance
#define forward 1   ;|
#define reverse 255 ;|--motor direction
#define stop 0      ;|
#define servopin portc.0
#define wasleft 1
#define wasright 0
;
last_turn = random and 1 ;0 or 1
servopos=1500 ;1.5ms servo mid position
servodir=1 ;servo rotation direction
mindist=200 ;minimum distance before avoiding
l_mot_pos=1:r_mot_pos=1
l_mot_dir=forward:r_mot_dir=forward
;-----------
;set up interrupts
dim mills as byte : mills = 0
    On Interrupt Timer0Match1 Call isr
    Dim OCR0  AS byte alias OCR0A
    Dim TCCR0 AS  byte alias TCCR0B
    WGM01 = 1
    OCR0 = 124 ;2ms
    TCCR0 = 0x28
    TCCR0 = TCCR0 or 0x04
;-----------
do ;main program
;check for objects and avoid
  getdistance ;any object too close?
  if distance <> 20 then ;20 means out of range so skip distance test
;
;distance to obstacle test
    if servopos > 1300 then ;not pointing right
      if servopos < 1700 then ;not pointing left
        mindist = 200 ;pointing forward min distance
      end if
    else
      mindist = 250 ; left and right min distance
    end if
;
    if distance < mindist then ;object close
      l_mot_dir=stop:r_mot_dir=stop ;stop motors
;
      if servopos > 1700 then
        turnright_until_clear ;object left
        last_turn = wasright
      else if servopos < 1300 then
        turnleft_until_clear ;object right
        last_turn = wasleft
      else ;obstacle forward
        l_mot_dir=reverse : r_mot_dir=reverse : wait 500 ms ;reverse 500ms
        if last_turn = wasright then ;turn right again
          servodir=1
          turnright_until_clear
        else ;turn left again
          servodir=0
          turnleft_until_clear
        end if
      end if
      l_mot_dir=forward:r_mot_dir=forward ;go forward
    end if
  end if
;
;rotate servo left-right
  if servodir = 1 then
    servopos = servopos +80
    if servopos = 2220 then
      servodir = 0
    end if
  else
    servopos = servopos - 80
    if servopos = 780 then
      servodir = 1
    end if
  end if
;  pulseout servopin,servopos us
  wait 30 ms 'to allow servo to move slower and not overheat and getdistance to work
loop
;end of main
;-------------------
sub isr
  motors ;every 2ms
  mills = mills + 2
  if mills = 20 then mills=0 : servo ;every 20ms
end sub
;--------------------
sub getdistance
  HI2CStart ;Sys V53L0X Range Start
  HI2CSend(0x52)
  HI2CSend(0x00)
  HI2CSend(0x01)
  HI2CStop

  HI2CStart ;read distance
  HI2CSend(0x52)
  HI2CSend(0x1e)
  HI2CReStart
  HI2CSend(0x53) ;set the read flag
  HI2CReceive(distance_hi)
  HI2CReceive(distance_lo, NACK) ;read one byte and conclude
  HI2CStop
end sub
;-------------------
sub turnleft_until_clear
  do
   l_mot_dir=reverse:r_mot_dir=forward ;rotate anti clockwise
   wait 20 ms
   l_mot_dir=stop:r_mot_dir=stop
   getdistance
  loop until distance > mindist ;rotate left until clear
end sub
;-------------------
sub turnright_until_clear
  do
   l_mot_dir=forward:r_mot_dir=reverse ;rotate clockwise
   wait 20 ms
   l_mot_dir=stop:r_mot_dir=stop
   getdistance
  loop until distance > mindist ;rotate right until clear
end sub
;-------------------
sub motors
;left motor
  if l_mot_dir=stop then
    set portd.0 off:set portd.1 off:set portd.2 off:set portd.3 off
  else if l_mot_dir=forward then
    l_mot_pos++
    if l_mot_pos=5 then
      l_mot_pos=1
    end if
  else
    l_mot_pos---
    if l_mot_pos=0 then
      l_mot_pos=4
    end if
  end if
;
  select case l_mot_pos
    case 4
      set portd.0 on:set portd.1 on:set portd.2 off:set portd.3 off
    case 3
      set portd.0 off:set portd.1 on:set portd.2 on:set portd.3 off
    case 2
      set portd.0 off:set portd.1 off:set portd.2 on:set portd.3 on
    case 1
      set portd.0 on:set portd.1 off:set portd.2 off:set portd.3 on

  end select
;
;right motor
  if r_mot_dir=stop then
    set portd.4 off:set portd.5 off:set portd.6 off:set portd.7 off
  else if r_mot_dir=forward then
    r_mot_pos++
    if r_mot_pos=5 then
      r_mot_pos=1
    end if
  else
    r_mot_pos---
    if r_mot_pos=0 then
      r_mot_pos=4
    end if
  end if
;
 select case r_mot_pos
  case 4
    set portd.4 on:set portd.5 off:set portd.6 off:set portd.7 on
  case 3
    set portd.4 off:set portd.5 off:set portd.6 on:set portd.7 on
  case 2
    set portd.4 off:set portd.5 on:set portd.6 on:set portd.7 off
  case 1
    set portd.4 on:set portd.5 on:set portd.6 off:set portd.7 off
 end select
end sub
;-------------------
sub servo
  pulseout servopin,servopos us
end sub
 

Last edit: stan cartwright 2020-06-23