ServoSpy: tuning without PC software

ManAtWork · 2025-08-16 17:35

I just started another small hardware project with the P2. I have to tune the control loops of Chinese servo drives. The cheaper ones don't support autotuning and there is often no PC software provided. Or if it is it's only in Chinese language and the manuals are really poor.

But fortunatelly, for the P2 it's no big deal to simply tap the Step/Dir command signals and the encoder cable. The idea is to compare both position values and provide some sort of scope display of nominal vs. actual velocity and following error.

Step/Dir is simple, there's a smart pin mode for this (%01101 = P_REG_UP_DOWN). The absolute encoder signal is a bit more tricky but not really hard to hack. I found out that the magnetic 23-bit absolute encoders use the same protocol as some older optical encoders from Tamagawa Seiki I already knew. It's called "SmartAbs" and uses a 2.5MBd serial communication via an RS485 line.

CON
  _xtlfreq = 25_000_000         ' KISS board Xtal
  _clkfreq = 200_000_000        ' clock frequency

  pinDp     = 33  ' RS485 D+
  pinDm     = 32  ' RS485 D-
  pinOut    = 31  ' Test output

  pinStep   = 41  ' Step input
  pinDir    = 40  ' Dir input


VAR
  byte  rxCog
  long  stepCmd

OBJ

PUB main ()
  wrpin (pinDir, P_SCHMITT_A)
  pinstart (pinStep, P_REG_UP_DOWN + P_MINUS1_B + P_SCHMITT_A, 0, 0)
  rxCog:= coginit (NEWCOG, @StartRx, @dataRq)
  repeat
    eofSig:= 1
    repeat while eofSig>0
    stepCmd:= rdpin (pinStep)
    debug (UHEX_BYTE_ARRAY (@dataRq, 7), UHEX_LONG (encPos), UHEX_LONG (stepCmd))
    waitms (100)

DAT
' SmartAbs Encoder Data transmitted over RS485 line @2.5MBd
' See Tamagawa Seiki docs for TS5700N8501 (17 bit or 23 bit)
dataRq        byte 0      ' request byte
dataCf        byte 0      ' control field
dataSf        byte 0      ' status field
dataDf        byte 0,0,0  ' position data
dataCrc       byte 0      ' CRC field
eofSig        byte 0      ' end of frame signal
encPos        long 0

DAT
  ORG
StartRx       fltl   #pinDp        ' reset smart pin
              wrpin  ModeRx,#pinDp ' async serial RX
              wxpin  BaudRx,#pinDp ' 2.5MBd 8N1
              drvl   #pinDp

newFrame      mov    rxCnt,#7
              mov    ptrb,ptra
newByte       mov    time,Timeout ' timeout 50us
waitRx        testp  #pinDp wc
        if_nc djnz   time,#waitRx
        if_nc jmp    #newFrame    ' if timeout -> start over
              rdpin  rx,#pinDp
              shr    rx,#24
              rolbyte newPos,rx,#0
              wrbyte rx,ptrb++    ' write byte to buffer
              djnz   rxCnt,#newByte
              movbyts newPos,#%00011011
              shl    newPos,#9    ' 23 bits -> 32 bits left justified
              sub    newPos,oldPos
              add    oldPos,newPos
              sar    newPos,#9
              add    pos,newPos   ' handle rollover/multi-turn
              wrlong pos,ptra[2]  ' write 32 bit position
              wrbyte #0,ptra[7]   ' write EOF flag

dropByte      mov    time,Timeout ' timeout 50us
waitGap       testp  #pinDp wc
        if_nc djnz   time,#waitGap
        if_nc jmp    #newFrame
              rdpin  rx,#pinDp    ' ignore more bytes until inter-frame gap
              jmp    #dropByte


ModeRx  long  P_ASYNC_RX + P_MINUS1_B + P_COMPARE_AB
BaudRx  long  80<<16 + 7 ' 2.5MBd 8N1
Timeout long  200*50/8 ' timeout 50us
oldPos  long  0
pos     long  0

rxCnt   RES 1
rx      RES 1
time    RES 1
newPos  RES 1

This little program can at least display the nominal and actual position. The next challange is to do proper scaling and offset correction. The servo uses some sort of so called "electronic gear" to scale 10,000 step pulses per revolution to 2^23 encoder counts.