---
# System prepended metadata

title: '#6 增量式encoder latch z'
tags: [sandal]

---

# #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 的問題了。