# #6 增量式encoder latch z
# situation
#6 增量式encoder 如果position mode開啟latch z value功能，會把該值寫入control loop導致第一次latch時有很大的震動
> 1.51020開QEP_FPGA功能，剛開機時第一圈，觸碰到Z相，6064位置會自動校正為0
2.51020不開QEP_FPGA功能，Latch無法抓取Z相當下位置
>目前希望可以以第二點處理，第一點是有困難的。
Latch功能有分IO跟Ethercat方式(Z相)，去存取位置
>但我們Ethercat Latch尚有問題
Ethercat Latch問題 那時與ITRI合作時測試發現
第一點6064位置會自動校正為0，這位導致PosError瞬間變大，導致位置控制會不穩
> Ethercat Latch 是 Touch Probe功能
第一點 是因為他會去把Z相當作位置0
所以就等於把Encoder轉到0就等於Z相
結論，因為method 1 (402正規設定)功能未實現齊全，控制還不穩定。~~不得已採用method 2。~~
method 2 by touch probe也是正規的，新增在2016 ver, ch12，任何mode 底下都會生效。
目前主要走method2
所以看 0x60B8, 0x60B9, 0x60BA, 0x60D0
## 設定
> 60B8設定21
60B9，1=已啟用等待信號，2=Latch到位置存在60BA
21--> 0b10101 --> Trigger with zero impulse signal or position encoder, Enable sampling at positive edge of touch probe 1
[ERN1387](https://datasheetspdf.com/pdf-file/828328/HEIDENHAIN/ERN1387/1)
## 分析
目前任何機器，都沒有用 homing mode。
什麼模式下發生：位置模式。
速度模式 不會有問題。(所以不會影響到 control loop)
目前都先在速度模式下轉一圈，然後才切掉位置模式。
## 大方向解法
* 不符合402
* 速度模式+QEP_FPGA_EN
* 符合402
* homing mode
* 速度模式+touch probe
* 位置模式+touch probe
CiA402 除了homing mode，本來就沒有 可以直接把6064 改變的東西，(reset 到0)。所以任何開QEP功能的方法都是不對的。
再來因為客戶不希望剛開機要做homing，可能是習慣以前手動車床的模式了。（一般情況是應該要homing才對）。所以一定是touch probe。
# code 略看
## metod1
51020 QEP_FPGA
`FPGA_QEP_EN`
--> control.c `Encoder.bFPGAQepEn`
懷疑 \_IncEncoder_28377.c
## method2
`sEntryDesc0x60B8`, register data `ProbeFun`
`ProbeStatus`
全部都在 objdef.c 的 `ProbeFunRun`
# 問題點
我code 看起來，是它會在馬達機械角轉一圈時存值。這很可能是有bug，因為應該要看z 訊號才對。
```c=3219
else
{
if (-(Get_MechAngle() - qAngelLast) > _IQ(0.9375))
{
if(ProbeFun.bit.TriggerType1 == 1 ||(ProbeFun.bit.TriggerType1 == 0 && ProbeStatus.bit.PValueStor1 == 0))
{
ProbeStatus.bit.NValueStor1 = ProbeStatus.bit.PValueStor1 = 1;
dwPos = Get_Position();
if ((dwPos & ((1L<<DBVALUE(ENCODER_FINERESOLUTION).wData) -1)) < (1L<<(DBVALUE(ENCODER_FINERESOLUTION).wData -1)))
dwPos = dwPos & (0x7FFFFFFF<<DBVALUE(ENCODER_FINERESOLUTION).wData);
else
dwPos = (dwPos +(1L<<DBVALUE(ENCODER_FINERESOLUTION).wData)) & (0x7FFFFFFF<<DBVALUE(ENCODER_FINERESOLUTION).wData);
if (DBVALUE(ENCODER_INVERSION_ACTUAL_VALUE).wData & INVERSION_POSITION_MASK)
u_lTP1NegPostion = u_lTP1PosPostion = - dwPos;
else
u_lTP1NegPostion = u_lTP1PosPostion = dwPos;
}
}
else if ((Get_MechAngle() - qAngelLast) > _IQ(0.9375))
{
if(ProbeFun.bit.TriggerType1 == 1 ||(ProbeFun.bit.TriggerType1 == 0 && ProbeStatus.bit.NValueStor1 == 0))
{
//ProbeStatus.bit.NValueStor1 = 1;
ProbeStatus.bit.NValueStor1 = ProbeStatus.bit.PValueStor1 = 1;
dwPos = Get_Position();
if ((dwPos & ((1L<<DBVALUE(ENCODER_FINERESOLUTION).wData) -1)) < (1L<<(DBVALUE(ENCODER_FINERESOLUTION).wData -1)))
dwPos = dwPos & (0x7FFFFFFF<<DBVALUE(ENCODER_FINERESOLUTION).wData);
else
dwPos = (dwPos +(1L<<DBVALUE(ENCODER_FINERESOLUTION).wData)) & (0x7FFFFFFF<<DBVALUE(ENCODER_FINERESOLUTION).wData);
if (DBVALUE(ENCODER_INVERSION_ACTUAL_VALUE).wData & INVERSION_POSITION_MASK)
u_lTP1PosPostion = u_lTP1NegPostion = - dwPos;
else
u_lTP1PosPostion = u_lTP1NegPostion = dwPos;
}
}
}
qAngelLast = Get_MechAngle();
bProbe1 = GetProbe1();
ProbeStatus.bit.ProbeEn1 = 1;
}
```
mechangle 好像是以開機時的角度為準。我認為是該分兩層，開機時的underline 角度（相對角度）（給控制用的），和轉給user 用的角度。如果是abs encoder，兩者沒差別。如果是incremental， user 角度在第一次碰到z signal 才知道。
而且這個做法，值會準嗎？我是很懷疑。因為這個function 是 non-realtime的，所以偵測到的時候，時間會有差。只有abs encoder的會準。
encoder 的 Read_Encoder 和 Get_Position 是分開的。所以 Read_Encoder 會在 ISR 週期的read。但如果一次 non-realtime 經過>=2次的 ISR。encoder value 就會差很多了。
---
但是應該至少是有存值的，所以0xb9的bit1 ，Touch probe 1 positive edge position stored，應該要=1。
ProbeStatus.bit.NValueStor1 = ProbeStatus.bit.PValueStor1 = 1;
## 解法
~~這應該是 encoder 那裡就要有一個 latched z signal，因為只有read_encoder 有控制時間是在timer ISR裡。~~
但是後來說，polling 的精度已經夠了。
也還是要提供一個判斷z signal 的方法，現有encoder提供的介面都不行很好的偵測。
一定是要新增 encoder 的介面，有兩個方法，function(method) or structure variable，那一個比較好？很明顯的是，用 vairable 會簡單很多。因為目前implement的inherit，是用很麻煩的address map 做的，而不是contrainer of。
polling to find edge 是在 touch probe function 裡做的。所以就是要有一個Z訊號產生前後，會變化的變數。(除非用observer mode)
--> dwLoopCount 放到 parent?
但 問題就是很難產生一個 z相前後有變的變數。唯一可以看到的還是z signal 本身。而且直接使用 z signal，這也才符合 touch probe function 自己detect value chnage的特性。
如果touch probe自己讀值，且toch probe function 不在non-real time裡了話，很有可能 reading 不夠即時導致沒讀到值。
但如果在 是io 點自己去判斷rising edge，並記錄值。touch probe 就不能直接去修改這些記錄值，因為它是public to everyone，一修改就會影響到其它部分。
另一個角度是，touch probe 不知道要不要覆蓋掉目前的值，到底如何處理值，應該只有應用端的touch probe 知道。
我覺得toch probe 自己讀是對的，只是讀的不夠快了話是錯的。
或是說假設能分成兩種style，一種是自己讀(上面)，第二種是源頭自己latch的。第二種的問題是，它無法確定目前的值是剛latch到的，還是上次就殘留的，因為它並沒有補捉edge。
讓源頭（producer）來補捉edge了話，producer 就要跟據consumer的數量自動生成對應數量的 edge catched 變數。consumer 拿到後就 reset edge catched。程式變得很複雜且縮限consumer 如何使用值。
# 衍生問題-無法enable
以下這個function 就是在while loop 裡的 routine 而已。
```c=1401
void ControlWordRun()
{
static WORD wFlag = 1;
WORD wControlWordTemp = 0;
if (u_bControlWordFlag)
{
if (!ControlWord.bit.SwitchON && ControlWord.bit.EnableVoltage && ControlWord.bit.QuickStop) // x110
{
// State Transition 2,6,8: => READY TO SWITCH ON
// Event: Shutdown command recerived from host
CANopenState_ReadyToSwitchOn();
if (Statusword.bit.Operationenabled)
u_bShutDown = TRUE;
}
else if (ControlWord.bit.SwitchON && ControlWord.bit.EnableVoltage && ControlWord.bit.QuickStop && !ControlWord.bit.EnableOperation) // 0111
{
// State Transition 3: READY TO SWITCH ON => SWITCHED ON
// Event: Switch On command recerived from host
// Action: The power section is swtiched on if it is not already switched on
if (Statusword.bit.Readytoswitchon && !Statusword.bit.Switchedon) // not already switched on
CANopenState_SwitchedOn();
// State Transition 5: OPERATION ENABLE => SWITCHED ON
// Event: Disable Opeation command recerived from host
// Action: The drive will be disabled
else if (Statusword.bit.Operationenabled) // already Opeation enabled
{
CANopenState_SwitchedOn();
u_bDisableOper = TRUE;
}
}
else if (ControlWord.bit.SwitchON && ControlWord.bit.EnableVoltage && ControlWord.bit.QuickStop && ControlWord.bit.EnableOperation) // this is most used. 1111
{
// State Transition 3: READY TO SWITCH ON => SWITCHED ON
// Event: Switch On command recerived from host
// Action: The power section is swtiched on if it is not already switched on
if (Statusword.bit.Readytoswitchon && !Statusword.bit.Switchedon) // not already switched on
// status word xx01
CANopenState_SwitchedOn();
// State Transition 4: SWITCHED ON => OPERATION ENABLE
// Event: Enable Opeation command recerived from host
// Action: The drive will be enabled
else if (Statusword.bit.Readytoswitchon && Statusword.bit.Switchedon && !Statusword.bit.Operationenabled) // not already Opeation enabled
// status word x011
CANopenState_OperationEnable();
// State Transition 16: QUICK STOP ACTIVE => OPERATION ENABLE
// Event: Enable Opeation command recerived from host.
// Action: The drive will be enabled
else if (Statusword.bit.Operationenabled && !Statusword.bit.Quickstop) // Quick Stop Active
{
if (GetClearPermit() && u_bQuickStopFlag)
{
u_bQuickStopFlag = FALSE;
CANopenState_OperationEnable();
ClearResponseStatus(); // from stop to normal status
}
}
}
else if (!ControlWord.bit.EnableVoltage)
{
// State Transition 7,9,10,12: => SWITCH ON DISABLED
// Event: Disable voltage command recerived from host
CANopenState_SwitchOnDisabled();
if (Statusword.bit.Operationenabled)
u_bDisableVolt = TRUE;
}
else if (ControlWord.bit.EnableVoltage && !ControlWord.bit.QuickStop)
{
// State Transition 7,10: => SWITCH ON DISABLED
// Event: Quick stop command recerived from host
if (!Statusword.bit.Operationenabled) // not already Opeation enabled
CANopenState_SwitchOnDisabled();
// State Transition 11: => QUICK STOP
// Event: Quick stop command recerived from host
else if (Statusword.bit.Operationenabled)
{ // already Opeation enabled
CANopenState_QuickStopActive(); // CANopenState_SwitchOnDisabled();
u_bQuickStopFlag = TRUE;
}
}
if (ControlWord.bit.FaultReset && wFlag) // avoid mapping in ISR run, clear fault will active long time task(encoder error),make ISR time full then reset CPU
{
u_bCANClrError = TRUE;
// State Transition 15: FAULT => READY TO SWITCH ON
// Event: Fault reset command recerived from host
CANopenState_SwitchOnDisabled();
wFlag = 0;
}
else if (!ControlWord.bit.FaultReset)
wFlag = 1;
if (DBVALUE(POWER_FUNTIONBIT).dwData & CAN_ENABLE)
{
// if state is in operation enabled, enable the motor
if (ControlWord.bit.EnableOperation && ControlWord.bit.QuickStop) // (Statusword.bit.Operationenabled && Statusword.bit.Quickstop) || && wLastEnableFlag == 0
{
if (GetDriveStatus())
u_wCANEnable = NULL_CTRLWORD;
else
u_wCANEnable = ENABLE_CTRLWORD;
}
else if (ControlWord.bit.EnableOperation == 0)
{
if (u_bDisableVolt)
ExecuteFaultResponse(OFF2);
else if (u_bQuickStopFlag)
u_wCANEnable = NULL_CTRLWORD;
else if (u_bShutDown)
u_wCANEnable = NULL_CTRLWORD;
else if (u_bDisableOper)
u_wCANEnable = NULL_CTRLWORD;
else if (GetDriveStatus())
u_wCANEnable = DISABLE_CTRLWORD;
else
u_wCANEnable = NULL_CTRLWORD;
}
if (GetControlSource() != PC_COMMAND)
{
if (u_bQuickStopFlag)
{
if (u_wQuickStopCode == SLOWBYQUICKRAMP || u_wQuickStopCode == SLOWBYCURRENTLMT || u_wQuickStopCode == SLOWBYVOLTLMT)
ExecuteFaultResponse(OFF1);
else if (u_wQuickStopCode == DISABLE)
ExecuteFaultResponse(OFF2);
else if (u_wQuickStopCode == SLOWBYSLOWRAMP)
ExecuteFaultResponse(OFF3);
else if (u_wQuickStopCode == SLOWBYQUICKRAMP_INSTOP || u_wQuickStopCode == SLOWBYCURRENTLMT_INSTOP || u_wQuickStopCode == SLOWBYVOLTLMT_INSTOP)
ExecuteFaultResponse(STOP1);
else if (u_wQuickStopCode == SLOWBYSLOWRAMP_INSTOP)
ExecuteFaultResponse(STOP2);
else
ExecuteFaultResponse(OFF2);
}
if (u_bShutDown)
{
if (u_wShutDownCode == DISABLE_DRIVE)
ExecuteFaultResponse(OFF2);
else if (u_wShutDownCode == SLOWDOWN_SLOWRAMP)
ExecuteFaultResponse(OFF3);
else
ExecuteFaultResponse(OFF2);
}
if (u_bDisableOper)
{
if (u_wDisableCode == DISABLE_DRIVE)
ExecuteFaultResponse(OFF2);
else if (u_wDisableCode == SLOWDOWN_SLOWRAMP)
ExecuteFaultResponse(OFF3);
else
ExecuteFaultResponse(OFF2);
}
}
Statusword.bit.Remote = 1;
}
else
Statusword.bit.Remote = 0;
wControlWordTemp = ControlWord.all;
if (u_wOpModeAct == HOMING_MODE) // Postion Homing mode
{
if ((ControlWord.bit.OperationModeSpecific & 0x1) == 1) // homing operation start
{
wControlWordTemp |= 0x0400; // home active
wControlWordTemp &= 0xFFEF; // clear active flag //
} //
else
wControlWordTemp &= 0xFBFF; //
}
SetCANPosControlWord(wControlWordTemp);
SetHomeStart(ControlWord.bit.HomeRun);
SetNoActionEn(ControlWord.bit.Manufacturer2NoAction);
u_bControlWordFlag = FALSE;
}
StatusWordDeal();
StatuswordBuf.all = Statusword.all;
// StatuswordBuf.all = 0x1037; // for
#ifndef D_DSP2812
u_wControlWord = ControlWord.all;
#endif
}
```
另
```c=1328
void CANopenState_OperationEnable()
{
// State Transition 4: SWITCHED ON => OPERATION ENABLE
// Event: Enable operation command recerived from host
// Action: The drive function is enabled
Statusword.bit.Readytoswitchon = 1;
Statusword.bit.Switchedon = 1;
Statusword.bit.Operationenabled = 1;
Statusword.bit.Fault = 0;
Statusword.bit.Quickstop = 1;
Statusword.bit.Switchondisabled = 0;
}
```
# log
10/12
本來想要直接用probe1，但發現 `u_awInputMap` 的 `I_PROBE1` 好像沒有引出來?
其實是 `IO.c` 裡有 `SetInputMap`，然後再用這個function 和 `SetInput1Map` （index `57103` ） 去把 DI1 設定為某種功能 e.g. 我們想要的 `I_PROBE1`。
取IO 值，就是先從這個map 去讀值(pin_number)，然後再 digital_read(pin_number)。
10/12
sincos, 絕對值 encoder 是fpga 來處理的，這件事已經確定。要換fpga code 也是確定的。
問題在於 sine ver 的 sandal 和 abs ver 的sandal 線路也可能不同。
> 之前他們有跟我說，SINCOS跟Endat設計的線路有所差異(大sandal, 小sandal)
這表示燒sincos 的fpga code 可能也沒用。
10/16
越走越遠，硬體搞不完。
10/19
接收不到z訊號的問題描述
encoder z訊號的路徑是 encoder --> 驅動器電路 --> FPGA --> DSP。
在 10/18 量不到 ERN1387 encoder 的Z訊號，但能量到A,B之後。更換了西門子的 encoder，馬上就量到Z訊號。
但今天DSP還是抓不到Z訊號。
首先，轉動軸，在 u20, 74LVC2G14 比較器的pin6(output) 和 pin1 (input) 都沒看到訊號。
接著，將 pin1 人工拉到 ground，DSP裡就收到z 訊號了。
所以，我們認為是在encoder port 到 u20之間的電路有問題。
---
手動給z訊號的結果，目前是正常的，發現z signal 在FPGA裡 好像是latched的，所以只有positive edge 有用。而且沒有negative edge 的FPGA了話，就永遠做不了。
10/20
跟周勇要FPGA 的reset 方法。
10/23
因為今天測試時，只有一次剛好讀到touch probe 的值。
很可能之前說到nonrealtime 問題發生了，所以將整個function 移到 ISR 裡。
10/24
無法enable 問題。
10/25
修正touch probe 2。28377d 最小 memory 為16bit。
10/26
mode switch 不小心改到，產生bug
要考慮 unittest 的問題了。
Sign in with Wallet
Connect another wallet