' {$STAMP BS2}
' {$PBASIC 2.5}
'
'     REV 1.0
'     MODIFIED 11/2019
'     CODE WRITTEN BY DAVE LANTIS with opensource patches
'
'
'3-AXIS bot-arm
'
'
'*****************************************************************************
'               VARIABLES N DEFINITIONS
'*****************************************************************************

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

HB25            PIN     10              ' speed I/O Pin For HB-25
HB25PWR         PIN      9              ' HB25 pwr on/off
BckswA          PIN      3              ' MotorA back
FwdswA          PIN      4              ' MotorA forward
BckswB          PIN      5              ' MotorB back
FwdswB          PIN      6              ' MotorB forward

' -----[ Variables ]-------------------------------------------------------
MotorA          VAR     Byte            '1st motor
EncoderA        VAR     Word            '1st motor encoder
MotorB          VAR     Byte            '2nd motor
EncoderB        VAR     Word            '2nd motor encoder

MotorC          VAR     Byte            '3rd motor
EncoderC        VAR     Word            '3rd motor encoder
MotorD          VAR     Byte            '4th motor
EncoderD        VAR     Word            '4th motor encoder


index           VAR     Byte            ' Counter For Ramping motors
leftoffset      VAR     Byte            ' motor tuning variables
rightoffset     VAR     Byte            '

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

HIGH HB25pwr                            ' powerup hb25's
PAUSE 20
LOW motorC                              ' condition motorC servo
PAUSE 20

'--------------------------------------------------------------------------
'                   POWERUP MOTOR CONTROLLERS
'--------------------------------------------------------------------------
   '********motors r listed A-D, lowest to highest physically*********



DO : LOOP UNTIL HB25 = 1                ' Wait For HB-25 Power Up
LOW HB25                                ' Make I/O Pin Output/Low
PAUSE 5                                 ' Wait For HB-25 To Initialize
PULSOUT HB25, 750                       ' Stop Motor 1
PAUSE 1                                 ' 1 mS Delay
PULSOUT HB25, 750                       ' Stop Motor 2 (If Connected)
leftoffset = 0
rightoffset = 0
'
'-------------------------------------------------------------------------
'TESTING
' 1.  move each axis, adjust/develop code for fwd/rev n speed
' 2.  verify encoder ticks/rev and inc pulse/rev
' 3.  verify estop switches and home switches - establish variables etc.
' 4.  create HOME position including basic operating speed for each motor
'-------------------------------------------------------------------------