Image Hosted by

  Robotic Madness


Copy and paste this code into your stamp editor

'{$PBASIC 2.5}

'This program is set to use only 5 of the LFM sensors
'They are placed at: 10100100101
' 0 0
'The jumper set to 5

LMotor CON 13 'Left servo pin
RMotor CON 12 'Right servo pin

LEDon CON 0 'Constant to turn LED on
LEDoff CON 1 'Constant to turn LED off

Wline CON 0 'Use to sense white lines on black
Bline CON 1 'Use to sense black lines on white
LFmode CON Wline 'Set which line type

MStop CON 750 'Basic Speed setting for servos, see the CBA manual
Speed100L CON 870 'Replace these values with the ones you found in testing
Speed075L CON 831
Speed050L CON 815
Speed025L CON 804
Speed010L CON 793
Speed100R CON 500
Speed075R CON 650
Speed050R CON 690
Speed025R CON 705
Speed010R CON 720

PulseWidthRt VAR Word 'Variables for pulse widths to servos
PulseWidthLt VAR Word

LEDPos VAR Nib 'The rest of the variables
LFMBits VAR Byte
lastBits VAR Byte
steer VAR Nib

Initialize: 'Initialize pins for LF module, P0 is input, P1 - P5 output
OUTL = %00111110
DIRL = %00111110
LastBits = %00100 'LastBits keeps track of where the line was last, initialize to center

PAUSE 2000 'Pause at start of program to allow time to set robot down

'*** This is the start of the main program loop ***
GOSUB Read_LFM 'Just as it sounds, read the LF module to see where line is

IF LFMBits = 0 THEN 'Off the line is a special case. This changes the LF reading to think
LFMBits = lastBits 'it was still on the line in the state it was just before it went off the line.
ENDIF 'That way it keep turning in the correct direction, while still looking for the line.

steer = NCD LFMBits 'Looks to see which sensor the line is under
'NCD takes the highest bit set in the variable lfBits and sets steer to its position.
'as example %00001000 will set steer to 4

'In the branch below we go to the pulse that is needed based on where the line is
BRANCH Steer - 1, [Hard_left, left, Straight, Right, Hard_Right] 'Minus 1 because Branch starts at 0
BackFromBranch: 'Branch has to come back to a label

PULSOUT Lmotor, PulseWidthLt 'Send pulse to left
PULSOUT Rmotor, PulseWidthRt 'Send pulse to right
lastBits = LFMBits 'Save the LF reading so we can use it later if needed

'---- Subs-----
LFMBits = 0 'Clear last reading
FOR LedPos = 1 TO 5 'Setup to loop through each sensor LED read only 5 sensors
OUTL.LOWBIT(ledPos) = LEDon 'Turn LED on
PAUSE 1 'Pause so sensor can respond
LFMBits.LOWBIT(ledPos) = IN0 ^ LFmode 'Read state of sensor, save into the bit position of that sensor
OUTL = OUTL | %01111110 'Turn off any and all sensor LEDs
LFMBits = LFMBits >>1 'When done with reading shift bits right one because
RETURN ' sensors start at P1 not P0

'* The 5 posabilities of steering are here*
PulseWidthLt = Speed100L
PulseWidthRt = Speed010R
GOTO BackFromBranch

PulseWidthLt = Speed100L
PulseWidthRt = Speed025R
GOTO BackFromBranch

PulseWidthLt = Speed100L
PulseWidthRt = Speed100R
GOTO BackFromBranch

PulseWidthLt = Speed025L
PulseWidthRt = Speed100R
GOTO BackFromBranch

PulseWidthLt = Speed010L
PulseWidthRt = Speed100R
GOTO BackFromBranch




  Today, there have been 5 visitors (103 hits) on this page! Copyright 2007  
=> Do you also want a homepage for free? Then click here! <=
Welcome visitor
free counters