go_away

Author Topic: Simple obstacle avoidance code for sumobot? (Basic stamp)  (Read 4789 times)

0 Members and 1 Guest are viewing this topic.

Offline izaktjTopic starter

  • Robot Overlord
  • ****
  • Posts: 216
  • Helpful? 0
Simple obstacle avoidance code for sumobot? (Basic stamp)
« on: June 29, 2009, 09:33:56 PM »
I'm trying to do some programming but I've never programmed with Basice stamp.
The idea is known an simple:

If (right_detect = 1 & left_detect = 0)
{
     Turn left
}

If (right_detect = 1 & left_detect = 1)
{
     Go backwards 2 seconds    
     Turn left
}

If (right_detect = 0 & left_detect = 1)
{
     Turn right
}

If (right_detect = 0 & left_detect = 0)
{
     Keep going forward
}

But once in Basic stamp, I don't seem to figure out what is what.
For instance, here is a generic code for the sumobot (test)
Code: [Select]
' SumoBot-4.2-IR-Scan.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}

' -----[ I/O Definitions ]-------------------------------------------------

LMotor       PIN        13                      ' left servo motor
RMotor       PIN        12                      ' right servo motor

LfIrOut      PIN        4                       ' left IR LED output
LfIrIn       PIN        11                      ' left IR sensor input
RtIrOut      PIN        15                      ' right IR LED output
RtIrIn       PIN        14                      ' right IR sensor input

' -----[ Constants ]-------------------------------------------------------

LFwdFast     CON        1000                    ' left motor fwd; fast
LFwdSlow     CON        800                     ' left motor fwd; slow
LStop        CON        750                     ' left motor stop
LRevSlow     CON        700                     ' left motor rev; slow
LRevFast     CON        500                     ' left motor rev; fast

RFwdFast     CON        500                     ' right motor fwd; fast
RFwdSlow     CON        700                     ' right motor fwd; slow
RStop        CON        750                     ' right motor stop
RRevSlow     CON        800                     ' right motor rev; slow
RRevFast     CON        1000                    ' right motor rev; fast

' -----[ Variables ]-------------------------------------------------------

irBits       VAR        Nib                     ' storage for IR target data
irLeft       VAR        irBits.BIT1
irRight      VAR        irBits.BIT0
lastIr       VAR        Nib                     ' info from last reading
pulses       VAR        Byte                    ' counter for motor control

' -----[ Initialization ]--------------------------------------------------

Reset:
  LOW LMotor                                    ' initialize motor outputs
  LOW RMotor

' -----[ Program Code ]----------------------------------------------------

Main:
  GOSUB Read_IR_Sensors
  BRANCH irBits, [Scan, Follow_Right, Follow_Left, Hold]

Scan:
  BRANCH lastIR, [Move_Fwd, Scan_Right, Scan_Left, Move_Fwd]

Move_Fwd:
  DEBUG HOME, "Forward", CLREOL
  GOTO Main

Scan_Right:                                     ' spin right, slow
  DEBUG HOME, "Scan Right", CLREOL
  PULSOUT LMotor, LFwdSlow
  PULSOUT RMotor, RRevSlow
  PAUSE 20
  GOTO Main

Scan_Left:                                      ' spin left, slow
  DEBUG HOME, "Scan Left", CLREOL
  PULSOUT LMotor, LRevSlow
  PULSOUT RMotor, RFwdSlow
  PAUSE 20
  GOTO Main

Follow_Right:                                   ' spin right, fast
  DEBUG HOME, "Follow Right", CLREOL
  PULSOUT LMotor, LFwdFast
  PULSOUT RMotor, RRevFast
  PAUSE 20
  lastIr = irBits                               ' save last direction found
  GOTO Main

Follow_Left:                                    ' spin left, fast
  DEBUG HOME, "Follow Left", CLREOL
  PULSOUT LMotor, LRevFast
  PULSOUT RMotor, RFwdFast
  PAUSE 20
  lastIr = irBits
  GOTO Main

Hold:                                           ' on target
  DEBUG HOME, "On Target", CLREOL
  FOR pulses = 1 TO 3
    PULSOUT LMotor, LStop
    PULSOUT RMotor, RStop
    PAUSE 20
  NEXT
  lastIr = %00
  GOTO Main

  END

' -----[ Subroutines ]-----------------------------------------------------

Read_IR_Sensors:
  FREQOUT LfIrOut, 1, 38500                     ' modulate left IR LED
  irLeft = ~LfIrIn                              ' read input (1 = target)
  FREQOUT RtIrOut, 1, 38500                     ' modulate right IR LED
  irRight = ~RtIrIn                             ' read input (1 = target)
  RETURN

« Last Edit: June 29, 2009, 09:35:46 PM by izaktj »

 


Get Your Ad Here

data_list