Controlling Servos

[NAVNEET ACHARYA]

Hi

I am trying to control 4 servos with the ISOPOD by reading in commands from the serial port. The data comes in a 6 bytes where the third byte tells me the servo id and the fourth one tells me in what direction and by how much I shud rotate the servo. Now the problem I have is the program seems to be missing out on data. I mean, it recognizes one command i.e. interprets one packet of 6 bytes properly, but loses out on the second packet. The program sometimes gives correct response to around 6/7 packets but there is no consistency. Please look at the code and tell me the problem:

DECIMAL 9600 SCI0 BAUD

VARIABLE BUFR DECIMAL 40 4 + ALLOT EEWORD
BUFR 40 4 + SCI0 RXBUFFER
VARIABLE BUFT DECIMAL 40 4 + ALLOT EEWORD
BUFT 40 4 + SCI0 TXBUFFER

DECIMAL 97 CONSTANT DLE EEWORD
DECIMAL 98 CONSTANT SERHEAD EEWORD
DECIMAL 103 CONSTANT ETX EEWORD

VARIABLE DATA1 EEWORD
VARIABLE DATA2 EEWORD
VARIABLE DATA3 EEWORD
VARIABLE DATA4 EEWORD
VARIABLE DATA5 EEWORD
VARIABLE DATA6 EEWORD

VARIABLE SERID EEWORD
VARIABLE DVAL EEWORD
VARIABLE DROT EEWORD

MACHINE SERVE4TEST EEWORD
ON-MACHINE SERVE4TEST
APPEND-STATE SER1 EEWORD
APPEND-STATE SER2 EEWORD
APPEND-STATE SER3 EEWORD
APPEND-STATE SER4 EEWORD
APPEND-STATE SER5 EEWORD
APPEND-STATE SER6 EEWORD


IN-STATE SER1
CONDITION SCI0 RX?
CAUSES RX DATA1 C!
RX DATA2 C!
RX DATA3 C!
RX DATA4 C!
RX DATA5 C!
RX DATA6 C!
THEN-STATE SER2 TO-HAPPEN IN-EE

IN-STATE SER2
CONDITION DATA1 C@ DLE =
CAUSES DLE SCI0 TX GRNLED OFF
THEN-STATE SER3 TO-HAPPEN IN-EE

IN-STATE SER3
CONDITION DATA2 C@ SERHEAD =
CAUSES SERHEAD SCI0 TX REDLED OFF
THEN-STATE SER4 TO-HAPPEN IN-EE

IN-STATE SER4
CONDITION DATA5 C@ DLE =
CAUSES DLE SCI0 TX
THEN-STATE SER5 TO-HAPPEN IN-EE

IN-STATE SER5
CONDITION DATA6 C@ ETX =
CAUSES ETX SCI0 TX YELLED OFF
THEN-STATE SER6 TO-HAPPEN IN-EE

IN-STATE SER6
CONDITION DATA3 C@ 99 =
CAUSES 32767 PWMA0 PWM-PERIOD
DATA4 C@ 56 * 6800 + DROT !
DROT @ PWMA0 PWM-OUT
YELLED ON
THEN-STATE SER1 TO-HAPPEN

IN-STATE SER6
CONDITION DATA3 C@ 100 =
CAUSES 32767 PWMA1 PWM-PERIOD
DATA4 C@ 56 * 6800 + DROT !
DROT @ PWMA1 PWM-OUT
GRNLED ON
THEN-STATE SER1 TO-HAPPEN

IN-STATE SER6
CONDITION DATA3 C@ 101 =
CAUSES 32767 PWMA2 PWM-PERIOD
DATA4 C@ 56 * 6800 + DROT !
DROT @ PWMA2 PWM-OUT
REDLED ON
THEN-STATE SER1 TO-HAPPEN

IN-STATE SER6
CONDITION DATA3 C@ 102 =
CAUSES 32767 PWMA3 PWM-PERIOD
DATA4 C@ 56 * 6800 + DROT !
DROT @ PWMA3 PWM-OUT
YELLED ON
GRNLED ON
REDLED ON
THEN-STATE SER1 TO-HAPPEN


: HANG BEGIN PB0 OFF? UNTIL ; EEWORD

: STARTUP
ISOMAX-START
SER1 SET-STATE
INSTALL SERVE4TEST
HANG
; EEWORD

HEX 3C00 AUTOSTART STARTUP
SAVE-RAM

In anticipation

Regards

Navneet

[nmitech]

Add IN-EE for all the SER6 states and try again see what happen!

[NAVNEET ACHARYA]

Thanks for response.

That was the only problem now it's working.

Regrads,

Navneet Acharya