MSP430-FORTH/test/PID.f

   1 \ PID controller written in Forth
   2 \ Based on the code presented here:
   3 \ http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
   4
   5 \ MSP_EXP430FR5739  MSP_EXP430FR5969    MSP_EXP430FR5994    MSP_EXP430FR6989
   6 \ MSP_EXP430FR4133  MSP_EXP430FR2433    MSP_EXP430FR2355    CHIPSTICK_FR2433
   7
   8 MARKER {PID}
   9
  10 [UNDEFINED] VARIABLE [IF]
  11 \ https://forth-standard.org/standard/core/VARIABLE
  12 \ VARIABLE <name>       --                      define a Forth VARIABLE
  13 : VARIABLE
  14 CREATE
  15 HI2LO
  16 MOV @RSP+,IP
  17 MOV #DOVAR,-4(W)        \   CFA = DOVAR
  18 MOV @IP+,PC
  19 ENDCODE
  20 [THEN]
  21
  22 [UNDEFINED] CONSTANT [IF]
  23 \ https://forth-standard.org/standard/core/CONSTANT
  24 \ CONSTANT <name>     n --                      define a Forth CONSTANT
  25 : CONSTANT
  26 CREATE
  27 HI2LO
  28 MOV TOS,-2(W)           \   PFA = n
  29 MOV @PSP+,TOS
  30 MOV @RSP+,IP
  31 MOV @IP+,PC
  32 ENDCODE
  33 [THEN]
  34
  35 [UNDEFINED] STATE [IF]
  36 \ https://forth-standard.org/standard/core/STATE
  37 \ STATE   -- a-addr       holds compiler state
  38 STATEADR CONSTANT STATE
  39 [THEN]
  40
  41 [UNDEFINED] ROT [IF]
  42 \ https://forth-standard.org/standard/core/ROT
  43 \ ROT    x1 x2 x3 -- x2 x3 x1
  44 CODE ROT
  45 MOV @PSP,W          \ 2 fetch x2
  46 MOV TOS,0(PSP)      \ 3 store x3
  47 MOV 2(PSP),TOS      \ 3 fetch x1
  48 MOV W,2(PSP)        \ 3 store x2
  49 MOV @IP+,PC
  50 ENDCODE
  51 [THEN]
  52
  53 [UNDEFINED] SWAP [IF]
  54 \ https://forth-standard.org/standard/core/SWAP
  55 \ SWAP     x1 x2 -- x2 x1    swap top two items
  56 CODE SWAP
  57 MOV @PSP,W      \ 2
  58 MOV TOS,0(PSP)  \ 3
  59 MOV W,TOS       \ 1
  60 MOV @IP+,PC     \ 4
  61 ENDCODE
  62 [THEN]
  63
  64 [UNDEFINED] DUP [IF]
  65 \ https://forth-standard.org/standard/core/DUP
  66 \ DUP      x -- x x      duplicate top of stack
  67 CODE DUP
  68 BW1 SUB #2,PSP      \ 2  push old TOS..
  69     MOV TOS,0(PSP)  \ 3  ..onto stack
  70     MOV @IP+,PC     \ 4
  71 ENDCODE
  72
  73 \ https://forth-standard.org/standard/core/qDUP
  74 \ ?DUP     x -- 0 | x x    DUP if nonzero
  75 CODE ?DUP
  76 CMP #0,TOS      \ 2  test for TOS nonzero
  77 0<> ?GOTO BW1    \ 2
  78 MOV @IP+,PC     \ 4
  79 ENDCODE
  80 [THEN]
  81
  82 [UNDEFINED] AND [IF]
  83 \ https://forth-standard.org/standard/core/AND
  84 \ C AND    x1 x2 -- x3           logical AND
  85 CODE AND
  86 AND @PSP+,TOS
  87 MOV @IP+,PC
  88 ENDCODE
  89 [THEN]
  90
  91 [UNDEFINED] SPACE [IF]
  92 \ https://forth-standard.org/standard/core/SPACE
  93 \ SPACE   --               output a space
  94 : SPACE
  95 $20 EMIT ;
  96 [THEN]
  97
  98 [UNDEFINED] R> [IF]
  99 \ https://forth-standard.org/standard/core/Rfrom
 100 \ R>    -- x    R: x --   pop from return stack ; CALL #RFROM performs DOVAR
 101 CODE R>
 102 MOV rDOVAR,PC
 103 ENDCODE
 104 [THEN]
 105
 106 [UNDEFINED] @ [IF]
 107 \ https://forth-standard.org/standard/core/Fetch
 108 \ @     c-addr -- char   fetch char from memory
 109 CODE @
 110 MOV @TOS,TOS
 111 MOV @IP+,PC
 112 ENDCODE
 113 [THEN]
 114
 115 [UNDEFINED] ! [IF]
 116 \ https://forth-standard.org/standard/core/Store
 117 \ !        x a-addr --   store cell in memory
 118 CODE !
 119 MOV @PSP+,0(TOS)    \ 4
 120 MOV @PSP+,TOS       \ 2
 121 MOV @IP+,PC         \ 4
 122 ENDCODE
 123 [THEN]
 124
 125 [UNDEFINED] C@ [IF]
 126 \ https://forth-standard.org/standard/core/CFetch
 127 \ C@     c-addr -- char   fetch char from memory
 128 CODE C@
 129 MOV.B @TOS,TOS
 130 MOV @IP+,PC
 131 ENDCODE
 132 [THEN]
 133
 134 [UNDEFINED] 1+ [IF]
 135 \ https://forth-standard.org/standard/core/OnePlus
 136 \ 1+      n1/u1 -- n2/u2       add 1 to TOS
 137 CODE 1+
 138 ADD #1,TOS
 139 MOV @IP+,PC
 140 ENDCODE
 141 [THEN]
 142
 143 [UNDEFINED] + [IF]
 144 \ https://forth-standard.org/standard/core/Plus
 145 \ +       n1/u1 n2/u2 -- n3/u3     add n1+n2
 146 CODE +
 147 ADD @PSP+,TOS
 148 MOV @IP+,PC
 149 ENDCODE
 150 [THEN]
 151
 152 [UNDEFINED] - [IF]
 153 \ https://forth-standard.org/standard/core/Minus
 154 \ -      n1/u1 n2/u2 -- n3/u3     n3 = n1-n2
 155 CODE -
 156 SUB @PSP+,TOS   \ 2  -- n2-n1 ( = -n3)
 157 XOR #-1,TOS     \ 1
 158 ADD #1,TOS      \ 1  -- n3 = -(n2-n1) = n1-n2
 159 MOV @IP+,PC
 160 ENDCODE
 161 [THEN]
 162
 163 [UNDEFINED] MAX [IF]
 164 \ https://forth-standard.org/standard/core/MAX
 165 \ MAX    n1 n2 -- n3       signed maximum
 166 CODE MAX
 167     CMP @PSP,TOS    \ n2-n1
 168     S<  ?GOTO FW1   \ n2<n1
 169 BW1 ADD #2,PSP
 170     MOV @IP+,PC
 171 ENDCODE
 172
 173 \ https://forth-standard.org/standard/core/MIN
 174 \ MIN    n1 n2 -- n3       signed minimum
 175 CODE MIN
 176     CMP @PSP,TOS    \ n2-n1
 177     S< ?GOTO BW1    \ n2<n1
 178 FW1 MOV @PSP+,TOS
 179     MOV @IP+,PC
 180 ENDCODE
 181 [THEN]
 182
 183
 184 [UNDEFINED] 2NIP [IF]
 185 \ 2NIP   d1 d2 -- d2
 186 CODE 2NIP
 187 MOV @PSP,X
 188 ADD #4,PSP
 189 MOV X,0(PSP)
 190 NEXT
 191 ENDCODE
 192 [THEN]
 193
 194 [UNDEFINED] 2DUP  [IF]
 195 \ https://forth-standard.org/standard/core/TwoDUP
 196 \ 2DUP   x1 x2 -- x1 x2 x1 x2   dup top 2 cells
 197 CODE 2DUP
 198 SUB #4,PSP          \ -- x1 x x x2
 199 MOV TOS,2(PSP)      \ -- x1 x2 x x2
 200 MOV 4(PSP),0(PSP)   \ -- x1 x2 x1 x2
 201 NEXT
 202 ENDCODE
 203 [THEN]
 204
 205 [UNDEFINED] 2SWAP [IF]
 206 \ https://forth-standard.org/standard/core/TwoSWAP
 207 \ 2SWAP  x1 x2 x3 x4 -- x3 x4 x1 x2
 208 CODE 2SWAP
 209 MOV @PSP,W          \ -- x1 x2 x3 x4    W=x3
 210 MOV 4(PSP),0(PSP)   \ -- x1 x2 x1 x4
 211 MOV W,4(PSP)        \ -- x3 x2 x1 x4
 212 MOV TOS,W           \ -- x3 x2 x1 x4    W=x4
 213 MOV 2(PSP),TOS      \ -- x3 x2 x1 x2    W=x4
 214 MOV W,2(PSP)        \ -- x3 x4 x1 x2
 215 NEXT
 216 ENDCODE
 217 [THEN]
 218
 219 [UNDEFINED] 2ROT [IF]
 220 \ https://forth-standard.org/standard/double/TwoROT
 221 \ Rotate the top three cell pairs on the stack bringing cell pair x1 x2 to the top of the stack.
 222 CODE 2ROT
 223 MOV 8(PSP),X        \ 3
 224 MOV 6(PSP),Y        \ 3
 225 MOV 4(PSP),8(PSP)   \ 5
 226 MOV 2(PSP),6(PSP)   \ 5
 227 MOV @PSP,4(PSP)     \ 4
 228 MOV TOS,2(PSP)      \ 3
 229 MOV X,0(PSP)        \ 3
 230 MOV Y,TOS           \ 1
 231 NEXT
 232 ENDCODE
 233 [THEN]
 234
 235 [UNDEFINED] 2DROP [IF]
 236 \ https://forth-standard.org/standard/core/TwoDROP
 237 \ 2DROP  x1 x2 --          drop 2 cells
 238 CODE 2DROP
 239 ADD #2,PSP
 240 MOV @PSP+,TOS
 241 MOV @IP+,PC
 242 ENDCODE
 243 [THEN]
 244
 245 [UNDEFINED] 2OVER [IF]
 246 \ https://forth-standard.org/standard/core/TwoOVER
 247 \ 2OVER  x1 x2 x3 x4 -- x1 x2 x3 x4 x1 x2
 248 CODE 2OVER
 249 SUB #4,PSP          \ -- x1 x2 x3 x x x4
 250 MOV TOS,2(PSP)      \ -- x1 x2 x3 x4 x x4
 251 MOV 8(PSP),0(PSP)   \ -- x1 x2 x3 x4 x1 x4
 252 MOV 6(PSP),TOS      \ -- x1 x2 x3 x4 x1 x2
 253 MOV @IP+,PC
 254 ENDCODE
 255 [THEN]
 256
 257 [UNDEFINED] DABS [IF]
 258 \ https://forth-standard.org/standard/double/DABS
 259 \ DABS     d1 -- |d1|     absolute value
 260 CODE DABS
 261 AND #-1,TOS         \ clear V, set N
 262 U< IF               \ if positive (N=0)
 263     XOR #-1,0(PSP)  \ 4
 264     XOR #-1,TOS     \ 1
 265     ADD #1,0(PSP)   \ 4
 266     ADDC #0,TOS     \ 1
 267 THEN
 268 MOV @IP+,PC
 269 ENDCODE
 270 [THEN]
 271
 272 [UNDEFINED] 2@ [IF]
 273     \ https://forth-standard.org/standard/core/TwoFetch
 274     \ 2@    a-addr -- x1 x2    fetch 2 cells ; the lower address will appear on top of stack
 275     CODE 2@
 276     SUB #2,PSP
 277     MOV 2(TOS),0(PSP)
 278     MOV @TOS,TOS
 279     NEXT
 280     ENDCODE
 281 [THEN]
 282
 283 [UNDEFINED] 2! [IF]
 284     \ https://forth-standard.org/standard/core/TwoStore
 285     \ x1 x2 addr --     Store the cell pair x1 x2 at a-addr, with x2 at a-addr and x1 at the next consecutive cell.
 286     CODE 2!
 287     MOV @PSP+,0(TOS)
 288     MOV @PSP+,2(TOS)
 289     MOV @PSP+,TOS
 290     NEXT
 291     ENDCODE
 292 [THEN]
 293
 294 \ https://forth-standard.org/standard/core/TwotoR
 295 \ ( x1 x2 -- ) ( R: -- x1 x2 )   Transfer cell pair x1 x2 to the return stack.
 296 CODE 2>R
 297 PUSH @PSP+
 298 PUSH TOS
 299 MOV @PSP+,TOS
 300 NEXT
 301 ENDCODE
 302
 303 \ https://forth-standard.org/standard/core/TwoRFetch
 304 \ ( -- x1 x2 ) ( R: x1 x2 -- x1 x2 ) Copy cell pair x1 x2 from the return stack.
 305 CODE 2R@
 306 SUB #4,PSP
 307 MOV TOS,2(PSP)
 308 MOV @RSP,TOS
 309 MOV 2(RSP),0(PSP)
 310 NEXT
 311 ENDCODE
 312
 313 \ https://forth-standard.org/standard/core/TwoRfrom
 314 \ ( -- x1 x2 ) ( R: x1 x2 -- )  Transfer cell pair x1 x2 from the return stack
 315 CODE 2R>
 316 SUB #4,PSP
 317 MOV TOS,2(PSP)
 318 MOV @RSP+,TOS
 319 MOV @RSP+,0(PSP)
 320 NEXT
 321 ENDCODE
 322
 323 [UNDEFINED] 2VARIABLE [IF]
 324 \ https://forth-standard.org/standard/double/TwoVARIABLE
 325 : 2VARIABLE \  --
 326 CREATE 4 ALLOT
 327 ;
 328 [THEN]
 329
 330 [UNDEFINED] 2CONSTANT [IF] \ defined if MEM_EXT
 331     \ https://forth-standard.org/standard/double/TwoCONSTANT
 332     : 2CONSTANT \  udlo/dlo/Qlo udhi/dhi/Qhi --         to create double or Q15.16 CONSTANT
 333     CREATE , ,  \ compile Qhi then Qlo
 334     DOES> 2@    \ execution part    addr -- Qhi Qlo
 335     ;
 336 [THEN]
 337
 338 [UNDEFINED] <> [IF]
 339 \ https://forth-standard.org/standard/core/ne
 340 \ =      ( x1 x2 -- flag ) flag is true if and only if x1 is not bit-for-bit the same as x2
 341 CODE <>
 342 SUB @PSP+,TOS   \ 2
 343 0<> IF
 344     MOV #-1,TOS
 345 THEN
 346 NEXT            \ 4
 347 ENDCODE
 348 [THEN]
 349
 350 [UNDEFINED] = [IF]
 351 \ https://forth-standard.org/standard/core/Equal
 352 \ =      x1 x2 -- flag         test x1=x2
 353 CODE =
 354 SUB @PSP+,TOS   \ 2
 355 0<> IF          \ 2
 356     AND #0,TOS  \ 1
 357     MOV @IP+,PC \ 4
 358 THEN
 359 XOR #-1,TOS     \ 1 flag Z = 1
 360 MOV @IP+,PC     \ 4
 361 ENDCODE
 362 [THEN]
 363
 364 \ https://forth-standard.org/standard/core/Uless
 365 \ U<    u1 u2 -- flag       test u1<u2, unsigned
 366 [UNDEFINED] U< [IF]
 367 CODE U<
 368 SUB @PSP+,TOS   \ 2 u2-u1
 369 0<> IF
 370     MOV #-1,TOS     \ 1
 371     U< IF           \ 2 flag
 372         AND #0,TOS  \ 1 flag Z = 1
 373     THEN
 374 THEN
 375 MOV @IP+,PC     \ 4
 376 ENDCODE
 377 [THEN]
 378
 379 \ ------------------------------------------------------------------------------
 380 \ CONTROL STRUCTURES
 381 \ ------------------------------------------------------------------------------
 382 \ THEN and BEGIN compile nothing
 383 \ DO compile one word
 384 \ IF, ELSE, AGAIN, UNTIL, WHILE, REPEAT, LOOP & +LOOP compile two words
 385 \ LEAVE compile three words
 386 \
 387 [UNDEFINED] IF [IF]
 388 \ https://forth-standard.org/standard/core/IF
 389 \ IF       -- IFadr    initialize conditional forward branch
 390 CODE IF       \ immediate
 391 SUB #2,PSP              \
 392 MOV TOS,0(PSP)          \
 393 MOV &DP,TOS             \ -- HERE
 394 ADD #4,&DP            \           compile one word, reserve one word
 395 MOV #QFBRAN,0(TOS)      \ -- HERE   compile QFBRAN
 396 ADD #2,TOS              \ -- HERE+2=IFadr
 397 MOV @IP+,PC
 398 ENDCODE IMMEDIATE
 399 [THEN]
 400
 401 [UNDEFINED] THEN [IF]
 402 \ https://forth-standard.org/standard/core/THEN
 403 \ THEN     IFadr --                resolve forward branch
 404 CODE THEN               \ immediate
 405 MOV &DP,0(TOS)          \ -- IFadr
 406 MOV @PSP+,TOS           \ --
 407 MOV @IP+,PC
 408 ENDCODE IMMEDIATE
 409 [THEN]
 410
 411 [UNDEFINED] ELSE [IF]
 412 \ https://forth-standard.org/standard/core/ELSE
 413 \ ELSE     IFadr -- ELSEadr        resolve forward IF branch, leave ELSEadr on stack
 414 CODE ELSE     \ immediate
 415 ADD #4,&DP              \ make room to compile two words
 416 MOV &DP,W               \ W=HERE+4
 417 MOV #BRAN,-4(W)
 418 MOV W,0(TOS)            \ HERE+4 ==> [IFadr]
 419 SUB #2,W                \ HERE+2
 420 MOV W,TOS               \ -- ELSEadr
 421 MOV @IP+,PC
 422 ENDCODE IMMEDIATE
 423 [THEN]
 424
 425 [UNDEFINED] DEFER! [IF]
 426 \ https://forth-standard.org/standard/core/DEFERStore
 427 \ Set the word xt1 to execute xt2. An ambiguous condition exists if xt1 is not for a word defined by DEFER.
 428 CODE DEFER!             \ xt2 xt1 --
 429 MOV @PSP+,2(TOS)        \ -- xt1=CFA_DEFER          xt2 --> [CFA_DEFER+2]
 430 MOV @PSP+,TOS           \ --
 431 MOV @IP+,PC
 432 ENDCODE
 433 [THEN]
 434
 435 [UNDEFINED] IS [IF]
 436 \ https://forth-standard.org/standard/core/IS
 437 \ IS <name>        xt --
 438 \ used as is :
 439 \ DEFER DISPLAY                         create a "do nothing" definition (2 CELLS)
 440 \ inline command : ' U. IS DISPLAY      U. becomes the runtime of the word DISPLAY
 441 \ or in a definition : ... ['] U. IS DISPLAY ...
 442 \ KEY, EMIT, CR, ACCEPT and WARM are examples of DEFERred words
 443 \
 444 \ as IS replaces the PFA value of any word, it's a TO alias for VARIABLE and CONSTANT words...
 445 : IS
 446 STATE @
 447 IF  POSTPONE ['] POSTPONE DEFER!
 448 ELSE ' DEFER!
 449 THEN
 450 ; IMMEDIATE
 451 [THEN]
 452
 453 [UNDEFINED] >BODY [IF]
 454 \ https://forth-standard.org/standard/core/toBODY
 455 \ >BODY     -- addr      leave BODY of a CREATEd word\ also leave default ACTION-OF primary DEFERred word
 456 CODE >BODY
 457 ADD #4,TOS
 458 MOV @IP+,PC
 459 ENDCODE
 460 [THEN]
 461
 462 \ =============================================================================
 463 \ fixpoint words
 464 CODE F+
 465 BW1 ADD @PSP+,2(PSP)
 466     ADDC @PSP+,TOS
 467     NEXT                \ 4
 468 ENDCODE
 469
 470 CODE F-
 471 BW1 SUB @PSP+,2(PSP)
 472     SUBC TOS,0(PSP)
 473     MOV @PSP+,TOS
 474     NEXT                \ 4
 475 ENDCODE
 476
 477 \ https://forth-standard.org/standard/core/HOLDS
 478 \ Adds the string represented by addr u to the pictured numeric output string
 479 \ compilation use: <# S" string" HOLDS #>
 480 \ free chars area in the 32+2 bytes HOLD buffer = {26,23,2} chars with a 32 bits sized {hexa,decimal,binary} number.
 481 \ (2 supplementary bytes are room for sign - and decimal point)
 482 \ C HOLDS    addr u --
 483 CODE HOLDS
 484 BW3         MOV @PSP+,X     \ 2
 485             ADD TOS,X       \ 1 src
 486             MOV &HP,Y       \ 3 dst
 487 BEGIN       SUB #1,X        \ 1 src-1
 488             SUB #1,TOS      \ 1 cnt-1
 489 U>= WHILE   SUB #1,Y        \ 1 dst-1
 490             MOV.B @X,0(Y)   \ 4
 491 REPEAT      MOV Y,&HP       \ 3
 492             MOV @PSP+,TOS   \ 2
 493             MOV @IP+,PC     \ 4  15 words
 494 ENDCODE
 495
 496 TLV_ORG 4 + @ $81F3 U<
 497 $81EF TLV_ORG 4 + @ U<
 498 = [IF]   ; MSP430FR2xxx|MSP430FR4xxx subfamilies without hardware_MPY
 499
 500
 501 CODE F/                     \ Q15.16 / Q15.16 --> Q15.16 result
 502         PUSHM #4,R7
 503         MOV @PSP+,R6        \ DVRlo
 504         MOV @PSP+,X         \ DVDhi --> REMlo
 505         MOV #0,W            \ REMhi = 0
 506         MOV @PSP,Y          \ DVDlo --> DVDhi
 507         MOV #0,T            \ DVDlo = 0
 508         MOV X,S             \
 509         XOR TOS,S           \ DVDhi XOR DVRhi --> S keep sign of result
 510         AND #-1,X           \ DVD < 0 ?
 511 S< IF   XOR #-1,Y           \ INV(DVDlo)
 512         XOR #-1,X           \ INV(DVDhi)
 513         ADD #1,Y            \ INV(DVDlo)+1
 514         ADDC #0,X           \ INV(DVDhi)+C
 515 THEN    AND #-1,TOS         \ DVR < 0 ?
 516 S< IF   XOR #-1,R6          \ INV(DVRlo)
 517         XOR #-1,TOS         \ INV(DVRhi)
 518         ADD #1,R6           \ INV(DVRlo)+1
 519         ADDC #0,TOS         \ INV(DVRhi)+C
 520 THEN
 521 \ don't uncomment lines below !
 522 \ ------------------------------------------------------------------------
 523 \           UD/MOD    DVDlo DVDhi DVRlo DVRhi -- REMlo REMhi QUOTlo QUOThi
 524 \ ------------------------------------------------------------------------
 525 \           MOV 4(PSP),T    \ DVDlo
 526 \           MOV 2(PSP),Y    \ DVDhi
 527 \           MOV #0,X        \ REMlo = 0
 528 \           MOV #0,W        \ REMhi = 0
 529             MOV #32,R5      \  init loop count
 530 BW1         CMP TOS,W       \ 1 REMhi = DVRhi ?
 531     0= IF   CMP R6,X        \ 1 REMlo U< DVRlo ?
 532     THEN
 533     U>= IF  SUB R6,X        \ 1 no:  REMlo - DVRlo  (carry is set)
 534             SUBC TOS,W      \ 1      REMhi - DVRhi
 535     THEN
 536 BW2         ADDC R7,R7      \ 1 RLC quotLO
 537             ADDC R4,R4      \ 1 RLC quotHI
 538             SUB #1,R5       \ 1 Decrement loop counter
 539             0< ?GOTO FW1    \ 2 out of loop if count<0
 540             ADD T,T         \ 1 RLA DVDlo
 541             ADDC Y,Y        \ 1 RLC DVDhi
 542             ADDC X,X        \ 1 RLC REMlo
 543             ADDC W,W        \ 1 RLC REMhi
 544             U< ?GOTO BW1    \ 2 15~ loop
 545             SUB R6,X        \ 1 REMlo - DVRlo
 546             SUBC TOS,W      \ 1 REMhi - DVRhi
 547             BIS #1,SR       \ 1
 548             GOTO BW2        \ 2 16~ loop
 549 FW1
 550 \           MOV X,4(PSP)    \ REMlo
 551 \           MOV W,2(PSP)    \ REMhi
 552 \           ADD #4,PSP      \ skip REMlo REMhi
 553             MOV R7,0(PSP)   \ QUOTlo
 554             MOV R4,TOS      \ QUOThi
 555             POPM #4,R7      \ restore R4 to R7
 556 \           MOV @IP+,PC     \ end of UD/MOD
 557 \ ------------------------------------------------------------------------
 558 BW1     AND #-1,S           \ clear V, set N; QUOT < 0 ?
 559 S< IF   XOR #-1,0(PSP)      \ INV(QUOTlo)
 560         XOR #-1,TOS         \ INV(QUOThi)
 561         ADD #1,0(PSP)       \ INV(QUOTlo)+1
 562         ADDC #0,TOS         \ INV(QUOThi)+C
 563 THEN    MOV @IP+,PC
 564 ENDCODE
 565
 566 \ F#S    Qlo Qhi u -- Qhi 0   convert fractional part Qlo of Q15.16 fixed point number
 567 \                             with u digits
 568 CODE F#S
 569             MOV 2(PSP),X            \ -- Qlo Qhi u      X = Qlo
 570             MOV @PSP,2(PSP)         \ -- Qhi Qhi u
 571             MOV X,0(PSP)            \ -- Qhi Qlo u
 572             PUSHM #2,TOS            \                   save TOS,IP
 573             MOV #0,S                \ -- Qhi Qlo x
 574 BEGIN       PUSH S                  \                   R-- limit IP count
 575             MOV &BASEADR,TOS        \ -- Qhi Qlo base
 576             LO2HI
 577             UM*                     \                   u1 u2 -- RESlo REShi
 578             HI2LO                   \ -- Qhi RESlo digit
 579             SUB #2,IP
 580             CMP #10,TOS             \                   digit to char
 581     U>= IF  ADD #7,TOS
 582     THEN    ADD #$30,TOS
 583             MOV @RSP+,S             \                       R-- limit IP
 584             MOV.B TOS,HOLDS_ORG(S)  \ -- Qhi RESlo char     char to string
 585             ADD #1,S                \                       count+1
 586             CMP 2(RSP),S            \                       count=limit ?
 587 U>= UNTIL
 588             POPM #2,TOS             \                       restore IP,TOS
 589             MOV #0,0(PSP)           \ -- Qhi 0 len
 590             SUB #2,PSP              \ -- Qhi 0 x len
 591             MOV #HOLDS_ORG,0(PSP)   \ -- Qhi 0 addr len
 592             GOTO BW3                \ jump HOLDS
 593 ENDCODE
 594
 595 \ unsigned multiply 32*32 = 64
 596 \ don't use S reg (keep sign)
 597 CODE UDM*
 598             PUSH IP         \ 3
 599             PUSHM #4,R7     \ 6 save R7 ~ R4 regs
 600             MOV 4(PSP),IP   \ 3 MDlo
 601             MOV 2(PSP),T    \ 3 MDhi
 602             MOV @PSP,W      \ 2 MRlo
 603             MOV #0,R4       \ 1 MDLO=0
 604             MOV #0,R5       \ 1 MDHI=0
 605             MOV #0,4(PSP)   \ 3 RESlo=0
 606             MOV #0,2(PSP)   \ 3 REShi=0
 607             MOV #0,R6       \ 1 RESLO=0
 608             MOV #0,R7       \ 1 RESHI=0
 609             MOV #1,X        \ 1 BIT TEST REGlo
 610             MOV #0,Y        \ 1 BIT TEST2 REGhi
 611 BEGIN       CMP #0,X
 612     0<> IF  BIT X,W         \ 2+1 TEST ACTUAL BIT MRlo
 613     ELSE    BIT Y,TOS       \ 2+1 TEST ACTUAL BIT MRhi
 614     THEN
 615     0<> IF  ADD IP,4(PSP)   \ 2+3 IF 1: ADD MDlo TO RESlo
 616             ADDC T,2(PSP)   \ 3      ADDC MDhi TO REShi
 617             ADDC R4,R6      \ 1      ADDC MDLO TO RESLO
 618             ADDC R5,R7      \ 1      ADDC MDHI TO RESHI
 619     THEN    ADD IP,IP       \ 1 (RLA LSBs) MDlo *2
 620             ADDC T,T        \ 1 (RLC MSBs) MDhi *2
 621             ADDC R4,R4      \ 1 (RLA LSBs) MDLO *2
 622             ADDC R5,R5      \ 1 (RLC MSBs) MDHI *2
 623             ADD X,X         \ 1 (RLA) NEXT BIT TO TEST
 624             ADDC Y,Y        \ 1 (RLA) NEXT BIT TO TEST
 625 U>= UNTIL   MOV R6,0(PSP)   \ 2+2 IF BIT IN CARRY: FINISHED    32 * 16~ (average loop)
 626             MOV R7,TOS      \ 1 high result in TOS
 627             POPM #4,R7      \ 6 restore R4 to R7
 628             MOV @RSP+,IP    \ 2
 629             MOV @IP+,PC
 630 ENDCODE
 631
 632 CODE F*                 \ s15.16 * s15.16 --> s15.16 result
 633     MOV 2(PSP),S        \
 634     XOR TOS,S           \ 1s15 XOR 2s15 --> S keep sign of result
 635     BIT #$8000,2(PSP)   \ MD < 0 ?
 636 0<> IF  XOR #-1,2(PSP)
 637         XOR #-1,4(PSP)
 638         ADD #1,4(PSP)
 639         ADDC #0,2(PSP)
 640 THEN
 641     COLON
 642     DABS UDM*           \ -- RES0 RES1 RES2 RES3
 643     HI2LO
 644     MOV @RSP+,IP
 645     MOV @PSP+,TOS       \ -- RES0 RES1 RES2
 646     MOV @PSP+,0(PSP)    \ -- RES1 RES2
 647     GOTO BW1            \ goto end of F/ to process sign of result
 648 ENDCODE
 649
 650 [ELSE] \ hardware multiplier
 651
 652 CODE F/                     \ Q15.16 / Q15.16 --> Q15.16 result
 653 \ TOS = DVRhi
 654 \ 0(PSP) = DVRlo
 655 \ 2(PSP) = DVDhi
 656 \ 4(PSP) = DVDlo
 657         PUSHM #4,R7         \ 6 PUSHM R7 to R4
 658         MOV @PSP+,R6        \ 2 DVRlo
 659         MOV @PSP+,X         \ 2 DVDhi --> REMlo
 660         MOV #0,W            \ 1 REMhi = 0
 661         MOV @PSP,Y          \ 2 DVDlo --> DVDhi
 662         MOV #0,T            \ 1 DVDlo = 0
 663         MOV X,S             \ 1
 664         XOR TOS,S           \ 1 DVDhi XOR DVRhi --> S keep sign of result
 665         AND #-1,X           \ 1 DVD < 0 ?
 666 S< IF   XOR #-1,Y           \ 1 INV(DVDlo)
 667         XOR #-1,X           \ 1 INV(DVDhi)
 668         ADD #1,Y            \ 1 INV(DVDlo)+1
 669         ADDC #0,X           \ 1 INV(DVDhi)+C
 670 THEN    AND #-1,TOS         \ 1 DVR < 0 ?
 671 S< IF   XOR #-1,R6          \ 1 INV(DVRlo)
 672         XOR #-1,TOS         \ 1 INV(DVRhi)
 673         ADD #1,R6           \ 1 INV(DVRlo)+1
 674         ADDC #0,TOS         \ 1 INV(DVRhi)+C
 675 THEN    MOV #32,R5          \ 2 init loop count
 676 BW1     CMP TOS,W           \ 1 REMhi = DVRhi ?
 677     0= IF                   \ 2
 678         CMP R6,X            \ 1 REMlo U< DVRlo ?
 679     THEN
 680     U>= IF                  \ 2
 681         SUB R6,X            \ 1 no:  REMlo - DVRlo  (carry is set)
 682         SUBC TOS,W          \ 1      REMhi - DVRhi
 683     THEN
 684 BW2     ADDC R7,R7          \ 1 RLC quotLO
 685         ADDC R4,R4          \ 1 RLC quotHI
 686         SUB #1,R5           \ 1 Decrement loop counter
 687         0< ?GOTO FW1        \ 2 out of loop if count<0
 688         ADD T,T             \ 1 RLA DVDlo
 689         ADDC Y,Y            \ 1 RLC DVDhi
 690         ADDC X,X            \ 1 RLC REMlo
 691         ADDC W,W            \ 1 RLC REMhi
 692         U< ?GOTO BW1        \ 2 19~ loop
 693         SUB R6,X            \ 1 REMlo - DVRlo
 694         SUBC TOS,W          \ 1 REMhi - DVRhi
 695         BIS #1,SR           \ 1
 696         GOTO BW2            \ 2 16~ loop
 697 FW1     AND #-1,S           \ 1 clear V, set N; QUOT < 0 ?
 698 S< IF   XOR #-1,R7          \ 1 INV(QUOTlo)
 699         XOR #-1,R4          \ 1 INV(QUOThi)
 700         ADD #1,R7           \ 1 INV(QUOTlo)+1
 701         ADDC #0,R4          \ 1 INV(QUOThi)+C
 702 THEN    MOV R7,0(PSP)       \ 3 QUOTlo
 703         MOV R4,TOS          \ 1 QUOThi
 704         POPM #4,R7          \ 6 restore R4 to R7
 705         MOV @IP+,PC         \ 4
 706 ENDCODE
 707
 708 \ F#S    Qlo Qhi u -- Qhi 0   convert fractionnal part of Q15.16 fixed point number
 709 \                             with u digits
 710 CODE F#S
 711             MOV 2(PSP),X            \ -- Qlo Qhi u      X = Qlo
 712             MOV @PSP,2(PSP)         \ -- Qhi Qhi u
 713             MOV X,0(PSP)            \ -- Qhi Qlo u
 714             MOV TOS,T               \                   T = limit
 715             MOV #0,S                \                   S = count
 716 BEGIN       MOV @PSP,&MPY           \                   Load 1st operand
 717             MOV &BASEADR,&OP2       \                   Load 2nd operand
 718             MOV &RES0,0(PSP)        \ -- Qhi RESlo x        low result on stack
 719             MOV &RES1,TOS           \ -- Qhi RESlo REShi    high result in TOS
 720             CMP #10,TOS             \                   digit to char
 721     U>= IF  ADD #7,TOS
 722     THEN    ADD #$30,TOS
 723             MOV.B TOS,HOLDS_ORG(S)  \ -- Qhi RESlo char     char to string
 724             ADD #1,S                \                   count+1
 725             CMP T,S                 \                   count=limit ?
 726 0= UNTIL    MOV #0,0(PSP)           \ -- Qhi 0 REShi
 727             MOV T,TOS               \ -- Qhi 0 limit
 728             SUB #2,PSP              \ -- Qhi 0 x len
 729             MOV #HOLDS_ORG,0(PSP)   \ -- Qhi 0 addr len
 730             GOTO BW3                \ jump HOLDS
 731 ENDCODE
 732
 733 CODE F*                 \ signed s15.16 multiplication --> s15.16 result
 734     MOV 4(PSP),&MPYS32L \ 5 Load 1st operand
 735     MOV 2(PSP),&MPYS32H \ 5
 736     MOV @PSP,&OP2L      \ 4 load 2nd operand
 737     MOV TOS,&OP2H       \ 3
 738     ADD #4,PSP          \ 1 remove 2 cells
 739 \    NOP2                \ 2
 740 \    NOP2                \ 2 wait 8 cycles after write OP2L before reading RES1
 741     MOV &RES1,0(PSP)    \ 5
 742     MOV &RES2,TOS       \ 5
 743     MOV @IP+,PC
 744 ENDCODE
 745
 746 [THEN]  \ hardware multiplier
 747
 748 CODE F.N            \ ( f n -- ) display a Q15.16 number with n digits after comma
 749 MOV TOS,T           \ T = #digits
 750 MOV @PSP+,TOS
 751 MOV TOS,S           \ S = sign
 752 PUSHM #3,IP         \                   R-- IP sign #digit
 753 LO2HI
 754     <# DABS         \ -- uQlo uQhi      R-- IP sign #digit
 755     R> F#S          \ -- uQhi 0         R-- IP sign
 756     $2C HOLD        \                   $2C = char ','
 757     #S              \ -- 0 0
 758     R> SIGN #>      \ -- addr len       R-- IP
 759     TYPE SPACE      \ --
 760 ;
 761
 762
 763 \ https://forth-standard.org/standard/double/Dless
 764 \ flag is true if and only if d1 is less than d2
 765 CODE D<
 766             MOV @PSP+,S         \ S=d2L
 767             MOV @PSP+,T         \ T=d1H
 768             MOV @PSP+,W         \ W=d1L
 769 BW1         CMP TOS,T           \ 1 d1H - d2H
 770             MOV #0,TOS          \ 1 -- false_flag       by default
 771 S< IF       MOV #-1,TOS         \ 2 -- true_flag        if d1H < d2H
 772 THEN
 773 0= IF       CMP S,W             \ 1 -- false_flag       d1L - d2L
 774     S< IF   MOV #-1,TOS         \ 1 -- true_flag        if (d1H = d2H) & (d1L < d2L)
 775     THEN
 776 THEN
 777 NEXT                            \ 4
 778 ENDCODE
 779
 780 \ : D> 2SWAP D< ;
 781 CODE D>
 782 MOV TOS,T           \ T=d2H
 783 MOV @PSP+,W         \ W=d2L
 784 MOV @PSP+,TOS       \ TOS=d1H
 785 MOV @PSP+,S         \ S=d1L
 786 GOTO BW1
 787 ENDCODE
 788
 789 CODE S2F \ ( s -- f )  Signed number to fixed point
 790     SUB #2,PSP
 791     MOV #0,0(PSP)
 792     MOV @IP+,PC
 793 ENDCODE
 794
 795 : F2S \ ( f -- s )  Fixed point to signed number (rounded)
 796   SWAP $8000 AND IF 1 + THEN ;
 797
 798 : DMIN \ ( d1 d2 -- d_min )  Minimum of double number (also for fixed-point)
 799   2OVER 2OVER
 800   D< IF 2DROP ELSE 2NIP THEN
 801 ;
 802
 803 : DMAX \ ( d1 d2 -- d_max )  Maximum of double number (also for fixed-point)
 804   2OVER 2OVER
 805   D> IF 2DROP ELSE 2NIP THEN
 806 ;
 807
 808 : DRANGE \ ( d_val d_min d_max -- d_val )  Make sure a double number is in range
 809   2ROT DMIN DMAX
 810 ;
 811
 812 : RANGE \ ( s_val s_min s_max -- s_val )  Make sure a number is in range
 813   ROT MIN MAX
 814 ;
 815
 816 : F.000 3 F.N ;  \ Output fixed point value
 817
 818 \ Setup variables for pid control
 819 2VARIABLE KP            \ Proportionnal coeff, scaled to input range.
 820 2VARIABLE KI            \ integral coeff, in second
 821 2VARIABLE KD            \ derivative coeff, in second
 822 VARIABLE SETPOINT       \ setpoint, same scale as input
 823
 824 VARIABLE SAMPLE_TIME    \ sampling interval in ms
 825 VARIABLE OUT_MAX        \ output max limit (--> 20 mA)
 826 VARIABLE OUT_MIN        \ output min limit (--> 4 mA)
 827 VARIABLE OUT-OVERRIDE   \ output override (auto mode if -1)
 828
 829 \ Working variables while pid is running
 830 VARIABLE SET-VAL        \ current setpoint
 831 VARIABLE INPUT_PREV     \ last seen input
 832 2VARIABLE I_SUM         \ cummulative i error
 833
 834 VARIABLE DEBUG          \ PID compute state
 835 0 DEBUG !
 836
 837 : ?DEBUG DEBUG @ ;
 838
 839
 840 \ =============================================================================
 841 \ Main PID - internal definitions (do not call manually)
 842 \ inputs and outputs are 16 bits numbers
 843 \ PID parameters and PID compute are Q15.16 numbers.
 844
 845 : CALC-P \ ( f_error -- f_correction )  Calculate proportionnal output
 846 KP 2@ F*                 \ fetch k-value and scale error
 847 ?DEBUG IF ." Pval:" 2DUP F2S .
 848 THEN
 849 ;
 850
 851
 852 : CALC-I \ ( f_error -- f_correction )  Calculate integral output
 853 KI 2@ F*                \ apply ki factor
 854 I_SUM 2@ F+             \ sum up with running integral error
 855 OUT_MIN @ S2F
 856 OUT_MAX @ S2F
 857 DRANGE \ cap inside output range
 858 2DUP I_SUM 2!           \ update running integral error
 859 ?DEBUG IF  ." Ival:" 2DUP F2S .
 860 THEN
 861 ;
 862
 863 : CALC-D \ ( s_is -- f_correction )  Calculate differential output
 864   \ actually use "derivative on input", not on error
 865   INPUT_PREV @ -           \ substract last input from current input
 866   S2F KD 2@ F*             \ make fixed point, fetch kd factor and multiply
 867 ?DEBUG IF  ." Dval:" 2DUP F2S .
 868 THEN
 869 ;
 870
 871 : PID_COMPUTE \ ( s_is -- s_corr )  Do a PID calculation, return duty-cycle
 872 \  CR ." SET:" SET-VAL @ .  ." IS:"  DUP . \ DEBUG
 873 \ feed error in p and i, current setpoint in d, sum up results
 874 DUP DUP SET-VAL @ SWAP - S2F  \ ( s_is s_is f_error )
 875 2DUP  CALC-P                  \ ( s_is s_is f_error f_p )
 876 2SWAP CALC-I F+               \ ( s_is s_is f_p+i )
 877 ROT   CALC-D F-               \ ( s_is f_p+i+d ) \ substract! derivate on input - not error
 878
 879 F2S                           \ ( s_is s_corr )
 880 ?DEBUG IF  ." OUT:" DUP .
 881 THEN
 882 SWAP INPUT_PREV !             \ Update INPUT_PREV for next run
 883 OUT_MIN @ OUT_MAX @ RANGE     \ Make sure we return something inside PWM range
 884 ?DEBUG IF  ." PWM:" DUP .
 885 THEN
 886 ;
 887
 888 \ =============================================================================
 889 \ Main PID - external interface
 890
 891 : SET \ ( s -- )  Change setpoint on a running pid
 892   SET-VAL ! ;
 893
 894 : TUNING \  ( f_kp f_ki f_kd -- )  Change tuning-parameters on a running pid
 895   \ depends on sampletime, so fetch it, move to fixed-point and change unit to seconds
 896   \ store on return stack for now
 897   SAMPLE_TIME @ S2F 1000,0 F/ 2>R  \
 898
 899   2R@ F/ KD 2!                  \ translate from 1/s to the sampletime
 900   2R> F* KI 2!                  \ translate from 1/s to the sampletime
 901          KP 2! ;
 902
 903 \ Init PID
 904 \ To use in a *reverse acting system* (bigger output value **reduced**
 905 \ input value make sure `kp`, `ki` and `kd` are **all** negative.
 906 \ Starts pid in manual mode (no setpoint set!). Set setpoint and call auto
 907 \ to start the control loop.
 908 : PID-INIT \ ( f_kp f_ki f_kd s_sampletime s_outmin s_outmax  -- )
 909   OUT_MAX !
 910   OUT_MIN !
 911   SAMPLE_TIME !
 912   TUNING
 913   0 OUT-OVERRIDE !         \ Make sure we're in manual mode
 914   CR ." PID initialized - kp:" KP 2@ F.000 ." ki:" KI 2@ F.000 ." kd:" KD 2@ F.000
 915 ;
 916
 917 \ Returns calculated PID value or override value if in manual mode
 918 : PID \ ( s_is -- s_corr )
 919   OUT-OVERRIDE @ -1 = IF   \ we're in auto-mode - do PID calculation
 920     PID_COMPUTE
 921   ELSE                     \ manual-mode! store input, return override value
 922     CR ." SET:" SET-VAL @ .  ." IS:"  DUP .
 923     INPUT_PREV !
 924     OUT-OVERRIDE @
 925     ." PWM:" DUP .
 926   THEN ;
 927
 928 : MANUAL \ ( s -- )  Override output - switches PID into *manual mode*
 929   OUT-OVERRIDE ! ;
 930
 931
 932 : AUTO \ ( -- )  Switch back to auto-mode after manual mode
 933   OUT-OVERRIDE @ -1 <> IF \ only do something if we'r in override mode
 934     \ store current output value as i to let it run smoothly
 935     OUT-OVERRIDE @
 936     OUT_MIN @ OUT_MAX @ RANGE   \ Make sure we return something inside PWM range
 937     S2F I_SUM 2!                \ init I_SUM
 938     -1 OUT-OVERRIDE !
 939   THEN ;
 940
 941 : AUTOHOLD \ ( -- )  Bring PID back to auto-mode after a manual override
 942   INPUT_PREV @ SET-VAL !   \ Use last input as setpoint (no bumps!)
 943   AUTO ;
 944
 945
 946
 947 \ \ ******************************\
 948 \ ASM BACKGROUND                  \
 949 \ \ ******************************\
 950 \ BEGIN
 951 \ \     ...                         \ insert here your background task
 952 \ \     ...                         \
 953 \ \     ...                         \
 954 \     CALL &RXON                  \ comment this line to disable TERMINAL_INPUT
 955 \     BIS &LPM_MODE,SR            \
 956 \ \ ******************************\
 957 \ \ here start all interrupts     \
 958 \ \ ******************************\
 959 \ \ here return all interrupts    \
 960 \ \ ******************************\
 961 \ AGAIN                           \
 962 \ ENDASM                          \
 963 \ \ ******************************\
 964
 965 \ ------------------------------\
 966 CODE STOP                       \ stops multitasking, must to be used before downloading app
 967 \ ------------------------------\
 968     MOV @IP+,PC
 969 ENDCODE
 970
 971 \ ------------------------------\
 972 CODE APP_INIT                   \ this routine completes the init of system, i.e. FORTH + this app.
 973 \ ------------------------------\
 974     MOV @IP+,PC
 975 ENDCODE                               \
 976
 977 \ ------------------------------\
 978 CODE START                      \ this routine replaces WARM and SLEEP default values by these of this application.
 979 \ ------------------------------\
 980 \ MOV #SLEEP,X                    \ replace default background process SLEEP
 981 \ MOV #BACKGROUND,2(X)            \ by RC5toLCD BACKGROUND
 982 \ MOV #WARM,X                     \ replace default WARM
 983 \ MOV #APP_INIT,2(X)              \ by RC5toLCD APP_INIT
 984 \ MOV X,PC                        \ then execute it
 985     MOV @IP+,PC
 986 ENDCODE
 987
 988
 989 ECHO