;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,l_mot_port,r_mot_port 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
  if l_mot_dir=forward then
    l_mot_pos++
    if l_mot_pos=5 then l_mot_pos=1
  else
    l_mot_pos---
    if l_mot_pos=0 then l_mot_pos=4
  end if
  readtable l_mot_phase,l_mot_pos,l_mot_port
else
  l_mot_port=0b11110000
end if
;right motor
if r_mot_dir<>stop then
  if r_mot_dir=forward then
    r_mot_pos++
    if r_mot_pos=5 then r_mot_pos=1
  else
    r_mot_pos---
    if r_mot_pos=0 then r_mot_pos=4
  end if
 readtable r_mot_phase,r_mot_pos,r_mot_port
else
  r_mot_port=0b00001111
end if
;out left right phase to port
portd=l_mot_port and r_mot_port
end sub
;
table l_mot_phase
0b11110011,0b11110110,0b11111100,0b11111001
end table
table r_mot_phase
0b00111111,0b01101111,0b11001111,0b10011111
end table
;-------------------
sub servo
pulseout servopin,servopos us
end sub

Clearscreen

For x1 = 1 To 400
    drawbox x1,10,400,400
Next x1

End

Sub drawbox(x1,y1,x2,y2)
 Print Chr$(27)+"[Z2;"+Str$(x1)+";"+Str$(y1)+";"+Str$(x2)+";"+Str$(y2)+"Z"
 Pause 30
 Clearscreen
End Sub

Sub Clearscreen
 Print Chr$(27)+"[2J"+Chr$(27)+"[H"
End Sub

;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

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

/CODE]
Edited 2022-09-03 07:35 by stanleyella