roboforum.ru

$regfile = "m48DEF.dat"
$crystal = 1000000

Config Pinc.0 = Input : Dleft Alias Pinc.0    'left sensor
Config Pinc.4 = Input : Dright Alias Pinc.4   'right sensor

Config Pinb.0 = Output : Dir_right Alias Portb.0
Config Pind.2 = Output : Dir_left Alias Portd.2

'---------------------------program------------------------------------------
Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down

Dim Turn_right_counter As Byte
Dim Turn_left_counter As Byte
Dim Speed_def As Byte
Dim Speed_min As Byte
Dim Speed_max As Byte
Const Dir_Forward = 0
Const Dir_Backward = 1
Const Pressed = 0
Const Turn_time = 500
Const Long_turn_time = 1000
Const Go_backward_time = 1000
Const Turn_limit_count = 30
Turn_right_counter = 0
Turn_left_counter = 0
Speed_def = 180
Speed_min = 100
Speed_max = 220
Dir_right = 0
Dir_left = 0
Do

   'decrement last turn action counters
   If Turn_right_counter > 0 Then
      Decr Turn_right_counter
   End If
   If Turn_left_counter > 0 Then
      Decr Turn_left_counter
   End If


   'if right sensor is pressed - turn left
   If Dright = Pressed Then
      'check if we just turned right
      If Turn_left_counter > 0 Then
         Gosub Go_backward
         Waitms Go_backward_time
         Gosub Turn_left
         Waitms Long_turn_time
      Else
         Gosub Turn_left
         Waitms Turn_time
      End If
      Gosub Go_forward
      Turn_right_counter = Turn_limit_count
      Turn_left_counter = 0
   End If

   'if left sensor is pressed - turn right
   If Dleft = Pressed Then
      'check if we just turned left
      If Turn_right_counter > 0 Then
         Gosub Go_backward
         Waitms Go_backward_time
         Gosub Turn_right
         Waitms Long_turn_time
      Else
         Gosub Turn_right
         Waitms Turn_time
      End If
      Gosub Go_forward
      Turn_right_counter = 0
      Turn_left_counter = Turn_limit_count
   End If

   Waitms 20
Loop


Go_forward:
   Dir_left = Dir_forward
   Dir_right = Dir_forward
   Pwm1b = Speed_def
   Pwm1a = Speed_def
Return

Go_backward:
   Dir_left = Dir_backward
   Dir_right = Dir_backward
   Pwm1b = Speed_def
   Pwm1a = Speed_def
Return

Turn_left:
   Dir_left = Dir_forward
   Dir_right = Dir_backward
   Pwm1a = Speed_max
   Pwm1b = Speed_min
Return

Turn_right:
   Dir_left = Dir_backward
   Dir_right = Dir_forward
   Pwm1a = Speed_min
   Pwm1b = Speed_max
Return