# 答 sandal 刀庫應用編碼器溢位
[TOC]
建議先看猜測問題發生原因。
# 情況描述
> 我們是用海德漢Endat (單圈13bit，多圈12bit)
我曲線測試是用TS5700(單23bit 多16bit)去測試，我沒有海德漢馬達，那時是另一位同事測試，沒有留圖。
>
>我懷疑是Endat整個加起來25bit，上位跟驅動器拿的是0x6064(32bit)，溢位時下次重新開機0x6064就整個錯誤。
>
>開啟ABS_MUTI，測試多圈數往負方向移動時，超過0，會從UINT32_MAX開始減少。但重新開機時，圈數變成52010範圍內。這可能導致0x6064出現問題。
>
>以上是我的想法。
> 刀庫 不是用PP 是CSP
一般用CSV 控制時，是讀0x6064的值做回授。所以要做到0x6064可以接收到改動。
## 訊息整理
* abs encoder type
* TS5700 tamagawa Tformat
* 海德漢 endat
首先是幾個事實
* encoder underflow 時，
* actual position 不明原因的跳變
* 在 encoder 沒跳，也沒有actual position 也沒有over, or under flow的時候.
* 坤宗也不知。
* abs encoder 的值確實在開機前後如遇期的truncate
* 見圖，關機前，關機後。 65831--> 295 (65831-65535=296)
*
# 找code處
刀庫的應用上更像是 Turn Table ，只是週期不再是pulse per revolution，而是pulse per track cycle。
turn table 相關集中在 `MotionControl` 的 `PositionMonitor28335`, `PositionModeControl28335`, `SetRotaryRange28335`, `SetTurntableStation28335`,
下面一些 `GetCANNPosStatusWord28335`，其它module 可以得到此 module 資料，本module 和cia402應該蠻有關的。
台中是開 profile position (PP mode)，做刀庫。
可參考 `MotionProfile.h` 的 `PPCTRLWORD` 。 在整份code 裡，有在 `UpdatePosSet` ，和各個profile type 裡的 entry 。 Profile 會在 `ProfileExecute` 裡執行。而 `ProfileExecute` 會在 `PositionModeControl28335` 被 call。
`ProfileExecute` 應該是控制要執行 PP mode 的那一個（下一個）block。並且有產生每一瞬間的位置，case `PROFILE_STAGE_EXECUTE`:`ExecuteLinear`。
建議controlword 選relative mode，這樣target position 才是增量的。
profile 的implement 是那種在database裡寫死的array
`_SetNextTask28335` called by `_SetNextTask` called by `UpdatePosSet` called by `SetPosCtrl` called by `PosFunction` called by `PositionModeControl28335` in `BLOCK_MODE` case. 這個應該就是指PP mode。
`PosFunction` 幾乎全部都是 `SetPosCtrl` ，所以這個function 應該也是流程控制相關的，第一次規劃完後就不再工作。
>注意下方還有 `PROFILE_VELOCITY` (PV mode)。雖然PV mode 應該和position 毫無關係。
`ModifyPosition` 有relative 的 position implement，
```c=477
if(pTask->cw.b.mode == RELATIVE) /* relative position */
{
pThis->lPos = pThis->lSetPos;
if(pThis->nLast != -1)
pThis->lAbsPos = pLast->lAbsPos + pThis->lSetPos;
else
pThis->lAbsPos = PosCtrl.lPosTarget + pThis->lSetPos;
}
else if(pTask->cw.b.mode == ABSOLUTE)
```
可以看出 `lSetPos` 應該是主要的user input。`_SetTargetPos28335` called by `_SetTargetPos` called by `SetPosTargetPos1` in DB 也驗證這點。
大致上說，我覺得上面這段code 是有一點問題的，但不至於頻繁發生。當AbsPos 是 接近 64bit 上下限時，必須要reset，否則溢位。
不太可能常發生的原因在於已經是64bit，且為有sign
另一個可能出現的問題在於，position loop 和 encoder feedback，如果 絕對型 encoder 的feedback 值沒有做reset 或，取其相對值之類的動作，就會可能產生bug，尚未看該部份。
# 猜測問題發生原因
目前還沒辦法確定發生當下在那一環出問題。但是綜合所有資訊，我們認為最可能發生的原因如下。由於問題的描述是，當絕對型編碼器溢位的當下，不會跳任何 error，接下來的工作也是正常的。問題是當關機之後再開，機器發生誤會自己的位置。 這比較可能的現象是，由於 `lAbsPos` 有64bit>> 絕對型編碼器精度(assume 65535(16bit))，當encoder溢位發生時，由於 `lAbsPos` 有64bit，且驅動器設定為relative mode， 所以只需要知道差值，應該是有做相關處理 e.g. 上1tick 是65532 這一個tick 是5，差值大於10000(hyper parameter)，所以差值是 5-65532+65535=8。`lAbsPos` 則會記錄為65532+8=65540
假設此時突然斷電。
但是當斷電再開後，不論驅動器是否記錄 65540，因為驅動器不會知道關機狀態下，馬達有沒有移動，所以都要從encoder 重新讀值（絕對值編碼器的意義），並視此值為絕對位置（絕對值編碼器），所以`lAbsPos`會被設為5。
以下再假設刀庫跑一個cycle 是 800 。斷電前真實所在的位置就是65540%800=740。然後斷電後，卻變成5，所以我們觀察者會認為位置跑掉。除非編碼器的溢位值，剛好是刀庫cycle的倍數 e.g. 771。 65540%771=5 (same)
## 標準的解法
我們要分析一下絕對值編碼器和增量型編碼器有何應用上的差異。一般來說，絕對型編碼器會被用在範圍有距離極限的工況，比如CNC的單軸。旋轉的case 則是手臂，此時手臂的旋轉角度也是有極限的比如+-3圈。
相反的是，增量型編碼器常被用在**無限循環**之類的工況，比如單向不間斷的輸送帶、將360 deg/拆成多個工位的旋轉盤。
兩者選用的差異就在無限循環。因為絕對型編碼器是一定有範圍限制的，且範圍越大的，價格也越貴。當絕對型編碼器會溢位時，它的本質就和增量型編碼器一樣了。
所以，以刀庫這個應用來說，其實一般是用增量型編碼器。
增量型編碼器用在任何位置控制上，開電就要homing。所以雖然我們是用絕對型編碼器，但因為我們的工況其實是增量型的工作，所以建議**開機homing**。
> 週期性工作的有週期，但是單位不同，e.g. 鏈狀刀庫可能是800cm ，轉盤則是360 deg
## 其它解法
如果就是希望不做homing ，並且有絕對型編碼器了話，能不能不做homing。
### 編碼器支援
是否能達成取決於幾個條件
1. 刀庫cycle < encoder overflow value，否則同一個值，對應n個可能的刀庫位置。(1-multi isn't fulfill function definition)
2. 同上面猜測問題點 的尾部，編碼器的溢位值 是 刀庫cycle的倍數，由於刀庫cycle是機械無法改變。只能動編碼器的溢位值。
1. 由於驅動器斷電時永遠不知道實際的機器發生什麼，所以一但上電就要 **無條件相信** 編碼器的值，所以修改encoder 溢位值的工作一定影響在編碼器上。
1. 如果編碼器可以設定溢位值，可以完美達成。
2. 如果編碼器可以設定位置值，不能設定溢位值，則若在驅動器斷電情況下overflow, underflow，仍會出錯。除非能保證不溢位。
* e.g. tamagawa t-format 用rs485, 要有電池才能工作。
3. 絕對型編碼器也不一定能調。
1. 如果絕對型編碼器的做法是純機械式的。就一定無法調整
1. 有部份電子式的理論上可以調整，但重裝電池後就非要homing 不可。e.g. tamagawa
#### 部份支援
編碼器支援是很困難的，驅動器的程式才可控。cia402 有 607C home offset。如果在overflow的瞬間，將跳變的差值算出並修改此值。效果同 第 編碼器支援-2-1-2 點。斷電不溢位了話可行。 但是cia402 有建議，只在homing mode 修改607C。
> NOTE The activation of a new value of the object home offset is manufacturer-specific. It is
recommended to apply the new value only while the drive is in homing mode.
### 增加規範
cia402 本身對encoder 的類型沒有做很多的規範，特別是大部份都假設是增量型而非絕對型的。
> * 607Ch: Home offset
> 對abs encoder來說，一般的流程就是在裝機後，因為不確定encoder的位置(第幾圈)，先用一般的homing 模式+sensor 設定儲存607C的值。下次開機以後都不再需要homing 的流程。直接用此607C值和encoder的讀值相加就知道目前座標。
驅動器無法記憶 abs encoder overflow的原因在於，它要無條件相信abs encoder 的input value。如果驅動器可以做判斷，則可以做一些操作。
如果加上幾個假設/條件，
* 事先知道encoder 的overflow value(encoder 多少bit)
* cia402 無此規範 register
* 最後一次存到ROM裡的絕對位置，和下一次開機間的誤差足夠小。
* 隱含假設斷電後，位置的移動量很小。
* 有不同的做法
* best: 能夠在**斷電時但完全斷電前**，保存最後的絕對位置座標(flash)。
* may work: 週期性的存值(e.g.3s)，只要差距夠小就沒事。
* 更好的方法可能是足夠密集的確認，但是只有在與前值有足夠大的差距（足以影響判斷時）才寫入（減少浪費效能，延長flash壽命）。
* 刀庫cycle < encoder overflow value
* 否則會1對多，非function。
如此一來，就能在開機上電時，先把儲存值與abs encoder值相比較，當兩值相差過大時，先將 儲存值/overflow value=y， ans=y\*overflow value +encoder value，if 儲存值~=ans，視為回復成功。
## 結論
~~綜合以上，homing 是最直接有效，保證適用任何的驅動器的方法。~~
~~另外建議要有 zero signal 確保刀庫繞了一圈，我這邊不太確定sandal 有沒有這個功能，不然就是如果旋轉很多次週期後，自動重做 homing動作。~~
我們的目的不是用任意的驅動器都可以兼容。而是只要自己的驅動器，而且我們目標就是要賣整套刀庫，而非單純用刀庫。所以不需考慮相容性問題。
另外梁ㄅㄟ他們真正想用的是5軸上的C軸有絕對型，所以一定要達成。
# 修改
## 52010 ABS_MULTITURN_REV setting.
同理 52007, 52009 都是abs encoder setting.
據說sandal 有這個功能。`52010 ABS_MUTI`。
> 51020 QEP_FPGA_EN
> 絕對值encoder 有BISS 協定。
> EnDat2.2
control.c裡
```c=1786
{{4096}, _DWORD, 52010, STATUSWRITE, 2, 0, 0xFFFFFFFF, NULL, SetEncoderType}, //EncoderRevSet //AUSTON_PARAMETER_ENCODER_ABS_MULTITURN_REV,
```
從 control.c `EncoderRevSet`, 和 `_SetAbsRevolution` 看起來，`ENCODER_ABS_MULTITURN_REV` 並非是直接擴展絕對值編碼器的數值範圍。而是問abs encoder 機械多圈值的上限為何 `u_wAbsMultiReso`(default 12)就4096。同理，`u_wAbsSingleReso`，就是abs encoder 單圈有多少pulse。 `ENCODER_PERIOD_PER_REV` 還不太知道幹麻，看起來也是incremental 才遇到。
另一個出現的是 control.c `MultiDistanceCheck`。
Montion control 則有 `SetEncRotaryRange`, `MultiControl28335`, `CheckPositionRange`。
`AUSTON_PARAMETER_ENCODER_ABS_MULTITURN_REV` ，使用上就call `DBVALUE(ENCODER_ABS_MULTITURN_REV)`
另有 `52023` for `AUSTON_PARAMETER_ENCODER_GET_ABS_MULTITURN_REV`，讀值。但看起來對 ABSOLUTEENCODER 目前無作用。
看起來和 `database.c` `POSITION_MULTICOUNT`有關。就連到`SetMultiRemainder28335` call `SetMultiCnt` call `_SetMultiPos` (`u_AbsMultiPosition = dwPos`)。
### 50064 ENCODERMULTI (not in TMDC yet)
`AUSTON_ACTUALDATA_ENCODERMULTI` implement at `_AbsEncoder_28377.c`
```c=4440
DWORD _GetAbsMulti377()
{
return (u_AbsEncoder.dwLoopCount / u_AbsEncoder.Encoder.wEnPolePairs);
}
```
`wEnPolePairs` 可能是某些encoder 有特別的週期性，其abs loop count 記錄的是換算之前的值。對於大部份的encoder，應該是都沒有用。此值在DB裡是 `52030`, `ENCODER_ENPOLEPAIRS`。此值目前看到都是1。
但只要就是要追 `dwLoopCount`。所以看 `encoderconf.h`, `_AbsEncoder_28377.c` 就對了.
`ReadAbsEncoder`,
`void _InitAbsLoop()`
```c=4393
else if (GetABSType() == ENCODER_TAMAGAWA)
{
u_AbsEncoder.dwLoopCount = u_dwMultiPosition & ((1L << u_wAbsMultiReso) - 1);
if (u_bPositionAdd)
{
if (u_AbsEncoder.dwLoopCount < (1L << (u_wAbsMultiReso - 1)))
u_AbsEncoder.dwLoopCount = u_AbsEncoder.dwLoopCount + (1L<<u_wAbsMultiReso);
}
}
```
`void _SetMultiAbsCom(BOOL bFlag) `其 flag 就是 `POWER_FUNTIONBIT`
```c
if (GetABSType() != ENCODER_HIPERFACE)
_SetMultiAbsCom((DBVALUE(POWER_FUNTIONBIT).dwData & MULTI_ABS)?1:0);
#endif
}
```
```c
{{0}, _DWORD, 51020, STATUSWRITE, 2, 0, 0xFFFFFFFF, NULL, SetFunctionBit}, //AUSTON_PARAMETER_POWER_FUNTIONBIT,
```
這應該是一個register 去決定要不要開各個功能(by different bit)。
### 0x51020 function bit
`MontionControl.c`, `CheckPositionRange()`
可以看出來，`MULTI_ABS`, 可能才是真正的extend abs encoder 功能。
```c=3477
if ((lMinPos > 0 && lMaxPos < 0) || (lMinPos > lTemp && lMaxPos < lTemp))
{
if (!(DBVALUE(POWER_FUNTIONBIT).dwData & MULTI_ABS))
SetDriveError(AUSTONE_POSITION_OVER_RANGE);
}
```
見驅動器datasheet
> P51020 驱动器功能选择
Bit0（ CAN_enable） ： 通讯使能,通过通讯使能驱动器。
Bit8（ ABS_Muti） ： 修改绝对位置(360 度转盘禁止使用)，
用于防止多圈数位于 0 附近， 仅适用于绝对值编码器和位置
模式， 启用后需要重新归零。
Bit11（ POS_TOR） ： 位置到位后限制扭矩， 用于较大转盘
机械定位后防止扭矩较大， 而导致的机械上的磨损或者伺服
的报警
Bit13（ END_SPEED） ： 转盘尾段速度激活
> E00122
位置超过
范围
位置控制中位置范围超过编码器多圈范
围， 启用51018中的bit2可以启用该功能
检查50044编码器数据里面实际编
码器多圈数， 若接近0或4096圈(以
马达实际多圈数为准)， 可以分离机
械结构速度模式运行一段时间或激
活51020参数中的ABS_Muti功能，
激活改功能后(sick编码器会自清除)
建议驱动器重启后再次归零
不過目前本來就是有開啟的，沒有開的話，encoder overflow 會 報警。
注意和 `_SetMultiAbsCom` 的關係， `_SetMultiAbsCom((DBVALUE(POWER_FUNTIONBIT).dwData & MULTI_ABS)?1:0);` 就是把`MULTI_ABS` 轉到給encoder 用變為`u_bPositionAdd`。換句話說，可以用這個variable 追multi abs功能。
### 58558 AUSTON_PARAMETER_POSITION_MULTICOUNT
這個值只用在內部的計算，不能設定。應該就是為了事先的 encoder 值處理。總之處理在這裡。
### abs encoder
`_SetMultiAbsCom` 和 `_ReadAbsEncoder` 看起來是用`u_wInitFlag`去dispatch start reading, read success 後就回到 0。
`58514`(`24676`) `POSITION_ACTUAL_VALUE` 是實際位置，也就是最重要的參數。 對應是 MotionControl.c `g_lActLuPosition`
`56008`, `AUSTON_PARAMETER_IDENTIFICATION_ENCODER_ACTUALPOSITION` 是 `當前位置`。 from `Encoder.GetPosition()`。
58514 和 56008 的差異是什麼？
_GetAbsPosition 用到的 `u_AbsMultiPosition` 就在`58558`，一般都是0。
`50064` `ENCODERMULTI`
function bit 打開之後才有開啟。
### save
要做abs 回復一定要有save
看到資料是存在eeprom，找 `Write_EEPROM` 只有 `WORD SaveDB()` 。這是存整個DB，而且只能在motor switch off 時存(blocking function)。
發現有 driver function
`WORD Save_EEPROM(WORD wAddress, int* pnData, WORD wLength)`. database doesn't use this function. directly used in `control.c`, `MotionControl.c`
`void Homeing() ` 有 `Save_EEPROM(DBID(POSITION_MULTICOUNT)*2, (int*)(&DBVALUE(POSITION_MULTICOUNT)), 2);` 除此之外，`Save_EEPROM(DBID(` 開頭的沒有什麼東西。
所以就是原source code 不可能有辦法做斷電回復。這也和經驗相符，不管underflow, overflow，一但發生，因為沒有存actual position 或類似的東西 所以一定會fail。
如果要做了話，我這邊要動的東西有。
1. 在main loop裡週期執行saving. e.g.10s（假設所有控制都在ISR裡執行）。存 POSITION_MULTICOUNT
1. encoder initial 時，和儲存的 POSITION_MULTICOUNT 比較判斷。
# encoder.c
`encoder.h` from `platform.h` from `_AbsEncoder_28377.h` form `encoderconf.h`
`WORD Set_Encoder(ENCODER* pEncoder)` 會bind 1個encoder implement e.g. abs encoder。 然後control.c 會call 這個binding，接下來所有模組要 position value 都用 `DWORD Get_ActPosition()`，包括 MotionControl.c。對於abs encoder，此時此function bind 到 `_AbsEncoder_28377.c`的 `_GetAbsPosition`。
但是有點炸了
讀取動作現在是用 `void EncoderAbsRead()` call `void _SetMultiAbsCom(BOOL bFlag) `。 I think this is because abs encoder require request and response, which different from other encoder.
`AUSTON_PARAMETER_ENCODER_FINERESOLUTION`, `ENCODER_FINERESOLUTION`, `wFineResoBit`.
# sandal 實作解法
* 發現 `u_AbsPosition` 只有32bit，對於39bit encoder(16+23)永遠會有overflow.
* 它實作的方式不是直接用 u_dwPosition 和 u_AbsPosition 換算。
* 而是用 u_AbsEncoder.dwLoopCount 記錄絕對圈數，此圈數用 single turn(lMechAngle)的overflow & underflow 達成，只有initial 時有參考。
* 單圈則是用lMechAngle，所以resolution永遠被壓縮到一定的range。
* 那它如何斷電回復abs encoder 的值?
## scope
一般用CSV 控制時，是讀0x6064的值做回授。所以要做到0x6064可以接收到改動。
0x6064的直實值
u_dwPosiActValue = Get_ActPosition();
最終就是 `Encoder.GetPosition()`
但是我內部如果知道目前的位置超過32bit max 上限了 e.g. 43E
我要怎樣把這個值傳給上位機？如果用0x6064了話。由其是 CSP target position 只能給 ABS value。看來真的要64bit ，最好的方法就是開額外的register。
如果有週期性了話，最好是直接回到原位置就reset ，這樣32bit也一定夠。不過這就是402 protocol 的弱項。而且control loop 如果沒有對應的處理也是白搭，overflow 時。
> 要注意0x6064 , 402 protocal 裡是Integer32。但是sandal 裡是uint32。
目前是說可以做在上位機了。問題一下就解決。
關於encoder 16bit 被 壓縮到12bit。這不會有問題。 e.g. 65535%4096==4095 16bit overflow 時，12bit 也會剛好overflow。從外面看起來，就是abs multi encoder 變成12bit而已。
只要real abs encoder bit >=12bit 就好，如果是8bit就會莫名在255就overflow。此時建議在驅動器端做expand功能。
---
我在另一邊證明type3 用週期性最好，也有描述如何做，就看要不一個 pesudo code。
implement問題要額外考慮在於如果6064 overflow問題，因為此假設功能做在上位機，無法reset 6064，如何處理。其實就是把現在的6064當作encoder value，上位機虛擬一個經過60F2, 607B 設定，會reset的6064。
drive 6064本來就會遇到abs encoder overflow。同樣的方法用在上位機6064 處理就好。e.g. value overflow 時真正的count up value多少就加上而已。結合前面的斷電回復處理斷電overflow。
Sign in with Wallet
Connect another wallet