Annotated RC Servo Code

[g_jilek]

Here is a highly annotated copy of RMD's RC Servo Code. I Did this annotation as part of my FORTH learning process. This code gives great insight into the inner workings of the PWM Generators and the Quad Timers while also providing a large number of basic coding examples for working with FORTH.

Applying what I've learned so far in FORTH I added an analog front-end to the servo code. I am using a potentiometer to control the position of an RC servo. I've also used a State Machine to monitor the pot by looking at the ADC Status Register.

I am having a blast learning FORTH. It makes sense! I hope these coding examples will assist others in crossing over to the FORTH Dimension.




( ANALOG CONTROLED RC SERVO CODE

HEX

: HALFSPEEDCPU F413 FA1 ! \ (CLKGEN: PLLDB Reg /2, IPbus 20Mhz) Set Loss of Ref
\ Tmr to Max, Leave PLL Divide-By Set to 19 80Mhz
82 F00 ! ; EEWORD \ (SCI0: SCIBR Reg, 9600 baud)

\ 0123456789012345 * THIS COMMENT LINE IS 80 CHARACTERS LONG * 34567890123456789

HALFSPEEDCPU


\ RC Servo Front-End Code

C3 CONSTANT PWMCTL# EEWORD
\ (PWM Control Reg value; PRSC[1:0] /8,set LDOK, set PWM_EN)

VARIABLE LOSTOP 4E2 LOSTOP ! EEWORD \ (4E2=1250=1mS)
( VARIABLE LOSTOP 2E4 LOSTOP ! EEWORD \ (2E4=740=0.6mS)

VARIABLE HISTOP 9C4 HISTOP ! EEWORD \ (9C4=2500=2mS)
( VARIABLE HISTOP B9A HISTOP ! EEWORD \ (B9A=2970=2.4mS)


VARIABLE PER# 61A8 PER# ! EEWORD ( 61A8 for 50 Hz, 5161 for 60 Hz
\ (PWM Counter Modulo Register Value) This Value Determines the PWM Period
\ PWM Period = (PWM Modulus) x (PWM Clock Period w/prescaler) x 2


VARIABLE SCALE# 3E8 SCALE# ! EEWORD \ (SCALE# = 1000 DECIMAL)


: MIDWAY HISTOP @ LOSTOP @ + 2/ ; EEWORD


: 8* 2* 2* 2* ; EEWORD ( 8 * WITHOUT THE SPEED PENALTY

: SETPWM ( PWM_Value PWM_Channel# -- )
DUP 6 U<
IF
E00 \ Determines PWMA or PWMB
ELSE
6 - \ Loads PWM Value Registers (PWMVAL0-5)
E20 \ Conditions and Loads PWM Control Reg (PMCLT)
THEN
>R R@ 6 + + ! R@ @ DROP PWMCTL# R> !
; EEWORD

: SETTMRCMP ( PWM_Value PWM_Channel# -- ) \ Loads Timer Compare Registers
8* D00 + 2DUP 1+ ! PER# @ ROT - SWAP ! ; EEWORD
\ PWM_Value Loaded into CMP2,this is the On-Time Value
\ The Off-Time is Calculated and Loaded into CMP1

: RC_INIT \ Sets up the PWM Generators PWMA E00-E1F and PWMB E20-E3F
8000 E03 ! \ PWM Output Control Reg(PMOUT),sets Output Pad Enable(PAD_EN)-Bit 15
0 E0D !
0 E0E ! \ clears PWM Disable Mapping Registers (PMDISMAP1-2)
000E E0F ! \ PWM Configure Register (PMCFG), sets (INDEP) Bits, Center Default
PER# @ E05 ! \ PWM Counter Modulo Register (PWMCM)
8000 E23 !
0 E2D ! \ Sets up PWMB
0 E2E !
000E E2F !
PER# @ E25 !

0C 0 \ Loads Midway Value into the PWM Generators
DO
MIDWAY I SETPWM
LOOP

\ Sets up Quad Timers for PWM Output Assuming Halfspeed CPU
0C 0 \ Defines Variable-Frequency PWM Mode
DO \ Control Registers (CTRL), Count Mode 001, Prescale /16, Count til
3824 I 8* D06 + ! \ Compare,Toggle OFLAG using Alternate Compare Registers
MIDWAY I SETTMRCMP \ Loads Midway Value into the TMR-PWM Generators
0001 I 8* D07 + ! \ Status & Control Registers (SCR),
LOOP \ sets Output Enable (OEN)-Bit 0
; EEWORD

RC_INIT

: RC ( Position from 0 TO SCALE#, PWM_Channel# 0-1D -- PWM_Value PWM_Channel# )
SWAP 0 MAX SCALE# @ MIN HISTOP @ LOSTOP @ - SCALE# @ */ LOSTOP @ + SWAP
0 MAX 1C MIN
DUP 0C U<
IF
SETPWM \ Determines if PWM_Channel# is PWMA-B or TMRA-D
ELSE
0C - SETTMRCMP \ Stack Values are Passed to either SETPWM or SETTMRCMP
THEN
; EEWORD

: RC? ( PWM_Channel# -- PWM_Value ) \ Places the Scaled Value on the Stack
0 MAX 1C MIN
DUP 0C U<
IF
DUP 6 U< IF E06 ELSE E20 THEN +
ELSE
0C - 8* D01 +
THEN
@ LOSTOP @ - SCALE# @ HISTOP @ LOSTOP @ - */
; EEWORD

: .RC? ( PWM_Channel# -- ) \ Displays the Scaled Value
0 MAX 1C MIN
DUP 0C U<
IF
DUP 6 U< IF E06 ELSE E20 THEN +
ELSE
0C - 8* D01 +
THEN
@ LOSTOP @ - SCALE# @ HISTOP @ LOSTOP @ - */ .
; EEWORD

\ Analog Front-End Code

: ADC_INIT ( -- ) \ Sets Up the Initial ADC Configuration
2000 0E80 ! \ Sets Start Bit & Once Sequential Scan Mode
0003 0E81 ! \ Maintains 2.5Mhz Default ADC Clock with HALFSPEEDCPU
3210 0E83 ! \ Default Scan Order
7654 0E84 ! \ Default Scan Order
0002 0E85 ! \ Sample Disable Register, Only Channel 0 is Scanned
; EEWORD

ADC_INIT

VARIABLE PREVADC0 00 PREVADC0 ! EEWORD \ Previous Value of ADC0

1000 CONSTANT 12BIT EEWORD
03E8 CONSTANT RCSCALE EEWORD
0003 CONSTANT PWM_3 EEWORD

: STARTCNV ( -- ) \ Provides Start Pulse to Initiate an ADC Conversion
2000 0E80 ! ; EEWORD

: 8/ 2/ 2/ 2/ ; EEWORD ( n -- n ) \ Fast 8 /, Right Shift

: READADC0 ( -- ) \ Passes Stack Values to RC & Updates Servo Position
E89 @ DUP PREVADC0 ! 8/ RCSCALE 12BIT */ PWM_3 RC ; EEWORD

: ADC0>< ( -- ) \ Tests to see if the ADC0 Value has Changed
PREVADC0 @ E89 @ - ABS DUP 0= NOT IF 28 > IF READADC0 THEN ELSE DROP THEN
STARTCNV ; EEWORD \ Nested IF Statements Add Hystresis

: ADC0_RDY ( -- f ) \ Tests ADC Status Register (ADSTAT) (RDY0)-Bit 0
E86 @ 0001 AND ; EEWORD

DECIMAL

\ State Machine Code

MACHINE CHECKPOT EEWORD
ON-MACHINE CHECKPOT
APPEND-STATE ADC0POT EEWORD

IN-STATE ADC0POT CONDITION ADC0_RDY
CAUSES ADC0>< THEN-STATE ADC0POT TO-HAPPEN IN-EE

: ANALOG_RC
HALFSPEEDCPU
RC_INIT
ADC_INIT
ADC0POT SET-STATE
EVERY 50000 CYCLES SCHEDULE-RUNS CHECKPOT
; EEWORD

HEX 7C00 AUTOSTART ANALOG_RC

SAVE-RAM

The RC Stack comment wraped funny. Copying int Notepad will clean this up. I also tried to attach the file.

Good luck,
g_jilek

[RMDumse]

You might want to check

EVERY 500000 CYCLES SCHEDULE-RUNS CHECKPOT

It looks like you are compiling in HEX, and since the time you specify 500000 is used for a hex number, which can only have xxxx digits of significance, the lower 0000 is what is truncated onto the stack. In the earliest IsoMax(TM) versions we didn't protect against this (which would try to run to fast). Now we check for a minimum time for things to be scheduled.

[g_jilek]

Would the following modification take care of this?

: ANALOG_RC
HEX
HALFSPEEDCPU
RC_INIT
ADC_INIT
DECIMAL
ADC0POT SET-STATE
EVERY 500000 CYCLES SCHEDULE-RUNS CHECKPOT
; EEWORD

Is CYCLES limited to a 16-Bit value?

Thanks for the feedback, I really appreciate it.

g_jilek

[nmitech]

Is CYCLES limited to a 16-Bit value?

Yes, CYCLES is limited to a 16-bit value.
The least number of CYCLES is DECIMAL 10 and max number is DECIMAL 65535. So you need to change the cycle on your example also.

[RMDumse]

Better, but not right yet. 50000 is an okay decimal number but when you started compiling this word, you were in hex. The HEX and DECIMAL that are >INSIDE< this definitions are compiled, and not run now, but instead, when ANALOG_RC is run later. During compiling, you stay in HEX, so again with 50000, only the xxxx is kept so again you have 0000 passed to SCHEDULE-RUNS and it bumps it up to a minimum of 10 on the later versions of IsoMax(TM)

Here's probably what you want...

DECIMAL
: ANALOG_RC
HALFSPEEDCPU
RC_INIT
ADC_INIT
ADC0POT SET-STATE
EVERY 50000 CYCLES SCHEDULE-RUNS CHECKPOT
; EEWORD

You see, DECIMAL is run before it goes into compiling mode, so when you translate 50000 it is translated in the base last executed, i.e. DECIMAL before compilation of the colon definition started, and the SCHEDULE-RUNS gets a decimal 50000, or a hex C350, which is a valid number between 10 and FFFF.

CHECKPOT runs every 50000 * 8 (8 for the system prescaler) IP clocks, which means it runs every 400,000 IP clocks, and with the IP at 20MHz HALFSPEEDCPU that means 50 times a second, same as the RC Servo update rate.

[g_jilek]

I was just reading about how a definition compiles with the last stated radix. I see what is happening there now.

I am not seeing where the 8* Precaler is coming from. I see the precaler in the PWM generator register, PWMCTL, but I'm not seeing how it could be affecting the machine cycle time.

Thanks again for the feedback,

g_jilek

[RMDumse]

In the system timer that schedules the machine chain to run, we added a prescaler of "8". TMRD3 is used for the system timer, by the way. So it counts a "cycle" (as in "EVERY xxx CYCLES") as 8 IP clocks. This was to move the range where chains could be scheduled from a very few IP clocks out to 65536 * 8 or 76 times a second at 40MHz. The 8 prescaler seemed to give the wides range of likely schedulings which might be desired for system design.

[g_jilek]

So TMRD3 is consumed by the system and should not be considered for project design. If one attempts to use TMRD3 with RC would that overwrite the prescaler? The RC Servo adapter board provides a connection to TMRD3, if the above is true we should not use it. Right?

[RMDumse]

Right. We have to have something to create the "heartbeat" of the system, and the last timer in the timer modules was chosen. You should avoid using TMRD3. Perhaps that will be changed in the future. But for now, TMRD3 is the system timer.

[-Eyeman-]

I have an error and can´t send the programm to the isobot.


IsoMax V0.3
( ANALOG CONTROLED RC SERVO CODE OK
OK
HEX OK
OK
: HALFSPEEDCPU F413 FA1 ! \ (CLKGEN: PLLDB Reg /2, IPbus 20Mhz) Set Loss of Ref


what can i do??

thanks ...

[nmitech]

: HALFSPEEDCPU F413 FA1 ! \ (CLKGEN: PLLDB Reg /2, IPbus 20Mhz) Set Loss of Ref

You are misssing the ";" character before the comment "\" character. Also you may limit and try not to exceed 80 characters per line to prevent compiling error in some cases.

[g_jilek]

Originally posted by -Eyeman-
: HALFSPEEDCPU F413 FA1 ! \ (CLKGEN: PLLDB Reg /2, IPbus 20Mhz) Set Loss of Ref
[/B]

The entire HALFSPEEDCPU definition is a little hard see becasuse it is more coment than definition. Here is that chunk of code:

: HALFSPEEDCPU F413 FA1 ! \ (CLKGEN: PLLDB Reg /2, IPbus 20Mhz) Set Loss of Ref
\ Tmr to Max, Leave PLL Divide-By Set to 19 80Mhz
82 F00 ! ; EEWORD \ (SCI0: SCIBR Reg, 9600 baud)

\ 0123456789012345 * THIS COMMENT LINE IS 80 CHARACTERS LONG * 34567890123456789

When one is using this much commenting within the code the 80 Character Limit that nmtech mentioned must be observed or the code will not compile correctly. Adding the chacacter count line to your text editor will help get the word wrap right.

Good luck,
g_jilek

[nmitech]

This,
: HALFSPEEDCPU F413 FA1 ! \ (CLKGEN: PLLDB Reg /2, IPbus 20Mhz) Set Loss of Ref
\ Tmr to Max, Leave PLL Divide-By Set to 19 80Mhz
82 F00 ! ; EEWORD \ (SCI0: SCIBR Reg, 9600 baud)

is compiled OK with NMITerm V0.9.73 & IsoMax V0.5


but this will cause a compiling error,
\0123456789012345 * THIS COMMENT LINE IS 80 CHARACTERS LONG * 234567890123456789

Every leading comment word "\" or "(" must follow a blank character. A comment within the leading comment does not require a blank character follows it.

\ 0123456789012345 * THIS COMMENT LINE IS 80 CHARACTERS LONG * 23456789012345678
\ 0123456789012345 *(THIS COMMENT LINE IS 80 CHARACTERS LONG * 23456789012345678
( 0123456789012345 *\THIS COMMENT LINE IS 80 CHARACTERS LONG * 23456789012345678

"Adding the chacacter count line to your text editor will help get the word wrap right."
Will consider to add a character count with an option setting for doc file open within the NMITerm window on the next NMITerm release.

[g_jilek]

Thanks nmtech.

I hadn't had my first cup of coffee.

I edited the posts to correct the missing space after the line comment "\" word.

The character count option sounds like a nice feature.

g_jilek

[-Eyeman-]

Ok, my first problem is sloved.

....but the next doesn´t let wait...



IsoMax V0.3
( ANALOG CONTROLED RC SERVO CODE OK
OK
HEX OK
OK
: HALFSPEEDCPU F413 FA1 ! \ (CLKGEN: PLLDB Reg /2, IPbus 20Mhz) Set Loss of Ref
\ Tmr to Max, Leave PLL Divide-By Set to 19 80Mhz
82 F00 ! ; EEWORD \ (SCI0: SCIBR Reg, 9600 baud) OK
OK
\ 0123456789012345 * THIS COMMENT LINE IS 80 CHARACTERS LONG * 34567890123456789 OK
OK
OK
HALFSPEEDCPU OK
OK
OK
\ RC Servo Front-End Code OK
OK
C3 CONSTANT PWMCTL# EEWORD OK
\ (PWM Control Reg value; PRSC[1:0] /8,set LDOK, set PWM_EN)