Hey guys. I’m trying to create subroutines to control the AL5A Lynxmotion arm connected via SSC-32 to the Bot Board 2 and Basic Atom Pro. I’d like to be able to press a button (via bluetooth, which works with other code), and have the arm move into a preset position.
I’ve successfully written code to move the arm’s base to a new set location, from wherever its current location is. I’ve now tried to convert this into a function that can move all servos on the arm one after another. I decided to do this using a ‘for’ loop, so it iterates through the different servos. An array is also used to store current positions and destinations at the beginning of the function.
However, it doesn’t work! When I try to execute the function, nothing appears to happen at all. It’s as if it can’t find it or something? I really thought this code would work and have been hitting my head against the wall a bit 
. I tried to SEROUT to Console 1 at the beginning of the function, but that didn’t work either. Here’s all the relevant code:
[code]
; SSC32 Settings
cSSC_OUT con P8 ;Output pin for (SSC32 RX) on BotBoard (Yellow)
cSSC_BAUD con i38400 ;SSC32 BAUD rate 38400 115200
'SSC-32 Connections
PAN con 0
TILT con 1
GRIP_UD con 2
GRIP_Rot con 3
GRIP_OC con 4
ELBOWTILT con 5
'Bluetooth
BT_Input var word
ENABLEHSERIAL
SETHSERIAL1 H9600, H8DATABITS, HNOPARITY, H1STOPBITS
destinationarray var word(6)
positionarray var word(6)
armdestination var word
armposition var word
armiterator var word
routine_mode var byte ’ 0 - None = 1 = NEW
RoutinePause var word
StepSize var word
RemainderSteps var word
PanDestination var word
TiltDestination var word
GripperVertDestination var word
GripperTurnDestination var word
GripperGripDestination var word
ElbowDestination var word
PanPosition var word
TiltPosition var word
GripperVertPosition var word
GripperTurnPosition var word
GripperGripPosition var word
ElbowPosition var word
'Auto Routine Config
StepSize = 200
RoutinePause = 100
'Lifter Config (start servos at mid position)
PanPosition = 1500
TiltPosition = 1500
GripperVertPosition = 1500
GripperTurnPosition = 1500
GripperGripPosition = 1500
ElbowPosition = 1500
'Ensure pulsout commands are positive going.
low PAN
low TILT
low GRIP_UD
low GRIP_Rot
low GRIP_OC
low ELBOWTILT
main:
pause 20
HSERIN noinput,20,[BT_Input]
if (BT_Input = "t") then
gosub armroutine[1500,1500,1500,1500,1500,1500]
endif
goto main
pulsearm:
'Send out the servo pulses
serout cSSC_OUT, cSSC_BAUD, “#”, dec PAN, “P”, dec PanPosition, |
“#”, dec TILT, “P”, dec TiltPosition, |
“#”, dec GRIP_UD, “P”, dec GripperVertPosition, |
“#”, dec GRIP_Rot, “P”, dec GripperTurnPosition, |
“#”, dec GRIP_OC, “P”, dec GripperGripPosition, |
“#”, dec ELBOWTILT, “P”, dec ElbowPosition, |
“#”, dec HEAVYLIFT, “P”, dec HeavyLiftPosition, 13]
pause RoutinePause
return
armroutine [PanDestination,TiltDestination,ElbowDestination,GripperVertDestination,GripperTurnDestination,GripperGripDestination]
destinationarray(0) = PanDestination
destinationarray(1) = TiltDestination
destinationarray(2) = ElbowDestination
destinationarray(3) = GripperVertDestination
destinationarray(4) = GripperTurnDestination
destinationarray(5) = GripperGripDestination
positionarray(0) = PanPosition
positionarray(1) = TiltPosition
positionarray(2) = ElbowPosition
positionarray(3) = GripperVertPosition
positionarray(4) = GripperTurnPosition
positionarray(5) = GripperGripPosition
armiterator = 0
for armiterator = 0 to 5
armdestination = destinationarray(armiterator)
armposition = positionarray(armiterator)
routine_mode = 1
if (armposition > armdestination) then
RemainderSteps = armposition - armdestination
while (RemainderSteps > StepSize)
RemainderSteps = RemainderSteps - StepSize
armposition = (armposition - StepSize)
gosub pulsearm
wend
armposition = (armposition - RemainderSteps)
gosub pulsearm
elseif (armposition < armdestination)
RemainderSteps = armdestination - armposition
while (RemainderSteps > StepSize)
RemainderSteps = RemainderSteps - StepSize
armposition = (armposition + StepSize)
gosub pulsearm
wend
armposition = (armposition + RemainderSteps)
gosub pulsearm
endif
next
routine_mode = 0
goto main[/code]
Would much appreciate any insight you may have!