We are having a problem interfacing with a CMPS03. The scorpion motor controller is connected to the atom bot board. We have made the following program that makes 90 degree turns. Whenever our robot gets to an angle of 123, our program stops sending motor pulses. Upon initial inspection, I have a feeling our problem has to do with the stack messing up somewhere. Is there something we are overlooking when designing this? Any help is appreciated.
init_ang var word
current_ang var word
next_ang var word
ang_diff var long
LEFT_CCW var word
RIGHT_CCW var word
NEUTRAL var word
TURN_THRESH var word
TURN_THRESH = 5 'Threshold used for angle comparisons
LEFT_CCW = 1700
RIGHT_CCW = 1700
NEUTRAL = 1500
low p0
low p1
pause 5000
main:
gosub initangle 'read initial angle
check: 'the angle needs to be read inside this loop
gosub readnextang
serout S_OUT,i57600,"Direction ", DEC (next_ang), 13]
pulsout 0, LEFT_CCW 'this is add so the pulsout is done inside a loop
pulsout 1, RIGHT_CCW
ang_diff = next_ang - current_ang
If ( ang_diff < 0 ) Then
ang_diff = ang_diff + 360
Endif
serout S_OUT,i57600,"Diff ", DEC (ang_diff), 13]
If (( ang_diff > ( 90 - TURN_THRESH )) AND ( ang_diff < ( TURN_THRESH + 90 ))) Then
pulsout 0, (NEUTRAL)
pulsout 1, (NEUTRAL)
end
Endif
goto check
initangle:
pulsin p9, 1, init_ang 'Read angle
if (init_ang = 0) Then
goto initangle
endif
init_ang = init_ang/100
current_ang = init_ang
return
readnextang:
pulsin p9, 1, next_ang 'Read angle
if (next_ang = 0) Then
goto readnextang
endif
next_ang = next_ang/100
return