Electronic Speed Control

[Raisin-Toe]

Here is the car in action for anyone who wants to see what my rediculouse mess of code does.

http://www.youtube.com/watch?v=Bl1rAJczyXg

If I get all the bugs worked out, maybe I will organize the code to where we can read it. :P

I should have a PIR sensor added to the car so it can tell if it's stuck.

This is a great educational project, I keep learnig of better ways to do things.






' {$STAMP BS1}
' {$PBASIC 1.0}
' -----[ I/O Definitions ]-------------------------------------------------

DIRS = %00000111 ' P0 - p2 are outputs, all others inputs

SYMBOL  Ping            = 6                      ' Ping Sensor

SYMBOL Motor            = 0                     ' Car Motor
SYMBOL steering         = 1                    ' Steering Servo
SYMBOL Servo            = 2                     ' Ping Servo
' -----[ Constants ]-------------------------------------------------------


SYMBOL  Trigger         = 1                     ' 10 uS trigger pulse
SYMBOL  Scale           = 10                    ' raw x 10.00 = uS

SYMBOL  RawToIn         = 889                   ' 1 / 73.746 (with **)
SYMBOL  RawToCm         = 2257                  ' 1 / 29.034 (with **)

SYMBOL  IsHigh          = 1                     ' for PULSOUT
SYMBOL  IsLow           = 0


' -----[ Variables ]-------------------------------------------------------
SYMBOL SteerPos         = B8
SYMBOL pos              = B9 ' servo position
SYMBOL delay            = B10
SYMBOL distance         = B11

SYMBOL  rawDist         = W1                    ' raw measurement
SYMBOL  inches          = W2
SYMBOL  inches_left     = W3

' -----[ Main Program ]----------------------------------------------------

Arm_motor:
  FOR delay = 1 TO 50                                                ' ESC requires time to comunicate to Prop-1 when first powered up
Arm_motorND:
  PULSOUT motor, 150
  PAUSE 19
  NEXT
  GOSUB Get_Sonar
  inches = rawDist ** RawToIn
  IF inches < 30 THEN C_Rev                                      ' Must Reverse to farther distance than the Stoping distance


Run_motor:
  PULSOUT motor, 175
  PAUSE 19
  GOSUB Get_Sonar
  inches = rawDist ** RawToIn
  IF inches < 25 THEN Stop_motor                              ' Stopping distance

  GOTO Run_Motor



Stop_Motor:
  FOR delay = 1 TO 25                                   ' Trying to use electric breaks, (didn't work)
  PULSOUT motor, 100                                   ' This ESC requires position 100 befor 150
  PAUSE 19
  NEXT
  GOTO Arm_motorND                                    ' go back to Arm_motor to simplify code

C_Rev:
  IF steerPos = 99 THEN Right
  GOSUB Turn_Left
  GOTO Rev

  Right:
  GOSUB Turn_Right
Rev:
  FOR delay = 1 TO 65                                   ' I don't remember why I have this delay
  PULSOUT motor, 130
  PAUSE 19
  NEXT                                                           ' Notice, program continues to _Steering



' -----[ Steering ]--------------------------------------------------------

' -----[ !Fix Error! Right = Left, and Left = Right ]----------------------------------

_Steering:
  GOSUB Check_Direction
  GOTO Run_Motor

Turn_Right:
  IF steerpos = 201 THEN RTN
  FOR Steerpos = 100 TO 200
  PULSOUT steering, steerpos
  PAUSE 19
  NEXT

RTN:
  RETURN

Turn_Left:
  IF steerpos = 99 THEN RTN
  FOR Steerpos = 200 TO 100 STEP -1
  PULSOUT steering, SteerPos
  PAUSE 19
  NEXT
  RETURN

' -----[ Direction servo ]-------------------------------------------------

Check_Direction:
  FOR pos = 150 TO 200                  ' Look Right

  PULSOUT servo, pos
  PAUSE 19

  NEXT

  GOSUB Get_Sonar
  inches_Left = rawDist ** RawToIn

  FOR pos = 200 TO 100 STEP -1

  PULSOUT servo, pos
  PAUSE 19
  NEXT



  GOSUB Get_Sonar
  inches = rawDist ** RawToIn

FOR pos = 100 TO 150

  PULSOUT servo, pos
  PAUSE 19
  NEXT

  IF inches >= inches_left THEN Sub_Right
  GOSUB Turn_Left
  RETURN

Sub_Right:
  GOSUB Turn_Right
  RETURN


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

Get_Sonar:
  LOW Ping                                      ' make trigger 0-1-0
  PULSOUT Ping, Trigger                         ' activate sensor
  PULSIN  Ping, IsHigh, rawDist                 ' measure echo pulse
  rawDist = rawDist * Scale                     ' convert to uS
  rawDist = rawDist / 2                         ' remove return trip

  RETURN