New to programming, Object avoidance bot. How's my programming look??? Thanks!

Josheb800 · 2011-12-04 20:02

Alright, it is for a BOE bot for object avoidance.
Using:
BS2
2 motors (not servos),
1 servo w/ PING))) sensor
I won't have an opportunity to actually test the code until tomorrow. I'm sure something will be wrong with it though. I would appreciate it if anyone with expirience could read through it and let me know why I might have any problems. Thanks!!

'Mad Max is about to blow something up - MadMax.bs2
' {$STAMP BS2}
' {$PBASIC 2.5}


'Definitions-----------------------------------------------------------------


DEBUG "I'm bad company."


'I/O Pin Definitions
LeftMotor                 PIN 0
RightMotor                PIN 1
Servo                     PIN 14
Ping                      PIN 15
Audio                     PIN 7
Speakers                  PIN 8


'Constant/Variable Declarations
time VAR Word                      'Variables - name/VAR/size, word (0-65535)
rawDist VAR Word                   'bit(0-1), Nib(0-15), Byte, (0-255)
counter VAR Word
inches VAR Word
leftvalue VAR Word
rightvalue VAR Word
RawToIn CON 889
RawToCM CON 2257
Trigger CON 5
Scale CON $200
IsHigh CON 1
IsLow CON 0


'Motor Initialization
PAUSE 2000
LOW LeftMotor                'Initalize motor I/O pins to output low
LOW RightMotor
HIGH LeftMotor
HIGH RightMotor
PAUSE 100                    'Let the motor circuits wake up
FOR counter = 1 TO 10        'Servo center
PULSOUT Servo, 720
PAUSE 20
NEXT


'MAIN------------------------------------------------------------------------


DO


GOSUB Get_Sonar


IF (inches < 8) THEN           'When obstacle is less than 8 inches away:
PULSOUT LeftMotor, 2000       'motors stop
PULSOUT RightMotor, 2000
PAUSE 500
GOSUB Servo_Look


  IF (leftvalue < rightvalue) THEN
GOSUB Turn_Right
  ELSEIF (leftvalue > rightvalue) THEN
GOSUB Turn_Left
  ELSEIF (leftvalue + rightvalue < 5) THEN
GOSUB Turn_Around
  ELSE
GOSUB Turn_Around
  ENDIF


ELSE
PULSOUT LeftMotor, 3000                     'Left motor forward full speed
PULSOUT RightMotor, 3000                    'Right motor forward full speed
ENDIF


LOOP


'SUBROUTINES------------------------------------------------------------------
'-----------SONAR-------------------------------------------------------------
Get_Sonar:                              'Gets sonar from straight ahead
Ping = IsLow
PULSOUT Ping, Trigger
PULSIN Ping, IsHigh, rawDist
rawDist = rawDist */ Scale
rawDist = rawDist / 2
inches = rawDist ** RawToIn
RETURN


Get_Sonar_Left:                        'Gets sonar from left
Ping = IsLow
PULSOUT Ping, Trigger
PULSIN Ping, IsHigh, rawDist
rawDist = rawDist */ Scale
rawDist = rawDist / 2
leftvalue = rawDist ** RawToIn
RETURN


Get_Sonar_Right:                       'gets sonar from right
Ping = IsLow
PULSOUT Ping, Trigger
PULSIN  Ping, IsHigh, rawDist
rawDist = rawDist */ Scale
rawDist = rawDist / 2
rightvalue = rawDist ** RawToIn
RETURN


'-------------------------SERVO MOVEMENT----------------------------------------
Servo_Look:
FOR counter = 1 TO 50                  'Servo left
PULSOUT Servo, 1250
PAUSE 500
NEXT
GOSUB Get_Sonar_Left                   'get left distance
FOR counter = 1 TO 50                  'Servo right
PULSOUT Servo, 300
PAUSE 500
NEXT
GOSUB Get_Sonar_Right                  'get right distance
FOR counter = 1 TO 50                  'Servo center
PULSOUT Servo, 720
PAUSE 500
NEXT
RETURN


'-------------------------------------------MOTOR TURNS------------------------
'Assuming 1000 turns motor full speed backward, 2000 makes motor stop, 3000 full
'speed forward.  Adjust accordingly to align motors.


Turn_Left:
PULSOUT RightMotor, 2500                   'Right motor forward
PULSOUT LeftMotor, 1500                    'left motor reverse
PAUSE 2000
RETURN


Turn_Right:
PULSOUT RightMotor, 1500                   'Right motor reverse
PULSOUT LeftMotor, 2500                    'left motor forward
PAUSE 2000
RETURN


Turn_Around:
PULSOUT RightMotor, 1500                   'right motor reverse
PULSOUT LeftMotor, 1500                    'left motor reverse
PAUSE 1000
PULSOUT RightMotor, 1500                   'right motor reverse
PULSOUT LeftMotor, 2500                    'left motor forward
PAUSE 2000
RETURN

Again, Thank you! I don't exactly know what I'm doing yet.

Franklin · 2011-12-04 21:03

Depending on the motors they probably will not work directly from the stamp pins and without current limiting resistors you may damage the stamp. Make sure you understand what you are doing before connecting power. It would help if we knew how you had things connected.

Josheb800 · 2011-12-04 21:30

The motors are wired from programmable pins to transistors. This is the reason I cannot test the code tonight, because I need to put resistors on the board in between the programmable pin and the transistor, which I do not have here.

How do you go about programming the motors if not from the stamp pins?

New to programming, Object avoidance bot. How's my programming look??? Thanks!

Comments