View Full Version : jerky movement of servos!
bcrepet
12-05-06, 11:20 AM
Hello,
I have a problem with the program I wrote to control the speed of my servos: I have a nice
smooth movement with the servos connected to PWMA and PWMB, but the servos connected on the
others PWM pins (I use all the other PWM pins excepted TA3) have a strange jerky movement
when driven by my speed control program, it's like they're always going forward, a little
bit backward and forward again to accomplish the whole movement.
The program I use adds a step (which corresponds to the top speed) to the position of the
servo over and over again, untill the servo reaches its desired position:
Here is a piece of it that drives two servos: 0 and 1 (The one I have problem with drives
ten servos, I can send everything but it is a bit long). MOVEDIR is used to know on which
side the servo has to move, REMAINING is used to know how far the servo is from its desired
position and SERVO is used to set the PWM signal. POS is supposed to be the actual position
of the servo and CMDPOS the desired position of the servo. D is the step of the movement and
MXSPD the speed of the movement.
: MAJ
MOVEDIR0?
IF
-1 M0 !
ELSE
1 M0 !
THEN
REMAINING0 MXSPD0 @ >
IF
MXSPD0 @ M0 @ * D0 !
ELSE
REMAINING0 M0 @ * D0 !
THEN
D0 @ POS0 @ + POS0 !
POS0 @ SERVO0
MOVEDIR1?
IF
-1 M1 !
ELSE
1 M1 !
THEN
REMAINING1 MXSPD1 @ >
IF
MXSPD1 @ M1 @ * D1 !
ELSE
REMAINING1 M1 @ * D1 !
THEN
D1 @ POS1 @ + POS1 !
POS1 @ SERVO1
; EEWORD
This piece of code adds one step to the position of the servo, it is used by a state
machine (3 with 25 servos) over and over again until the MOVEDONE condition (position of the
servos same as desired position) becomes true:
MACHINE SERVJ
ON-MACHINE SERVJ APPEND-STATE IM
IN-STATE IM CONDITION MOVEDONE? NOT CAUSES
MAJ
REDLED OFF
THEN-STATE IM
TO-HAPPEN
IN-STATE IM CONDITION MOVEDONE? CAUSES
0 D0 !
0 D1 !
REDLED ON
THEN-STATE IM
TO-HAPPEN
IM SET-STATE INSTALL SERVOJ
EVERY 50000 CYCLES SCHEDULE-RUNS SERVOJ
The period of the state machine is used to control the speed of the servo's movement.
The code is exactly the same for all the servos and the servos connected to PWMA and PWMB
and the other ones are the same type of servos. The voltage also is the same for all the
servos...) I checked the TCFOVFLO after using the machine but it is always at 0...
Is there a difference of PWM signal between PWMA-B and the other PWM pins? Is there an error
I did not see in my program?
Thank you in advance for your help,
BCREPET.
Yes, there is a difference in the pulses generated by the PWMA and PWMB pins in comparison to the Timer pins TA0-3, TB0-3, TC0,1, and TD0-3 .
The Timer pins accept a number between 4 and 65535 for PWM-PERIOD, determining the period of the pulses needed, with 50000 used for a 50 Hz update rate typically used for servos. Therefore a value of 3277 to 6554 is needed for PWM-OUT to generate a 1 to 2 ms pulse on a Timer pin, when used with a 50 hZ pulse rate (5 to 10% of a 20 ms period). Timer pins must also be designated ACTIVE-HIGH in use, to generate a high pulse, rather than a low pulse from a steady high.
The PWM pins can accept a number between 4 and 32767 for PWM-PERIOD, allowing the minimum update rate of 76.9 Hz at full processor speed. Most servos we've observed seem to respond well to this rate, and PWM-OUT is generally 5000 to 10000 for PWM-OUT to generate a 1 to 2 ms pulse at this frequency.
In use, perhaps setting the Timer pins to the same 32767 PWM-PERIOD as the PWM pins will have the same reaction in the servos attached to them as to those attached to the PWM pins. As each timer pin should also be initialized as ACTIVE-HIGH.
Further information is available on page 15 of the IO-Programming (http://www.newmicros.com/store/product_manual/IOProgramming.pdf) document and page 5 of the IO Glossary (http://www.newmicros.com/store/product_manual/IOglossary.pdf) document.
bcrepet
12-08-06, 09:47 AM
I tryed to initialize all the timer pins as ACTIVE-HIGH and all the pwm period at 32767 but I still have this strange movement for the servos connected to the timer pins. The problem is the same if I change the pwm period.
Is it possible that the pwm signals from the timer pins don't have the same shape in the time, the same accuracy or the same refresh time as the pwm pins?
In all our applications using the Pods to drive RC-servos, even in precise methods that include inverse kinematic calculations, there has been no difference in the reactions of servos attached to either PWM pins or Timer pins.
This 18 servo Hexapod walker code (http://www.lynxmotion.com/images/files/eh3r01.txt) uses inverse kinematics to calculate foot position.
It would be helpful to see all the code involved to see what might be causing the different servo reactions.
bcrepet
12-08-06, 12:32 PM
I look at this hexapod walker code.
My entire code is a bit long, i hope it will be usefull.
Thank you for your help.
SCRUB
VARIABLE M0 EEWORD
VARIABLE POS0 EEWORD
VARIABLE CMDPOS0 EEWORD
VARIABLE MXSPD0 EEWORD
VARIABLE D0 EEWORD
VARIABLE ACC0 EEWORD
VARIABLE REMDEC0 EEWORD
VARIABLE M1 EEWORD
VARIABLE POS1 EEWORD
VARIABLE CMDPOS1 EEWORD
VARIABLE MXSPD1 EEWORD
VARIABLE D1 EEWORD
VARIABLE ACC1 EEWORD
VARIABLE REMDEC1 EEWORD
VARIABLE M2 EEWORD
VARIABLE POS2 EEWORD
VARIABLE CMDPOS2 EEWORD
VARIABLE MXSPD2 EEWORD
VARIABLE D2 EEWORD
VARIABLE ACC2 EEWORD
VARIABLE REMDEC2 EEWORD
VARIABLE M3 EEWORD
VARIABLE POS3 EEWORD
VARIABLE CMDPOS3 EEWORD
VARIABLE MXSPD3 EEWORD
VARIABLE D3 EEWORD
VARIABLE ACC3 EEWORD
VARIABLE REMDEC3 EEWORD
VARIABLE M4 EEWORD
VARIABLE POS4 EEWORD
VARIABLE CMDPOS4 EEWORD
VARIABLE MXSPD4 EEWORD
VARIABLE D4 EEWORD
VARIABLE ACC4 EEWORD
VARIABLE REMDEC4 EEWORD
VARIABLE M5 EEWORD
VARIABLE POS5 EEWORD
VARIABLE CMDPOS5 EEWORD
VARIABLE MXSPD5 EEWORD
VARIABLE D5 EEWORD
VARIABLE ACC5 EEWORD
VARIABLE REMDEC5 EEWORD
VARIABLE M6 EEWORD
VARIABLE POS6 EEWORD
VARIABLE CMDPOS6 EEWORD
VARIABLE MXSPD6 EEWORD
VARIABLE D6 EEWORD
VARIABLE ACC6 EEWORD
VARIABLE REMDEC6 EEWORD
VARIABLE M7 EEWORD
VARIABLE POS7 EEWORD
VARIABLE CMDPOS7 EEWORD
VARIABLE MXSPD7 EEWORD
VARIABLE D7 EEWORD
VARIABLE ACC7 EEWORD
VARIABLE REMDEC7 EEWORD
VARIABLE M8 EEWORD
VARIABLE POS8 EEWORD
VARIABLE CMDPOS8 EEWORD
VARIABLE MXSPD8 EEWORD
VARIABLE D8 EEWORD
VARIABLE ACC8 EEWORD
VARIABLE REMDEC8 EEWORD
VARIABLE M9 EEWORD
VARIABLE POS9 EEWORD
VARIABLE CMDPOS9 EEWORD
VARIABLE MXSPD9 EEWORD
VARIABLE D9 EEWORD
VARIABLE ACC9 EEWORD
VARIABLE REMDEC9 EEWORD
VARIABLE M10 EEWORD
VARIABLE POS10 EEWORD
VARIABLE CMDPOS10 EEWORD
VARIABLE MXSPD10 EEWORD
VARIABLE D10 EEWORD
VARIABLE ACC10 EEWORD
VARIABLE REMDEC10 EEWORD
VARIABLE M11 EEWORD
VARIABLE POS11 EEWORD
VARIABLE CMDPOS11 EEWORD
VARIABLE MXSPD11 EEWORD
VARIABLE D11 EEWORD
VARIABLE ACC11 EEWORD
VARIABLE REMDEC11 EEWORD
VARIABLE M12 EEWORD
VARIABLE POS12 EEWORD
VARIABLE CMDPOS12 EEWORD
VARIABLE MXSPD12 EEWORD
VARIABLE D12 EEWORD
VARIABLE ACC12 EEWORD
VARIABLE REMDEC12 EEWORD
VARIABLE M13 EEWORD
VARIABLE POS13 EEWORD
VARIABLE CMDPOS13 EEWORD
VARIABLE MXSPD13 EEWORD
VARIABLE D13 EEWORD
VARIABLE ACC13 EEWORD
VARIABLE REMDEC13 EEWORD
VARIABLE M14 EEWORD
VARIABLE POS14 EEWORD
VARIABLE CMDPOS14 EEWORD
VARIABLE MXSPD14 EEWORD
VARIABLE D14 EEWORD
VARIABLE ACC14 EEWORD
VARIABLE REMDEC14 EEWORD
VARIABLE MA EEWORD
VARIABLE POSA EEWORD
VARIABLE CMDPOSA EEWORD
VARIABLE MXSPDA EEWORD
VARIABLE DA EEWORD
VARIABLE ACCA EEWORD
VARIABLE REMDECA EEWORD
VARIABLE MB EEWORD
VARIABLE POSB EEWORD
VARIABLE CMDPOSB EEWORD
VARIABLE MXSPDB EEWORD
VARIABLE DB EEWORD
VARIABLE ACCB EEWORD
VARIABLE REMDECB EEWORD
VARIABLE MC EEWORD
VARIABLE POSC EEWORD
VARIABLE CMDPOSC EEWORD
VARIABLE MXSPDC EEWORD
VARIABLE DC EEWORD
VARIABLE ACCC EEWORD
VARIABLE REMDECC EEWORD
VARIABLE MD EEWORD
VARIABLE POSD EEWORD
VARIABLE CMDPOSD EEWORD
VARIABLE MXSPDD EEWORD
VARIABLE DD EEWORD
VARIABLE ACCD EEWORD
VARIABLE REMDECD EEWORD
VARIABLE ME EEWORD
VARIABLE POSE EEWORD
VARIABLE CMDPOSE EEWORD
VARIABLE MXSPDE EEWORD
VARIABLE DE EEWORD
VARIABLE ACCE EEWORD
VARIABLE REMDECE EEWORD
VARIABLE MF EEWORD
VARIABLE POSF EEWORD
VARIABLE CMDPOSF EEWORD
VARIABLE MXSPDF EEWORD
VARIABLE DF EEWORD
VARIABLE ACCF EEWORD
VARIABLE REMDECF EEWORD
VARIABLE MG EEWORD
VARIABLE POSG EEWORD
VARIABLE CMDPOSG EEWORD
VARIABLE MXSPDG EEWORD
VARIABLE DG EEWORD
VARIABLE ACCG EEWORD
VARIABLE REMDECG EEWORD
VARIABLE MH EEWORD
VARIABLE POSH EEWORD
VARIABLE CMDPOSH EEWORD
VARIABLE MXSPDH EEWORD
VARIABLE DH EEWORD
VARIABLE ACCH EEWORD
VARIABLE REMDECH EEWORD
VARIABLE MI EEWORD
VARIABLE POSI EEWORD
VARIABLE CMDPOSI EEWORD
VARIABLE MXSPDI EEWORD
VARIABLE DI EEWORD
VARIABLE ACCI EEWORD
VARIABLE REMDECI EEWORD
VARIABLE MJ EEWORD
VARIABLE POSJ EEWORD
VARIABLE CMDPOSJ EEWORD
VARIABLE MXSPDJ EEWORD
VARIABLE DJ EEWORD
VARIABLE ACCJ EEWORD
VARIABLE REMDECJ EEWORD
VARIABLE PDEF0 EEWORD
VARIABLE PDEF1 EEWORD
VARIABLE PDEF2 EEWORD
VARIABLE PDEF3 EEWORD
VARIABLE PDEF4 EEWORD
VARIABLE PDEF5 EEWORD
VARIABLE PDEF6 EEWORD
VARIABLE PDEF7 EEWORD
VARIABLE PDEF8 EEWORD
VARIABLE PDEF9 EEWORD
VARIABLE PDEF10 EEWORD
VARIABLE PDEF11 EEWORD
VARIABLE PDEF12 EEWORD
VARIABLE PDEF13 EEWORD
VARIABLE PDEF14 EEWORD
VARIABLE PDEFA EEWORD
VARIABLE PDEFB EEWORD
VARIABLE PDEFC EEWORD
VARIABLE PDEFD EEWORD
VARIABLE PDEFE EEWORD
VARIABLE PDEFF EEWORD
VARIABLE PDEFG EEWORD
VARIABLE PDEFH EEWORD
VARIABLE PDEFI EEWORD
VARIABLE PDEFJ EEWORD
VARIABLE VDEF0 EEWORD
VARIABLE VDEF1 EEWORD
VARIABLE VDEF2 EEWORD
VARIABLE VDEF3 EEWORD
VARIABLE VDEF4 EEWORD
VARIABLE VDEF5 EEWORD
VARIABLE VDEF6 EEWORD
VARIABLE VDEF7 EEWORD
VARIABLE VDEF8 EEWORD
VARIABLE VDEF9 EEWORD
VARIABLE VDEF10 EEWORD
VARIABLE VDEF11 EEWORD
VARIABLE VDEF12 EEWORD
VARIABLE VDEF13 EEWORD
VARIABLE VDEF14 EEWORD
VARIABLE VDEFA EEWORD
VARIABLE VDEFB EEWORD
VARIABLE VDEFC EEWORD
VARIABLE VDEFD EEWORD
VARIABLE VDEFE EEWORD
VARIABLE VDEFF EEWORD
VARIABLE VDEFG EEWORD
VARIABLE VDEFH EEWORD
VARIABLE VDEFI EEWORD
VARIABLE VDEFJ EEWORD
VARIABLE ADEF0 EEWORD
VARIABLE ADEF1 EEWORD
VARIABLE ADEF2 EEWORD
VARIABLE ADEF3 EEWORD
VARIABLE ADEF4 EEWORD
VARIABLE ADEF5 EEWORD
VARIABLE ADEF6 EEWORD
VARIABLE ADEF7 EEWORD
VARIABLE ADEF8 EEWORD
VARIABLE ADEF9 EEWORD
VARIABLE ADEF10 EEWORD
VARIABLE ADEF11 EEWORD
VARIABLE ADEF12 EEWORD
VARIABLE ADEF13 EEWORD
VARIABLE ADEF14 EEWORD
VARIABLE ADEFA EEWORD
VARIABLE ADEFB EEWORD
VARIABLE ADEFC EEWORD
VARIABLE ADEFD EEWORD
VARIABLE ADEFE EEWORD
VARIABLE ADEFF EEWORD
VARIABLE ADEFG EEWORD
VARIABLE ADEFH EEWORD
VARIABLE ADEFI EEWORD
VARIABLE ADEFJ EEWORD
VARIABLE REMDEC0 EEWORD
VARIABLE REMDEC1 EEWORD
VARIABLE REMDEC2 EEWORD
VARIABLE REMDEC3 EEWORD
VARIABLE REMDEC4 EEWORD
VARIABLE REMDEC5 EEWORD
VARIABLE REMDEC6 EEWORD
VARIABLE REMDEC7 EEWORD
VARIABLE REMDEC8 EEWORD
VARIABLE REMDEC9 EEWORD
VARIABLE REMDEC10 EEWORD
VARIABLE REMDEC11 EEWORD
VARIABLE REMDEC12 EEWORD
VARIABLE REMDEC13 EEWORD
VARIABLE REMDEC14 EEWORD
VARIABLE REMDECA EEWORD
VARIABLE REMDECB EEWORD
VARIABLE REMDECC EEWORD
VARIABLE REMDECD EEWORD
VARIABLE REMDECE EEWORD
VARIABLE REMDECF EEWORD
VARIABLE REMDECG EEWORD
VARIABLE REMDECH EEWORD
VARIABLE REMDECI EEWORD
VARIABLE REMDECJ EEWORD
: SERVO0 9600 10000 */
2200 +
PWMA0 PWM-OUT
; EEWORD
: SERVO1 6000 10000 */
6000 +
PWMA1 PWM-OUT
; EEWORD
: SERVO2 8000 10000 */
4000 +
PWMA2 PWM-OUT
; EEWORD
: SERVO3 7000 10000 */
4000 +
PWMA3 PWM-OUT
; EEWORD
: SERVO4 5500 10000 */
7000 +
PWMA4 PWM-OUT
; EEWORD
: SERVO5 6500 10000 */
4500 +
PWMA5 PWM-OUT
; EEWORD
: SERVO6 -6500 10000 */
11000 +
PWMB0 PWM-OUT
; EEWORD
: SERVO7 -4800 10000 */
8000 +
PWMB1 PWM-OUT
; EEWORD
: SERVO8 -8000 10000 */
12100 +
PWMB2 PWM-OUT
; EEWORD
: SERVO9 -8000 10000 */
12700 +
PWMB5 PWM-OUT
; EEWORD
: SERVO10 -6000 10000 */
9000 +
PWMB4 PWM-OUT
; EEWORD
: SERVO11 7800 10000 */
4000 +
PWMB3 PWM-OUT
; EEWORD
: SERVO12 3800 1000 */
5500 +
TD0 PWM-OUT
; EEWORD
: SERVO13 3000 1000 */
3000 +
TD1 PWM-OUT
; EEWORD
: SERVO14 4000 1000 */
6000 +
TD2 PWM-OUT
; EEWORD
: SERVOA 8500 1000 */
2500 +
TC1 PWM-OUT
; EEWORD
: SERVOB -8500 1000 */
11000 +
TC0 PWM-OUT
; EEWORD
: SERVOC 8500 1000 */
2500 +
TA2 PWM-OUT
; EEWORD
: SERVOD -8500 1000 */
11000 +
TB2 PWM-OUT
; EEWORD
: SERVOE 8500 1000 */
2500 +
TA3 PWM-OUT
; EEWORD
: SERVOF -8500 1000 */
11000 +
TB3 PWM-OUT
; EEWORD
: SERVOG 8500 1000 */
2500 +
TA1 PWM-OUT
; EEWORD
: SERVOH -8500 1000 */
11000 +
TB1 PWM-OUT
; EEWORD
: SERVOI 8500 1000 */
2500 +
TA0 PWM-OUT
; EEWORD
: SERVOJ -8500 1000 */
11000 +
TB0 PWM-OUT
; EEWORD
: DEB0
PWMA0 OFF
; EEWORD
: DEB1
PWMA1 OFF
; EEWORD
: DEB2
PWMA2 OFF
; EEWORD
: DEB3
PWMA3 OFF
; EEWORD
: DEB4
PWMA4 OFF
; EEWORD
: DEB5
PWMA5 OFF
; EEWORD
: DEB6
PWMB0 OFF
; EEWORD
: DEB7
PWMB1 OFF
; EEWORD
: DEB8
PWMB2 OFF
; EEWORD
: DEB9
PWMB5 OFF
; EEWORD
: DEB10
PWMB4 OFF
; EEWORD
: DEB11
PWMB3 OFF
; EEWORD
: DEB12
TD0 OFF
; EEWORD
: DEB13
TD1 OFF
; EEWORD
: DEB14
TD2 OFF
; EEWORD
: DEBA
TC1 OFF
; EEWORD
: DEBB
TC0 OFF
; EEWORD
: DEBC
TA2 OFF
; EEWORD
: DEBD
TB2 OFF
; EEWORD
: DEBE
TA3 OFF
; EEWORD
: DEBF
TB3 OFF
; EEWORD
: DEBG
TA1 OFF
; EEWORD
: DEBH
TB1 OFF
; EEWORD
: DEBI
TA0 OFF
; EEWORD
: DEBJ
TB0 OFF
; EEWORD
: DEBALL
PWMA0 OFF
PWMA1 OFF
PWMA2 OFF
PWMA3 OFF
PWMA4 OFF
PWMA5 OFF
PWMB0 OFF
PWMB1 OFF
PWMB2 OFF
PWMB5 OFF
PWMB4 OFF
PWMB3 OFF
TD0 OFF
TD1 OFF
TD2 OFF
TC1 OFF
TC0 OFF
TA2 OFF
TB2 OFF
TA3 OFF
TB3 OFF
TA1 OFF
TB1 OFF
TA0 OFF
TB0 OFF
; EEWORD
: MOVEDONE?
CMDPOS0 @ POS0 @ =
CMDPOS1 @ POS1 @ = AND
CMDPOS2 @ POS2 @ = AND
CMDPOS3 @ POS3 @ = AND
CMDPOS4 @ POS4 @ = AND
CMDPOS5 @ POS5 @ = AND
CMDPOS6 @ POS6 @ = AND
CMDPOS7 @ POS7 @ = AND
CMDPOS8 @ POS8 @ = AND
CMDPOS9 @ POS9 @ = AND
CMDPOS10 @ POS10 @ = AND
CMDPOS11 @ POS11 @ = AND
CMDPOS12 @ POS12 @ = AND
CMDPOS13 @ POS13 @ = AND
CMDPOS14 @ POS14 @ = AND
CMDPOSA @ POSA @ = AND
CMDPOSB @ POSB @ = AND
CMDPOSC @ POSC @ = AND
CMDPOSD @ POSD @ = AND
CMDPOSE @ POSE @ = AND
CMDPOSF @ POSF @ = AND
CMDPOSG @ POSG @ = AND
CMDPOSH @ POSH @ = AND
CMDPOSI @ POSI @ = AND
CMDPOSJ @ POSJ @ = AND
; EEWORD
: MOVEDONJ?
CMDPOS0 @ POS0 @ =
CMDPOS1 @ POS1 @ = AND
CMDPOS2 @ POS2 @ = AND
CMDPOS3 @ POS3 @ = AND
CMDPOS4 @ POS4 @ = AND
CMDPOS5 @ POS5 @ = AND
CMDPOS6 @ POS6 @ = AND
CMDPOS7 @ POS7 @ = AND
CMDPOS8 @ POS8 @ = AND
CMDPOS9 @ POS9 @ = AND
CMDPOS10 @ POS10 @ = AND
CMDPOS11 @ POS11 @ = AND
; EEWORD
: MOVEDONET?
CMDPOS12 @ POS12 @ =
CMDPOS13 @ POS13 @ = AND
CMDPOS14 @ POS14 @ = AND
; EEWORD
: MOVEDONB?
CMDPOSA @ POSA @ =
CMDPOSB @ POSB @ = AND
CMDPOSC @ POSC @ = AND
CMDPOSD @ POSD @ = AND
CMDPOSE @ POSE @ = AND
CMDPOSF @ POSF @ = AND
CMDPOSG @ POSG @ = AND
CMDPOSH @ POSH @ = AND
CMDPOSI @ POSI @ = AND
CMDPOSJ @ POSJ @ = AND
; EEWORD
: MOVEDONE0?
CMDPOS0 @ POS0 @ =
; EEWORD
: MOVEDONE1?
CMDPOS1 @ POS1 @ =
; EEWORD
: MOVEDONE2?
CMDPOS2 @ POS2 @ =
; EEWORD
: MOVEDONE3?
CMDPOS3 @ POS3 @ =
; EEWORD
: MOVEDONE4?
CMDPOS4 @ POS4 @ =
; EEWORD
: MOVEDONE5?
CMDPOS5 @ POS5 @ =
; EEWORD
: MOVEDONE6?
CMDPOS6 @ POS6 @ =
; EEWORD
: MOVEDONE7?
CMDPOS7 @ POS7 @ =
; EEWORD
: MOVEDONE8?
CMDPOS8 @ POS8 @ =
; EEWORD
: MOVEDONE9?
CMDPOS9 @ POS9 @ =
; EEWORD
: MOVEDONE10?
CMDPOS10 @ POS10 @ =
; EEWORD
: MOVEDONE11?
CMDPOS11 @ POS11 @ =
; EEWORD
: MOVEDONE12?
CMDPOS12 @ POS12 @ =
; EEWORD
: MOVEDONE13?
CMDPOS13 @ POS13 @ =
; EEWORD
: MOVEDONE14?
CMDPOS14 @ POS14 @ =
; EEWORD
: MOVEDONEA?
CMDPOSA @ POSA @ =
; EEWORD
: MOVEDONEB?
CMDPOSB @ POSB @ =
; EEWORD
: MOVEDONEC?
CMDPOSC @ POSC @ =
; EEWORD
: MOVEDONED?
CMDPOSD @ POSD @ =
; EEWORD
: MOVEDONEE?
CMDPOSE @ POSE @ =
; EEWORD
: MOVEDONEF?
CMDPOSF @ POSF @ =
; EEWORD
: MOVEDONEG?
CMDPOSG @ POSG @ =
; EEWORD
: MOVEDONEH?
CMDPOSH @ POSH @ =
; EEWORD
: MOVEDONEI?
CMDPOSI @ POSI @ =
; EEWORD
: MOVEDONEJ?
CMDPOSJ @ POSJ @ =
; EEWORD
: MOVEDIR0?
CMDPOS0 @ POS0 @ <
; EEWORD
: REMAINING0
CMDPOS0 @ POS0 @ - ABS
; EEWORD
: MOVEDIR1?
CMDPOS1 @ POS1 @ <
; EEWORD
: REMAINING1
CMDPOS1 @ POS1 @ - ABS
; EEWORD
: MOVEDIR2?
CMDPOS2 @ POS2 @ <
; EEWORD
: REMAINING2
CMDPOS2 @ POS2 @ - ABS
; EEWORD
: MOVEDIR3?
CMDPOS3 @ POS3 @ <
; EEWORD
: REMAINING3
CMDPOS3 @ POS3 @ - ABS
; EEWORD
: MOVEDIR4?
CMDPOS4 @ POS4 @ <
; EEWORD
: REMAINING4
CMDPOS4 @ POS4 @ - ABS
; EEWORD
: MOVEDIR5?
CMDPOS5 @ POS5 @ <
; EEWORD
: REMAINING5
CMDPOS5 @ POS5 @ - ABS
; EEWORD
: MOVEDIR6?
CMDPOS6 @ POS6 @ <
; EEWORD
: REMAINING6
CMDPOS6 @ POS6 @ - ABS
; EEWORD
: MOVEDIR7?
CMDPOS7 @ POS7 @ <
; EEWORD
: REMAINING7
CMDPOS7 @ POS7 @ - ABS
; EEWORD
: MOVEDIR8?
CMDPOS8 @ POS8 @ <
; EEWORD
: REMAINING8
CMDPOS8 @ POS8 @ - ABS
; EEWORD
: MOVEDIR9?
CMDPOS9 @ POS9 @ <
; EEWORD
: REMAINING9
CMDPOS9 @ POS9 @ - ABS
; EEWORD
: MOVEDIR10?
CMDPOS10 @ POS10 @ <
; EEWORD
: REMAINING10
CMDPOS10 @ POS10 @ - ABS
; EEWORD
: MOVEDIR11?
CMDPOS11 @ POS11 @ <
; EEWORD
: REMAINING11
CMDPOS11 @ POS11 @ - ABS
; EEWORD
: MOVEDIR12?
CMDPOS12 @ POS12 @ <
; EEWORD
: REMAINING12
CMDPOS12 @ POS12 @ - ABS
; EEWORD
: MOVEDIR13?
CMDPOS13 @ POS13 @ <
; EEWORD
: REMAINING13
CMDPOS13 @ POS13 @ - ABS
; EEWORD
: MOVEDIR14?
CMDPOS14 @ POS14 @ <
; EEWORD
: REMAINING14
CMDPOS14 @ POS14 @ - ABS
; EEWORD
: MOVEDIRA?
CMDPOSA @ POSA @ <
; EEWORD
: REMAININGA
CMDPOSA @ POSA @ - ABS
; EEWORD
: MOVEDIRB?
CMDPOSB @ POSB @ <
; EEWORD
: REMAININGB
CMDPOSB @ POSB @ - ABS
; EEWORD
: MOVEDIRC?
CMDPOSC @ POSC @ <
; EEWORD
: REMAININGC
CMDPOSC @ POSC @ - ABS
; EEWORD
: MOVEDIRD?
CMDPOSD @ POSD @ <
; EEWORD
: REMAININGD
CMDPOSD @ POSD @ - ABS
; EEWORD
: MOVEDIRE?
CMDPOSE @ POSE @ <
; EEWORD
: REMAININGE
CMDPOSE @ POSE @ - ABS
; EEWORD
: MOVEDIRF?
CMDPOSF @ POSF @ <
; EEWORD
: REMAININGF
CMDPOSF @ POSF @ - ABS
; EEWORD
: MOVEDIRG?
CMDPOSG @ POSG @ <
; EEWORD
: REMAININGG
CMDPOSG @ POSG @ - ABS
; EEWORD
: MOVEDIRH?
CMDPOSH @ POSH @ <
; EEWORD
: REMAININGH
CMDPOSH @ POSH @ - ABS
; EEWORD
: MOVEDIRI?
CMDPOSI @ POSI @ <
; EEWORD
: REMAININGI
CMDPOSI @ POSI @ - ABS
; EEWORD
: MOVEDIRJ?
CMDPOSJ @ POSJ @ <
; EEWORD
: REMAININGJ
CMDPOSJ @ POSJ @ - ABS
; EEWORD
: MAJ
MOVEDONE0? NOT
IF
MOVEDIR0?
IF
-1 M0 !
ELSE
1 M0 !
THEN
REMAINING0 MXSPD0 @ >
IF
MXSPD0 @ M0 @ * D0 !
ELSE
REMAINING0 M0 @ * D0 !
THEN
D0 @ POS0 @ + POS0 !
POS0 @ SERVO0
THEN
MOVEDONE1? NOT
IF
MOVEDIR1?
IF
-1 M1 !
ELSE
1 M1 !
THEN
REMAINING1 MXSPD1 @ >
IF
MXSPD1 @ M1 @ * D1 !
ELSE
REMAINING1 M1 @ * D1 !
THEN
D1 @ POS1 @ + POS1 !
POS1 @ SERVO1
THEN
MOVEDONE2? NOT
IF
MOVEDIR2?
IF
-1 M2 !
ELSE
1 M2 !
THEN
REMAINING2 MXSPD2 @ >
IF
MXSPD2 @ M2 @ * D2 !
ELSE
REMAINING2 M2 @ * D2 !
THEN
D2 @ POS2 @ + POS2 !
POS2 @ SERVO2
THEN
MOVEDONE3? NOT
IF
MOVEDIR3?
IF
-1 M3 !
ELSE
1 M3 !
THEN
REMAINING3 MXSPD3 @ >
IF
MXSPD3 @ M3 @ * D3 !
ELSE
REMAINING3 M3 @ * D3 !
THEN
D3 @ POS3 @ + POS3 !
POS3 @ SERVO3
THEN
MOVEDONE4? NOT
IF
MOVEDIR4?
IF
-1 M4 !
ELSE
1 M4 !
THEN
REMAINING4 MXSPD4 @ >
IF
MXSPD4 @ M4 @ * D4 !
ELSE
REMAINING4 M4 @ * D4 !
THEN
D4 @ POS4 @ + POS4 !
POS4 @ SERVO4
THEN
MOVEDONE5? NOT
IF
MOVEDIR5?
IF
-1 M5 !
ELSE
1 M5 !
THEN
REMAINING5 MXSPD5 @ >
IF
MXSPD5 @ M5 @ * D5 !
ELSE
REMAINING5 M5 @ * D5 !
THEN
D5 @ POS5 @ + POS5 !
POS5 @ SERVO5
THEN
MOVEDONE6? NOT
IF
MOVEDIR6?
IF
-1 M6 !
ELSE
1 M6 !
THEN
REMAINING6 MXSPD6 @ >
IF
MXSPD6 @ M6 @ * D6 !
ELSE
REMAINING6 M6 @ * D6 !
THEN
D6 @ POS6 @ + POS6 !
POS6 @ SERVO6
THEN
MOVEDONE7? NOT
IF
MOVEDIR7?
IF
-1 M7 !
ELSE
1 M7 !
THEN
REMAINING7 MXSPD7 @ >
IF
MXSPD7 @ M7 @ * D7 !
ELSE
REMAINING7 M7 @ * D7 !
THEN
D7 @ POS7 @ + POS7 !
POS7 @ SERVO7
THEN
MOVEDONE8? NOT
IF
MOVEDIR8?
IF
-1 M8 !
ELSE
1 M8 !
THEN
REMAINING8 MXSPD8 @ >
IF
MXSPD8 @ M8 @ * D8 !
ELSE
REMAINING8 M8 @ * D8 !
THEN
D8 @ POS8 @ + POS8 !
POS8 @ SERVO8
THEN
MOVEDONE9? NOT
IF
MOVEDIR9?
IF
-1 M9 !
ELSE
1 M9 !
THEN
REMAINING9 MXSPD9 @ >
IF
MXSPD9 @ M9 @ * D9 !
ELSE
REMAINING9 M9 @ * D9 !
THEN
D9 @ POS9 @ + POS9 !
POS9 @ SERVO9
THEN
MOVEDONE10? NOT
IF
MOVEDIR10?
IF
-1 M10 !
ELSE
1 M10 !
THEN
REMAINING10 MXSPD10 @ >
IF
MXSPD10 @ M10 @ * D10 !
ELSE
REMAINING10 M10 @ * D10 !
THEN
D10 @ POS10 @ + POS10 !
POS10 @ SERVO10
THEN
MOVEDONE11? NOT
IF
MOVEDIR11?
IF
-1 M11 !
ELSE
1 M11 !
THEN
REMAINING11 MXSPD11 @ >
IF
MXSPD11 @ M11 @ * D11 !
ELSE
REMAINING11 M11 @ * D11 !
THEN
D11 @ POS11 @ + POS11 !
POS11 @ SERVO11
THEN
; EEWORD
: MAT
MOVEDONE12? NOT
IF
MOVEDIR12?
IF
-1 M12 !
ELSE
1 M12 !
THEN
REMAINING12 MXSPD12 @ >
IF
MXSPD12 @ M12 @ * D12 !
ELSE
REMAINING12 M12 @ * D12 !
THEN
D12 @ POS12 @ + POS12 !
POS12 @ SERVO12
THEN
MOVEDONE13? NOT
IF
MOVEDIR13?
IF
-1 M13 !
ELSE
1 M13 !
THEN
REMAINING13 MXSPD13 @ >
IF
MXSPD13 @ M13 @ * D13 !
ELSE
REMAINING13 M13 @ * D13 !
THEN
D13 @ POS13 @ + POS13 !
POS13 @ SERVO13
THEN
MOVEDONE14? NOT
IF
MOVEDIR14?
IF
-1 M14 !
ELSE
1 M14 !
THEN
REMAINING14 MXSPD14 @ >
IF
MXSPD14 @ M14 @ * D14 !
ELSE
REMAINING14 M14 @ * D14 !
THEN
D14 @ POS14 @ + POS14 !
POS14 @ SERVO14
THEN
; EEWORD
: MAB
MOVEDONEA? NOT
IF
MOVEDIRA?
IF
-1 MA !
ELSE
1 MA !
THEN
REMAININGA MXSPDA @ >
IF
MXSPDA @ MA @ * DA !
ELSE
REMAININGA MA @ * DA !
THEN
DA @ POSA @ + POSA !
POSA @ SERVOA
THEN
MOVEDONEB? NOT
IF
MOVEDIRB?
IF
-1 MB !
ELSE
1 MB !
THEN
REMAININGB MXSPDB @ >
IF
MXSPDB @ MB @ * DB !
ELSE
REMAININGB MB @ * DB !
THEN
DB @ POSB @ + POSB !
POSB @ SERVOB
THEN
MOVEDONEC? NOT
IF
MOVEDIRC?
IF
-1 MC !
ELSE
1 MC !
THEN
REMAININGC MXSPDC @ >
IF
MXSPDC @ MC @ * DC !
ELSE
REMAININGC MC @ * DC !
THEN
DC @ POSC @ + POSC !
POSC @ SERVOC
THEN
MOVEDONED? NOT
IF
MOVEDIRD?
IF
-1 MD !
ELSE
1 MD !
THEN
REMAININGD MXSPDD @ >
IF
MXSPDD @ MD @ * DD !
ELSE
REMAININGD MD @ * DD !
THEN
DD @ POSD @ + POSD !
POSD @ SERVOD
THEN
MOVEDONEE? NOT
IF
MOVEDIRE?
IF
-1 ME !
ELSE
1 ME !
THEN
REMAININGE MXSPDE @ >
IF
MXSPDE @ ME @ * DE !
ELSE
REMAININGE ME @ * DE !
THEN
DE @ POSE @ + POSE !
POSE @ SERVOE
THEN
MOVEDONEF? NOT
IF
MOVEDIRF?
IF
-1 MF !
ELSE
1 MF !
THEN
REMAININGF MXSPDF @ >
IF
MXSPDF @ MF @ * DF !
ELSE
REMAININGF MF @ * DF !
THEN
DF @ POSF @ + POSF !
POSF @ SERVOF
THEN
MOVEDONEG? NOT
IF
MOVEDIRG?
IF
-1 MG !
ELSE
1 MG !
THEN
REMAININGG MXSPDG @ >
IF
MXSPDG @ MG @ * DG !
ELSE
REMAININGG MG @ * DG !
THEN
DG @ POSG @ + POSG !
POSG @ SERVOG
THEN
MOVEDONEH? NOT
IF
MOVEDIRH?
IF
-1 MH !
ELSE
1 MH !
THEN
REMAININGH MXSPDH @ >
IF
MXSPDH @ MH @ * DH !
ELSE
REMAININGH MH @ * DH !
THEN
DH @ POSH @ + POSH !
POSH @ SERVOH
THEN
MOVEDONEI? NOT
IF
MOVEDIRI?
IF
-1 MI !
ELSE
1 MI !
THEN
REMAININGI MXSPDI @ >
IF
MXSPDI @ MI @ * DI !
ELSE
REMAININGI MI @ * DI !
THEN
DI @ POSI @ + POSI !
POSI @ SERVOI
THEN
MOVEDONEJ? NOT
IF
MOVEDIRJ?
IF
-1 MJ !
ELSE
1 MJ !
THEN
REMAININGJ MXSPDJ @ >
IF
MXSPDJ @ MJ @ * DJ !
ELSE
REMAININGJ MJ @ * DJ !
THEN
DJ @ POSJ @ + POSJ !
POSJ @ SERVOJ
THEN
; EEWORD
: PERIODINIT
32767 PWMA0 PWM-PERIOD
32767 PWMB0 PWM-PERIOD
32767 TD0 PWM-PERIOD
TD0 ACTIVE-HIGH
32767 TD1 PWM-PERIOD
TD1 ACTIVE-HIGH
32767 TD2 PWM-PERIOD
TD2 ACTIVE-HIGH
32767 TC0 PWM-PERIOD
TC0 ACTIVE-HIGH
32767 TC1 PWM-PERIOD
TC1 ACTIVE-HIGH
32767 TA0 PWM-PERIOD
TA0 ACTIVE-HIGH
32767 TA1 PWM-PERIOD
TA1 ACTIVE-HIGH
32767 TA2 PWM-PERIOD
TA2 ACTIVE-HIGH
32767 TA3 PWM-PERIOD
TA3 ACTIVE-HIGH
32767 TB0 PWM-PERIOD
TB0 ACTIVE-HIGH
32767 TB1 PWM-PERIOD
TB1 ACTIVE-HIGH
32767 TB2 PWM-PERIOD
TB2 ACTIVE-HIGH
32767 TB3 PWM-PERIOD
TB3 ACTIVE-HIGH
; EEWORD
: RCINIT
PDEF0 @ POS0 !
PDEF0 @ CMDPOS0 !
PDEF0 @ SERVO0
VDEF0 @ MXSPD0 !
ADEF0 @ ACC0 !
0 D0 !
PDEF1 @ POS1 !
PDEF1 @ CMDPOS1 !
PDEF1 @ SERVO1
VDEF1 @ MXSPD1 !
ADEF1 @ ACC1 !
0 D1 !
PDEF2 @ POS2 !
PDEF2 @ CMDPOS2 !
PDEF2 @ SERVO2
VDEF2 @ MXSPD2 !
ADEF2 @ ACC2 !
0 D2 !
PDEF3 @ POS3 !
PDEF3 @ CMDPOS3 !
PDEF3 @ SERVO3
VDEF3 @ MXSPD3 !
ADEF3 @ ACC3 !
0 D3 !
PDEF4 @ POS4 !
PDEF4 @ CMDPOS4 !
PDEF4 @ SERVO4
VDEF4 @ MXSPD4 !
ADEF4 @ ACC4 !
0 D4 !
PDEF5 @ POS5 !
PDEF5 @ CMDPOS5 !
PDEF5 @ SERVO5
VDEF5 @ MXSPD5 !
ADEF5 @ ACC5 !
0 D5 !
PDEF6 @ POS6 !
PDEF6 @ CMDPOS6 !
PDEF6 @ SERVO6
VDEF6 @ MXSPD6 !
ADEF6 @ ACC6 !
0 D6 !
PDEF7 @ POS7 !
PDEF7 @ CMDPOS7 !
PDEF7 @ SERVO7
VDEF7 @ MXSPD7 !
ADEF7 @ ACC7 !
0 D7 !
PDEF8 @ POS8 !
PDEF8 @ CMDPOS8 !
PDEF8 @ SERVO8
VDEF8 @ MXSPD8 !
ADEF8 @ ACC8 !
0 D8 !
PDEF9 @ POS9 !
PDEF9 @ CMDPOS9 !
PDEF9 @ SERVO9
VDEF9 @ MXSPD9 !
ADEF9 @ ACC9 !
0 D9 !
PDEF10 @ POS10 !
PDEF10 @ CMDPOS10 !
PDEF10 @ SERVO10
VDEF10 @ MXSPD10 !
ADEF10 @ ACC10 !
0 D10 !
PDEF11 @ POS11 !
PDEF11 @ CMDPOS11 !
PDEF11 @ SERVO11
VDEF11 @ MXSPD11 !
ADEF11 @ ACC11 !
0 D11 !
PDEF12 @ POS12 !
PDEF12 @ CMDPOS12 !
PDEF12 @ SERVO12
VDEF12 @ MXSPD12 !
ADEF12 @ ACC12 !
0 D12 !
PDEF13 @ POS13 !
PDEF13 @ CMDPOS13 !
PDEF13 @ SERVO13
VDEF13 @ MXSPD13 !
ADEF13 @ ACC13 !
0 D13 !
PDEF14 @ POS14 !
PDEF14 @ CMDPOS14 !
PDEF14 @ SERVO14
VDEF14 @ MXSPD14 !
ADEF14 @ ACC14 !
0 D14 !
PDEFA @ POSA !
PDEFA @ CMDPOSA !
PDEFA @ SERVOA
VDEFA @ MXSPDA !
ADEFA @ ACCA !
0 DA !
PDEFB @ POSB !
PDEFB @ CMDPOSB !
PDEFB @ SERVOB
VDEFB @ MXSPDB !
ADEFB @ ACCB !
0 DB !
PDEFC @ POSC !
PDEFC @ CMDPOSC !
PDEFC @ SERVOC
VDEFC @ MXSPDC !
ADEFC @ ACCC !
0 DC !
PDEFD @ POSD !
PDEFD @ CMDPOSD !
PDEFD @ SERVOD
VDEFD @ MXSPDD !
ADEFD @ ACCD !
0 DD !
PDEFE @ POSE !
PDEFE @ CMDPOSE !
PDEFE @ SERVOE
VDEFE @ MXSPDE !
ADEFE @ ACCE !
0 DE !
PDEFF @ POSF !
PDEFF @ CMDPOSF !
PDEFF @ SERVOF
VDEFF @ MXSPDF !
ADEFF @ ACCF !
0 DF !
PDEFG @ POSG !
PDEFG @ CMDPOSG !
PDEFG @ SERVOG
VDEFG @ MXSPDG !
ADEFG @ ACCG !
0 DG !
PDEFH @ POSH !
PDEFH @ CMDPOSH !
PDEFH @ SERVOH
VDEFH @ MXSPDH !
ADEFH @ ACCH !
0 DH !
PDEFI @ POSI !
PDEFI @ CMDPOSI !
PDEFI @ SERVOI
VDEFI @ MXSPDI !
ADEFI @ ACCI !
0 DI !
PDEFJ @ POSJ !
PDEFJ @ CMDPOSJ !
PDEFJ @ SERVOJ
VDEFJ @ MXSPDJ !
ADEFJ @ ACCJ !
0 DJ !
; EEWORD
: INITDEF
1 ADEF0 !
1 ADEF1 !
1 ADEF2 !
1 ADEF3 !
1 ADEF4 !
1 ADEF5 !
1 ADEF6 !
1 ADEF7 !
1 ADEF8 !
1 ADEF9 !
1 ADEF10 !
1 ADEF11 !
1 ADEF12 !
1 ADEF13 !
1 ADEF14 !
1 ADEFA !
1 ADEFB !
1 ADEFC !
1 ADEFD !
1 ADEFE !
1 ADEFF !
1 ADEFG !
1 ADEFH !
1 ADEFI !
1 ADEFJ !
10 VDEF0 !
10 VDEF1 !
10 VDEF2 !
10 VDEF3 !
10 VDEF4 !
10 VDEF5 !
10 VDEF6 !
10 VDEF7 !
10 VDEF8 !
10 VDEF9 !
10 VDEF10 !
10 VDEF11 !
10 VDEF12 !
10 VDEF13 !
10 VDEF14 !
10 VDEFA !
10 VDEFB !
10 VDEFC !
10 VDEFD !
10 VDEFE !
10 VDEFF !
10 VDEFG !
10 VDEFH !
10 VDEFI !
10 VDEFJ !
5000 PDEF0 !
5000 PDEF1 !
8000 PDEF4 !
2000 PDEF5 !
1000 PDEF6 !
8000 PDEF7 !
5000 PDEF10 !
5000 PDEF11 !
3000 PDEFC !
0 PDEFD !
5000 PDEFE !
5000 PDEFF !
5000 PDEFG !
5000 PDEFH !
5000 PDEFI !
5000 PDEFJ !
; EEWORD
: RAZD
0 D0 !
0 D1 !
0 D2 !
0 D3 !
0 D4 !
0 D5 !
0 D6 !
0 D7 !
0 D8 !
0 D9 !
0 D10 !
0 D11 !
0 D12 !
0 D13 !
0 D14 !
0 DA !
0 DB !
0 DC !
0 DD !
0 DE !
0 DF !
0 DG !
0 DH !
0 DI !
0 DJ !
; EEWORD
: RAZDJ
0 D0 !
0 D1 !
0 D2 !
0 D3 !
0 D4 !
0 D5 !
0 D6 !
0 D7 !
0 D8 !
0 D9 !
0 D10 !
0 D11 !
0 REMDEC0 !
0 REMDEC1 !
0 REMDEC2 !
0 REMDEC3 !
0 REMDEC4 !
0 REMDEC5 !
0 REMDEC6 !
0 REMDEC7 !
0 REMDEC8 !
0 REMDEC9 !
0 REMDEC10 !
0 REMDEC11 !
; EEWORD
: RAZDT
0 D12 !
0 D13 !
0 D14 !
0 REMDEC12 !
0 REMDEC13 !
0 REMDEC14 !
; EEWORD
: RAZDB
0 DA !
0 DB !
0 DC !
0 DD !
0 DE !
0 DF !
0 DG !
0 DH !
0 DI !
0 DJ !
0 REMDECA !
0 REMDECB !
0 REMDECC !
0 REMDECD !
0 REMDECE !
0 REMDECF !
0 REMDECG !
0 REMDECH !
0 REMDECI !
0 REMDECJ !
; EEWORD
: RQJA
.» 0 « D0 @ . POS0 @ . CMDPOS0 @ . MXSPD0 @ . REMAINING0 . CR
.» 1 « D1 @ . POS1 @ . CMDPOS1 @ . MXSPD1 @ . REMAINING1 . CR
.» 2 « D2 @ . POS2 @ . CMDPOS2 @ . MXSPD2 @ . REMAINING2 . CR
.» 3 « D3 @ . POS3 @ . CMDPOS3 @ . MXSPD3 @ . REMAINING3 . CR
.» 4 « D4 @ . POS4 @ . CMDPOS4 @ . MXSPD4 @ . REMAINING4 . CR
.» 5 « D5 @ . POS5 @ . CMDPOS5 @ . MXSPD5 @ . REMAINING5 . CR
.» 6 « D6 @ . POS6 @ . CMDPOS6 @ . MXSPD6 @ . REMAINING6 . CR
.» 7 « D7 @ . POS7 @ . CMDPOS7 @ . MXSPD7 @ . REMAINING7 . CR
.» 8 « D8 @ . POS8 @ . CMDPOS8 @ . MXSPD8 @ . REMAINING8 . CR
.» 9 « D9 @ . POS9 @ . CMDPOS9 @ . MXSPD9 @ . REMAINING9 . CR
.» 10 « D10 @ . POS10 @ . CMDPOS10 @ . MXSPD10 @ . REMAINING10 . CR
.» 11 « D11 @ . POS11 @ . CMDPOS11 @ . MXSPD11 @ . REMAINING11 . CR
; EEWORD
: RQTO
.» 12 « D12 @ . POS12 @ . CMDPOS12 @ . MXSPD12 @ . REMAINING12 . CR
.» 13 « D13 @ . POS13 @ . CMDPOS13 @ . MXSPD13 @ . REMAINING13 . CR
.» 14 « D14 @ . POS14 @ . CMDPOS14 @ . MXSPD14 @ . REMAINING14 . CR
; EEWORD
: RQBR
.» A « DA @ . POSA @ . CMDPOSA @ . MXSPDA @ . REMAININGA . CR
.» B « DB @ . POSB @ . CMDPOSB @ . MXSPDB @ . REMAININGB . CR
.» C « DC @ . POSC @ . CMDPOSC @ . MXSPDC @ . REMAININGC . CR
.» D « DD @ . POSD @ . CMDPOSD @ . MXSPDD @ . REMAININGD . CR
.» E « DE @ . POSE @ . CMDPOSE @ . MXSPDE @ . REMAININGE . CR
.» F « DF @ . POSF @ . CMDPOSF @ . MXSPDF @ . REMAININGF . CR
.» G « DG @ . POSG @ . CMDPOSG @ . MXSPDG @ . REMAININGG . CR
.» H « DH @ . POSH @ . CMDPOSH @ . MXSPDH @ . REMAININGH . CR
.» I « DI @ . POSI @ . CMDPOSI @ . MXSPDI @ . REMAININGI . CR
.» J « DJ @ . POSJ @ . CMDPOSJ @ . MXSPDJ @ . REMAININGJ . CR
; EEWORD
: RQ0
D0 @ . POS0 @ . CMDPOS0 @ . MXSPD0 @ . REMAINING0 .
; EEWORD
: RQ1
D1 @ . POS1 @ . CMDPOS1 @ . MXSPD1 @ . REMAINING1 .
; EEWORD
: RQ2
D2 @ . POS2 @ . CMDPOS2 @ . MXSPD2 @ . REMAINING2 .
; EEWORD
: RQ3
D3 @ . POS3 @ . CMDPOS3 @ . MXSPD3 @ . REMAINING3 .
; EEWORD
: RQ4
D4 @ . POS4 @ . CMDPOS4 @ . MXSPD4 @ . REMAINING4 .
; EEWORD
: RQ5
D5 @ . POS5 @ . CMDPOS5 @ . MXSPD5 @ . REMAINING5 .
; EEWORD
: RQ6
D6 @ . POS6 @ . CMDPOS6 @ . MXSPD6 @ . REMAINING6 .
; EEWORD
: RQ7
D7 @ . POS7 @ . CMDPOS7 @ . MXSPD7 @ . REMAINING7 .
; EEWORD
: RQ8
D8 @ . POS8 @ . CMDPOS8 @ . MXSPD8 @ . REMAINING8 .
; EEWORD
: RQ9
D9 @ . POS9 @ . CMDPOS9 @ . MXSPD9 @ . REMAINING9 .
; EEWORD
: RQ10
D10 @ . POS10 @ . CMDPOS10 @ . MXSPD10 @ . REMAINING10 .
; EEWORD
: RQ11
D11 @ . POS11 @ . CMDPOS11 @ . MXSPD11 @ . REMAINING11 .
; EEWORD
: RQ12
D12 @ . POS12 @ . CMDPOS12 @ . MXSPD12 @ . REMAINING12 .
; EEWORD
: RQ13
D13 @ . POS13 @ . CMDPOS13 @ . MXSPD13 @ . REMAINING13 .
; EEWORD
: RQ14
D14 @ . POS14 @ . CMDPOS14 @ . MXSPD14 @ . REMAINING14 .
; EEWORD
: RQA
DA @ . POSA @ . CMDPOSA @ . MXSPDA @ . REMAININGA .
; EEWORD
: RQB
DB @ . POSB @ . CMDPOSB @ . MXSPDB @ . REMAININGB .
; EEWORD
: RQC
DC @ . POSC @ . CMDPOSC @ . MXSPDC @ . REMAININGC .
; EEWORD
: RQD
DD @ . POSD @ . CMDPOSD @ . MXSPDD @ . REMAININGD .
; EEWORD
: RQE
DE @ . POSE @ . CMDPOSE @ . MXSPDE @ . REMAININGE .
; EEWORD
: RQF
DF @ . POSF @ . CMDPOSF @ . MXSPDF @ . REMAININGF .
; EEWORD
: RQG
DG @ . POSG @ . CMDPOSG @ . MXSPDG @ . REMAININGG .
; EEWORD
: RQH
DH @ . POSH @ . CMDPOSH @ . MXSPDH @ . REMAININGH .
; EEWORD
: RQI
DI @ . POSI @ . CMDPOSI @ . MXSPDI @ . REMAININGI .
; EEWORD
: RQJ
DJ @ . POSJ @ . CMDPOSJ @ . MXSPDJ @ . REMAININGJ .
; EEWORD
MACHINE SERVJ EEWORD
ON-MACHINE SERVJ
APPEND-STATE BGJ EEWORD
IN-STATE BGJ CONDITION MOVEDONJ? NOT CAUSES
MAJ
REDLED OFF
THEN-STATE BGJ
TO-HAPPEN IN-EE
IN-STATE BGJ CONDITION MOVEDONJ? CAUSES
RAZDJ
REDLED ON
THEN-STATE BGJ
TO-HAPPEN IN-EE
BGJ SET-STATE INSTALL SERVJ
MACHINE SERVT EEWORD
ON-MACHINE SERVT
APPEND-STATE BGT EEWORD
IN-STATE BGT CONDITION MOVEDONET? NOT CAUSES
MAT
YELLED OFF
THEN-STATE BGT
TO-HAPPEN IN-EE
IN-STATE BGT CONDITION MOVEDONET? CAUSES
RAZDT
YELLED ON
THEN-STATE BGT
TO-HAPPEN IN-EE
BGT SET-STATE INSTALL SERVT
MACHINE SERVB EEWORD
ON-MACHINE SERVB
APPEND-STATE BGB EEWORD
IN-STATE BGB CONDITION MOVEDONB? NOT CAUSES
MAB
GRNLED OFF
THEN-STATE BGB
TO-HAPPEN IN-EE
IN-STATE BGB CONDITION MOVEDONB? CAUSES
RAZDB
GRNLED ON
THEN-STATE BGB
TO-HAPPEN IN-EE
BGB SET-STATE INSTALL SERVB
INITDEF
PERIODINIT
RCINIT
SAVE-RAM
I have not found anything specific yet, other than the driving words for the servos appears to have a different divisor for the Timer pins, compared to the PWM pins, for example :
: SERVO0 9600 10000 */
2200 +
PWMA0 PWM-OUT
; EEWORD
compared with
: SERVO12 3800 1000 */
5500 +
TD0 PWM-OUT
; EEWORD
The PWM pin is getting a result that had been divided by 10000 where the Timer is getting a 1000 divisor, when all pins should be getting some number in the range between 0 and 65535 for PWM-OUT. I'm wondering if those servos on the timer pins are getting an out of range number, and trying to make the best of estimate.
bcrepet
12-09-06, 04:08 AM
Sorry, I forgot to say I changed the timer servo's divisors.
I tried that to see if it would work better if there were less iterations in the movement. But it doesn't... The movement for servo's on timer pins is not good weather the period is set to 50000 or 32767 or the servo's divisor is set to 1000 or 10000...
Only tried bits of the code, and cannot find anything specific that would be causing the problem, but may not be following usage well.
In just driving individual servos, the only time I get jitters is when the servo is being driven beyond it's range. This occurs with both PWM pins and Timer pins.
I'd also been advised that insufficient power can cause servos to act strangely. This would seem to affect both PWM and Timer driven servos.
It appears in the program that the servos are being sent the same pulses, and therefore should operate the same.
I've tried to look at how different parts are changing the numbers to be sent to the pins, but have followed the logic well. I do see the PWM pins grouped together in the MAJ word, where there is also a MAT word for one set of Timer pins, and a MAB word for another set. Perhaps an experiment might be to mix a Timer pin in with the PWM pins in MAJ, to see if that servo reacts well when going through the additional math functions associated with the PWM pins.
Only other matter seen is that each MACHINE is being started after its states declared. I do not know if this would cause a problem, but it might be another experiment to run the SET-STATE and INSTALLs all after all machines have been defined, something like :
MACHINE SERVJ ...
...
MACHINE SERVT ...
...
MACHINE SERVJ ...
...
BGJ SET-STATE INSTALL SERVJ
BGT SET-STATE INSTALL SERVT
BGB SET-STATE INSTALL SERVB
I am curious about the RQJA RQTO and RQBR words. They appear to be test display words, but it seems they should not work.
My idea is that ." 0 " may have been intended where .» 0 « appears in RQJA, and continues in those definitions.
bcrepet
12-13-06, 04:09 PM
I tried to insert 3 timer pins in the MAJ word and to make some movements: This changes nothing, while the servos connected to PWM move perfectly, the servos connected to timer pins jitter while they move from a position to another.
I am sure the power is not the problem, because all the servos use the same battery. It is also sure the problem doesn't come from the servos because I made the try to change the pin connection between servos: I always have this strange movement for the servos connected to timer pins.
With a 1000 divisor for a servo connected to a timer pin, I also tried to make a movement step by step without using the state machine and looking at the theoric position for each step (with the appropriate display word). While the theoric position is correct for each step, the movement of the servo doesn't look to be the same from a step to another, this would explain the strange movement, but what about the irregularity from a step to another?
English is not my mother language (I'm french ;)) and I am not sure that I perfectly understood your message. You tried a part of my code with one servo and you got servo's jittering only when it was out of range but not while the movement? I tryed to put only one servo in MAB but the problem is still there...
The Isomax version of my servopod is 0.6. Would this help to find where the problem comes from?
The display words RQJA, RQTO and RQBR work correctly when I use them. the " or » looks to have the same effect when copied in the terminal window...
I looked at the hexapod servo code and tried to understand it the best I could, but I fear I am not trained enough to Isomax to use this kind of code!
Programming is for me a leisure, I am begining with this servo control code to make (later) an other program for a small walking robot.
Thank you for your help.
There are some errors that I've been working through in the code posted :
In the variable definitions at the top REMDECx is duplicated, removed later repeats.
RCINIT was resetting the Pod when attempted, found that PDEF2, PDEF3, PDEF8, PDEF9, PDEF12, PDEF13, PDEF14, PDEFA, PDEFB have not been initialized to a value in INITDEF before attempted use in RCINIT, probably causing reset ( seeing IsoMax 0.6 in terminal window after a word has been run usually indicates reset )
Probably unrelated to machine operation :
In attempting loading .» x « load errors appear. Replaced all » and « in report words with " as shown in display definitions. Also error in RQJA spelling on REMAINING3 causes load error, fixed.
I've flashed in V 0.6 of IsoMax to run from the same base , but have not gotten working code yet. I've also tried MAJ or MAT to check and usually have a reset, but do not with the MAB word. I may not be understanding how the servos are being repositioned, but the rest conditions should be taken care of first.
There have been some interesting effects seen when working through the code. I've added the needed PDEFx values for initializing them (picked 5000 as it seemed to be used most). And made sure all SERVOx words had a 10000 divisor, otherwise they would command a pulse too wide for the servo to respond to.
I found that RCINIT had to be divided up to keep from resetting the Pod. I made one entry called RCINIT0 that sets the first 12 entries corresponding to the PMW pins, and made a second word called RCINIT1 that corresponds to setting all timers. Running each of these words one after the other allowed them to set the values, where running them together caused a reset.
There are still resets that are associated now with running the MAJ, MAT and MAB words. Decoding the logic therein may take some time, but there is some concern over a stack over-run.
vBulletin v3.0.7, Copyright ©2000-2013, Jelsoft Enterprises Ltd.