Lynxmotion Tech Support

' {$STAMP BS2}
' {$PBASIC 2.5}
righthip   CON 10                '| Servo Offset Finder        |'
rightknee   CON 8               '|    Written by James Frye    |'
rightankle   CON 7                '|                  |'
lefthip      CON 6                '| Use this program TO find     |'
leftknee   CON 5                '| the servo offsets FOR your    |'
leftankle   CON 4                '| Lynxmotion BRAT using an Atom Pro|'
                        '| AND a Bot Board!          |'
righthip_start CON 0              '|                  |'
rightknee_start CON 0              '+----------------------------------+'
rightankle_start CON 0               '|        How TO use      |'
lefthip_start CON 0               '|                  |'
leftknee_start CON 0               '| Press A TO decrease the servo   |'
leftankle_start CON 0               '|  offset by 5us          |'
                         '|                  |'
rhip  VAR Word                '| Press C TO increase the servo  |'
rknee  VAR Word                '|  offset by 5us          |'
rankle  VAR Word                '|                  |'
lhip  VAR Word                '| Press B TO change which servo is |'
lknee  VAR Word                '|  being manipulated, AND TO send  |'
lankle  VAR Word                '|  DATA back TO the terminal    |'
                         '|                  |'
rhip  = ((righthip_start / 24) + 750)    '| The servos are changed in this   |'
rknee  = ((rightknee_start / 24) + 750)    '|  order              |'
rankle  = ((rightankle_start / 24) + 750)     '|                  |'
lhip  = ((lefthip_start / 24) + 750)      '| Right Hip -> Right Knee -> Right  |'
lknee  = ((leftknee_start / 24) + 750)    '|    ^            Ankle  |'
lankle  = ((leftankle_start / 24) + 750)    '|    |            |  |'
                        '|    |            v  |'
 currentServo VAR Byte              '|    Send <- Left <- Left <- Left|'
 currentServo = 1                '|    DATA  Ankle  Knee   Hip  |'
                        '+----------------------------------+'
'BUTTON init
buttonA VAR Bit
buttonB VAR Bit
buttonC VAR Bit

prevA VAR Bit
prevB VAR Bit
prevC VAR Bit
INPUT 12
INPUT 13
INPUT 14

main:

 GOSUB pulsemove

 prevA = buttonA
 prevB = buttonB
 prevC = buttonC

 buttonA = IN12
 buttonB = IN13
 buttonC = IN14

 IF (buttonA = 0) AND (prevA = 1) THEN      'A wasn't already pressed the last time

  GOSUB decreaseOffset

  GOTO main
 ENDIF

 IF (buttonB = 0) AND (prevB = 1) THEN


  currentServo = currentServo + 1
  IF(currentServo = 7) THEN

      LOW 13

  ENDIF
  GOTO main
 ENDIF


 IF (buttonC = 0) AND (prevC = 1)  THEN


  GOSUB increaseOffset

  GOTO main
 ENDIF

GOTO main
increaseOffset:

 IF(currentServo = 1) AND (rhip < 800) THEN
  rhip = rhip + 5


  ELSEIF(currentServo = 2) AND (rknee < 800) THEN
   rknee = rknee + 5

   ELSEIF(currentServo = 3) AND (rankle < 800)THEN
    rankle = rankle + 5

    ELSEIF(currentServo = 4) AND (lhip < 800) THEN
     lhip = lhip + 5

     ELSEIF(currentServo = 5) AND (lknee < 800)   THEN
      lknee = lknee + 5

      ELSEIF(currentServo = 6) AND (lankle < 800)     THEN
       lankle = lankle + 5

       ELSE

 ENDIF
RETURN
   decreaseOffset:

 IF(currentServo = 1) AND (rhip > 700) THEN
  rhip = rhip - 5

  ELSEIF(currentServo = 2) AND (rknee > 700) THEN
   rknee = rknee - 5

   ELSEIF(currentServo = 3) AND (rankle > 700)THEN
    rankle = rankle - 5

    ELSEIF(currentServo = 4) AND (lhip > 700)  THEN
     lhip = lhip - 5

     ELSEIF(currentServo = 5) AND (lknee > 700)    THEN
      lknee = lknee - 5

      ELSEIF(currentServo = 6) AND (lankle > 700)      THEN
       lankle = lankle - 5

       ELSE

 ENDIF
RETURN
pulsemove:
 PULSOUT leftankle, (lankle)
 PULSOUT leftknee, (lknee)
 PULSOUT lefthip, (lhip)

 PULSOUT rightankle, (rankle)
 PULSOUT rightknee, (rknee)
 PULSOUT righthip, (rhip)
 PAUSE 20

RETURN


waitloop:

 GOSUB pulsemove

 buttonB = IN13

 IF (buttonB = 0) AND (prevB = 1) THEN
   currentServo = 1

   GOTO main
 ENDIF

 prevB = buttonB

GOTO waitloop