Python人工智慧圖控 - 研習交流廳 造訪社團 » 座標

相機標定可取得內、外參數， 可用來校準相機，這個參數在相機量測目標物尺寸及距離時非常重要。再找時間說明原理。

內參(intrinstic parameters)與相機內部結構有關，如焦距、透鏡畸變、像素間距、中心點座標等。

外參表示相機在世界座標系中的3D位置。

p.s. 標定板大小及行列數目需輸入正確, 可視範圍約1/4大小。約擷取10張不同位置的標定位置, 圖三是校準前有徑向及切向畸變, 圖四是校準後。

繼昨天貼文 ' Python 呼叫 DLL 的方法 '後， 但能夠做甚麼呢? 下面給出Source code實例是Python模組PyCNC程式庫，我將C++運動控制驅動程式封裝給Python呼叫的範例，控制動作很流暢， 因為程式長所以就節錄部份程式碼給對控制硬體有興趣的朋友參考一下。

圖1. Python + PyQt5控制機台程式 影片 2. Python控制機台程式執行實例 影片 3. 程式控制機台實例 影片 4. TCP/IP遠距通信監控機台程式執行實例
使用Python語言做開發平台的三大動機 [語法簡單明瞭] Python語法簡單易學，最重要的是還有大量功能強大的免費模組可下載，其強大的應用層面已經發展到令人不可忽視的重要地位，甚至NASA也拿來當作航太人機介面的控制語言。早期接的專案我都是用Assembly、C、C++、C#等設計自動控制系統，這幾年我很多是改用Python來撰寫，好處是取得系統傳回的資料後，可很容易且快速的結合各種海量模組演算法發展出很專業的人工智慧機器。 [Python是跨平台語言] Python本身沒有支援特定硬體控制的功能，也正因為如此它才能夠跨平台，但這不是原罪，相反的卻是它的優點，換句話說，在x86、Arm、Arduino、PC、手機或平板等不同的作業系統環境下，相同的程式皆可以很容易互相移植過去正常執行，這 print('免費資源 + 免費的模組 + 簡單易學') 根本是一場世界革命，能不紅嗎! 綜合以上講了一大堆，無非就是要說服工程師們是該改變自已接受世界脈動的時候了，我也不例外，以此共勉之! [軟體IC] 接下來進入主題談論如何用 Python 開發一個多軸的 CNC 平台，會舉這個 CNC 專題當作例子是因為在我眼裡，CNC 其實就是一個機器人，我的經驗是 - 只要搞懂 CNC 軟硬體知識後，無論是自駕車、四軸無人機或是工具機皆是囊中物，至於想要用它做些甚麼，端看你無限的想像力而定! 因為 Python 沒有直接存取硬體的介面，我的方法是用 Python 當作主程式，將底層存取硬體的 API 程式(動態連結程式庫 DLL)封裝成Python 可調用的格式即可，如此一來多年使用 C、C++、C# 寫的程式庫都可調用了。將來使用者只要將封裝的軟體看成是黑箱直接呼叫就對了，而不須知道內部的演算法，事實上 Python 模組就是軟體IC的概念。

# -*- coding: utf-8 -*- ''' 石小川 / Michael Shih 光騰高科技工作室 / Quantum Tek. ''' import ctypes import ctypes.wintypes #------------------------------------------------------------------------------ class __IMC_EventType(): IMC_Allways = 0 #"無條件執行" IMC_Edge_Zero = 1 #"邊沿型條件執行——變為0時" IMC_Edge_NotZero = 2 #"邊沿型條件執行——變為非0時" IMC_Edge_Great = 3 #"邊沿型條件執行——變為大於時" IMC_Edge_GreatEqu = 4 #"邊沿型條件執行——變為大於等於時" IMC_Edge_Little = 5 #"邊沿型條件執行——變為小於時" IMC_Edge_Carry = 6 #"邊沿型條件執行——變為溢出時" IMC_Edge_NotCarry = 7 #"邊沿型條件執行——變為無溢出時" IMC_IF_Zero = 8 #"電位型條件執行——若為0" IMC_IF_NotZero = 9 #"電位型條件執行——若為非0" IMC_IF_Great = 10 #"電位型條件執行——若大於" IMC_IF_GreatEqu = 11 #"電位型條件執行——若大於等於" IMC_IF_Little = 12 #"電位型條件執行——若小於" IMC_IF_Carry = 13 #"電位型條件執行——若溢出" IMC_IF_NotCarry = 14 #"電位型條件執行——若無溢出" IMC_EventType = __IMC_EventType() #------------------------------------------------------------------------------ #define WR_MUL_DES, *pWR_MUL_DES class __WR_MUL_DES(ctypes.Structure): _fields_ = [ ("addr", ctypes.c_int16), ("axis", ctypes.c_uint16), ("len", ctypes.c_uint16), ("data_0", ctypes.c_uint16), ("data_1", ctypes.c_int16), ("data_2", ctypes.c_int16), ("data_3", ctypes.c_int16) ] #------------------------------------------------------------------------------ class __EventInfo(ctypes.Structure): _fields_ = [ ("EventCMD", ctypes.c_short), ("EventType", ctypes.c_short), ("Src1_loc", ctypes.c_short), ("Src1_axis", ctypes.c_short), ("Src2_loc", ctypes.c_short), ("Src2_axis", ctypes.c_short), ("reserve1", ctypes.c_int), ("dest_loc", ctypes.c_short), ("dest_axis", ctypes.c_short), ("reserve2", ctypes.c_int) ] #------------------------------------------------------------------------------ def CharArrayToList(array, num): btext = False text = "" List = [] if(num == 0): return List for i in range(len(array)): if(text != ""): btext = True if(array[i] == b'\x00'): array[i] = b'\n' text += str(array[i].decode('utf-8')) if(i%255 == 0 and btext == True): text = text.strip() text = str(text) if(len(text) > 0): List.append(text) text = "" btext = False return List #------------------------------------------------------------------------------ class __IMC_Pkg(): def __init__(self): self.ptr = ctypes.WinDLL('IMC_Pkg.dll') #_stdcall #--------------------------------------------------------------------------- def FindNetCard(self): charArray = ctypes.c_char * 4096 #16 x 2156 = 4096 array = charArray() num = ctypes.c_int() #ptr = ctypes.WinDLL('IMC_Pkg.dll') #_stdcall self.ptr.PKG_IMC_FindNetCard.argtypes = (ctypes.POINTER(charArray), ctypes.POINTER (ctypes.c_int)) self.ptr.PKG_IMC_FindNetCard.restype = ctypes.c_int status = self.ptr.PKG_IMC_FindNetCard(ctypes.byref(array), ctypes.byref(num)) return status, CharArrayToList(array, num) #--------------------------------------------------------------------------- def Open(self, netcardId, imcId): self.ptr.PKG_IMC_Open.argtypes = (ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_Open.restype = ctypes.POINTER(ctypes.c_voidp) return self.ptr.PKG_IMC_Open(netcardId, imcId) #--------------------------------------------------------------------------- def OpenX(self, netcardId, imcId, timeout, openMode): self.ptr.PKG_IMC_OpenX.argtypes = (ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_OpenX.restype = ctypes.POINTER(ctypes.c_voidp) return self.ptr.PKG_IMC_OpenX(netcardId, imcId) #--------------------------------------------------------------------------- def OpenUsePassword(self, netcardId, imcId, password): self.ptr.PKG_IMC_OpenUsePassword.argtypes = (ctypes.c_int, ctypes.c_int, ctypes.POINTER(ctypes. c_char_p)) self.ptr.PKG_IMC_OpenUsePassword.restype = ctypes.POINTER(ctypes.c_voidp) return self.ptr.PKG_IMC_OpenUsePassword(netcardId, imcId, password) #--------------------------------------------------------------------------- def Close(self, handle): self.ptr.PKG_IMC_Close.argtypes = (ctypes.POINTER(ctypes.c_voidp), ) #單獨也必須加上括弧 self.ptr.PKG_IMC_Close.restype = ctypes.c_int return self.ptr.PKG_IMC_Close(handle) #--------------------------------------------------------------------------- def InitCfg(self, handle): self.ptr.PKG_IMC_InitCfg.argtypes = (ctypes.POINTER(ctypes.c_voidp), ) #單獨也必須加上括弧 self.ptr.PKG_IMC_InitCfg.restype = ctypes.c_int return self.ptr.PKG_IMC_InitCfg(handle) #--------------------------------------------------------------------------- def ClearIMC(self, handle): self.ptr.PKG_IMC_ClearIMC.argtypes = (ctypes.POINTER(ctypes.c_voidp), ) #單獨也必須加上括弧 self.ptr.PKG_IMC_ClearIMC.restype = ctypes.c_int return self.ptr.PKG_IMC_ClearIMC(handle) #--------------------------------------------------------------------------- def ClearAxis(self, handle, axis): self.ptr.PKG_IMC_ClearAxis.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int) self.ptr.PKG_IMC_ClearAxis.restype = ctypes.c_int return self.ptr.PKG_IMC_ClearAxis(handle, axis) #--------------------------------------------------------------------------- def SetPulWidth(self, handle, ns, axis): self.ptr.PKG_IMC_SetPulWidth.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_uint, ctypes.c_int) self.ptr.PKG_IMC_SetPulWidth.restype = ctypes.c_int return self.ptr.PKG_IMC_SetPulWidth(handle, ns, axis) #--------------------------------------------------------------------------- def SetPulPolar(self, handle, pul, dir, axis): self.ptr.PKG_IMC_SetPulPolar.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetPulPolar.restype = ctypes.c_int return self.ptr.PKG_IMC_SetPulPolar(handle, pul, dir, axis) #--------------------------------------------------------------------------- def SetEncpEna(self, handle, ena, axis): self.ptr.PKG_IMC_SetEncpEna.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetEncpEna.restype = ctypes.c_int return self.ptr.PKG_IMC_SetEncpEna(handle, ena, axis) #--------------------------------------------------------------------------- def SetEncpMode(self, handle, mode, dir, axis): self.ptr.PKG_IMC_SetEncpModer.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetEncpModer.restype = ctypes.c_int return self.ptr.PKG_IMC_SetEncpModer(handle, mode, dir, axis) #--------------------------------------------------------------------------- def SetEncpRate(self, handle, rate, axis): self.ptr.PKG_IMC_SetEncpRate.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_double, ctypes.c_int) self.ptr.PKG_IMC_SetEncpRate.restype = ctypes.c_int return self.ptr.PKG_IMC_SetEncpRate(handle, rate, axis) #--------------------------------------------------------------------------- def SetVelAccLim(self, handle, vellim, acclim, axis): self.ptr.PKG_IMC_SetVelAccLim.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_double, ctypes.c_double, ctypes.c_int) self.ptr.PKG_IMC_SetVelAccLim.restype = ctypes.c_int return self.ptr.PKG_IMC_SetVelAccLim(handle, vellim, acclim, axis) #--------------------------------------------------------------------------- def SetEna(self, handle, ena, axis): self.ptr.PKG_IMC_SetEna.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetEna.restype = ctypes.c_int return self.ptr.PKG_IMC_SetEna(handle, ena, axis) #--------------------------------------------------------------------------- def Setlimit(self, handle, plimEna, plimPolar, nlimEna, nlimPolar, axis): self.ptr.ptr.PKG_IMC_Setlimit.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int) self.ptr.ptr.PKG_IMC_Setlimit.restype = ctypes.c_int return self.ptr.PKG_IMC_Setlimit(handle, plimEna, plimPolar, nlimEna, nlimPolar, axis) #--------------------------------------------------------------------------- def SetAlm(self, handle, ena, polar, axis): self.ptr.ptr.PKG_IMC_SetAlm.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int, ctypes.c_int) self.ptr.ptr.PKG_IMC_SetAlm.restype = ctypes.c_int return self.ptr.PKG_IMC_SetAlm(handle, ena, polar, axis) #--------------------------------------------------------------------------- def SetINP(self, handle, ena, polar, axis): self.ptr.ptr.PKG_IMC_SetINP.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int, ctypes.c_int) self.ptr.ptr.PKG_IMC_SetINP.restype = ctypes.c_int return self.ptr.PKG_IMC_SetINP(handle, ena, polar, axis) #--------------------------------------------------------------------------- def SetEmstopPolar(self, handle, polar, axis): self.ptr.PKG_IMC_SetEmstopPolar.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetEmstopPolar.restype = ctypes.c_int return self.ptr.PKG_IMC_SetEmstopPolar(handle, polar, axis) #--------------------------------------------------------------------------- def SetInPolar(self, handle, polar, inPort): self.ptr.PKG_IMC_SetInPolar.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetInPolar.restype = ctypes.c_int return self.ptr.PKG_IMC_SetInPolar(handle, polar, inPort) #--------------------------------------------------------------------------- def SetStopfilt(self, handle, stop, axis): self.ptr.PKG_IMC_SetStopfilt.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetStopfilt.restype = ctypes.c_int return self.ptr.PKG_IMC_SetStopfilt(handle, stop, axis) #--------------------------------------------------------------------------- def SetExitfilt(self, handle, exit, axis): self.ptr.PKG_IMC_SetExitfilt.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetExitfilt.restype = ctypes.c_int return self.ptr.PKG_IMC_SetExitfilt(handle, exit, axis) #--------------------------------------------------------------------------- def SetRecoupRange(self, handle, range, axis): self.ptr.PKG_IMC_SetRecoupRange.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_SetRecoupRange.restype = ctypes.c_int return self.ptr.PKG_IMC_SetRecoupRange(handle, range, axis) #--------------------------------------------------------------------------- #--------------------------------------------------------------------------- def MoveAbs(self, handle, pos, startvel, tgvel, wait, axis): self.ptr.PKG_IMC_MoveAbs.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_double, ctypes.c_double, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_MoveAbs.restype = ctypes.c_int return self.ptr.PKG_IMC_MoveAbs(handle, pos, startvel, tgvel, wait, axis) #--------------------------------------------------------------------------- def MoveAbs_P(self, handle, pos, startvel, tgvel, wait, axis): #P 輔助座標 self.ptr.PKG_IMC_MoveAbs_P.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_double, ctypes.c_double, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_MoveAbs_P.restype = ctypes.c_int return self.ptr.PKG_IMC_MoveAbs_P(handle, pos, startvel, tgvel, wait, axis) #--------------------------------------------------------------------------- def MoveDist(self, handle, dist, startvel, tgvel, wait, axis): self.ptr.PKG_IMC_MoveDist.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_double, ctypes.c_double, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_MoveDist.restype = ctypes.c_int return self.ptr.PKG_IMC_MoveDist(handle, dist, startvel, tgvel, wait, axis)

機器視覺在工業4.0的應用非常廣泛，例如樣本比對、良率偵測、工件大小量測、微生物標定…..等，如果結合人工智慧資料分析就是一套很完整的專家系統了。

圖中軟體是我將紅血球/白血球辨識及標定的核心程式改寫成簡單的CNC自動對刀及工件偵測系統，理論上量測精度可控制在1um(10^-6m)以內，不過我的CNC機台螺桿可沒這麼精密，所以我把它控制在浮點數第3位即可，整體而言精密度與攝像頭、機台、都有關係，端看應用者的要求而調整軟硬體。

自動對刀是用演算法找出工件中心點，而自動對焦是採用Z軸移動加上演算法達成，另外比例尺可配合3軸移動及像素自動求得，一旦中心點座標找出來就很容易結合Mach3或PLC將主軸定位在工件上。值得一提的是打光技術在影像辨識也非常重要，打光打得好辨識率就很好，演算法就可精簡些，其餘利用影像偵測工作臺大小及影像多軸限位開關技巧和Z軸單鏡頭工件深度偵測技巧，這些功能我就留在下回再演示了

p.s. 雷達是機器的眼睛，以後還會多方介紹這類的遙測技術，掌握這門知識對於設計智慧型機器幫助會很大 。 光學雷達(LiDAR)這幾年很紅, 從汽車自動駕駛、火炮射控、巡弋飛彈,、機器人或掃地機等都可看到蹤影, 因為有人問我光學雷達的點雲(point cloud)資料如何產生， 所以今天簡單講解一下點雲基礎。 其實原理很簡單, 由雷達發射的仰角α(elevation), 水平角β(azimuth)及接收訊號的距離r(distance), 參考下面圖的座標三角函數關係就可得知目標物的方位等資訊P(x’, y’, z’), 一連串的數據產生這就是點雲的故事了。其格式如下: Point cloud = {P1, P2, P3…..Pn}; //表示一組點雲數據
Pi = [xi’, yi’, zi’]; //每一個點雲就是一個座標 更進階的點雲還加入目標物的RGB顏色, 灰階值等資訊φi, 則格式會變成
Pi = [xi’, yi’, zi’, φi]; 下面是點雲公式推導, 參考一下就好.
定義符號,
仰角 : α(elevation)
水平角 : β(azimuth)
距離 : r(distance) r’ = r * cosα (1) x’ = r’ * cosβ (2)
y’ = r’ * sinβ (3)
z’ = r * sinα (4) 將 (1)式代入(2), (3)式, 重新整理如下: x’ = r * cosα * cosβ (5)
y’ = r * cosα * sinβ (6)
z’ = r * sinα (7) 從LiDAR發射光點及接收訊號, 可以知道(α,β,r)參數, 所以很快就可求出目標物的方位值(x’, y’, z’)。 p.s. 下面程式(LiDAR + 3軸雲台控制)是我以前用Arduino寫來測試LiDAR用的, 有點長慎入, 日子久了有點不知程式在寫啥! 程式沒有最佳化但可以動作, 純粹Demo點雲公式如何使用給有興趣的好友參考一下, 有興趣的朋友也可改成Python版本嘉惠其他人, 如果有不解地也可私我。 //Program of LiDAR******************************** #include <string.h>
#include <Servo.h>
#include <LIDARLite.h> //----------------------------------------------------------------------------- #define Def_minAzimuth 30
#define Def_maxAzimuth 160
#define Def_standAzimuth 90 #define Def_minElevation 0
#define Def_maxElevation 60
#define Def_standElevation 0
#define Def_offsetElevation 90 //----------------------------------------------------------------------------- LIDARLite _lidar;
Servo _servoX;
Servo _servoY; // Minimum and maximum servo angle in degrees
// Modify to avoid hitting limits imposed by pan/tilt bracket geometry
int _minPosX = 30; //0;
int _maxPosX = 160; // 180; //Servo real position
int _minPosY = 90;
int _maxPosY = 100; int _midPosX = 90; //(_maxPosX + _minPosX) / 2;
int _midPosY = 100; //(_maxPosY + _minPosY) / 2; boolean _scanning = false;
boolean _scanDirectionX = false;
boolean _scanDirectionY = false; int _posX = 0;
int _posY = 0; //pole coordinate
int _Azimuth = 0;
int _Elevation = 0;
int _Distance = 0; unsigned int _loopCount = 0; //----------------------------------------------------------------------------- void setup()
{
_lidar.begin(0, true);
_lidar.configure(0); _servoX.attach(2);
_servoY.attach(3); _Azimuth = Def_standAzimuth; //90
//_Elevation = Def_standElevation + Def_offsetElevation; //0 - 90
_Elevation = Def_standElevation; //0 ServoHome(_Azimuth, _Elevation);
LidarON(); Serial.begin(115200); delay(100);
} //end setup() //----------------------------------------------------------------------------- void loop()
{
int px, py;
int length, count;
char command[12], code[12];
String data[6]; delay(1000); if(length = Serial.available())
{
Serial.readBytes(command,length);
command[length]= '\0'; count = CommandParse(command, data, " ");
DataParse(data[0], code); if(code[0] == 'C')
{
switch(code[1])
{
case '0': //Serial.println("C0 OK!");
break; case '1': ServoHome(_Azimuth, _Elevation); Serial.println("C1 " + String(_Azimuth) + " " + String(_Elevation));
delay(1000);
break; case '2': //指向測距
DataParse(data[1], code);
_Azimuth = atoi(code); DataParse(data[2], code);
_Elevation = atoi(code); LidarScan(_Azimuth, _Elevation, true); //px= _Azimuth;
//py = _Elevation + 90; //ServoMove(px, py);
//delay(100); Serial.println("C2 " + String(_Azimuth) + " " + String(_Elevation));
break; case '3':
Serial.println("C3");
break; case '4': //全面掃描
LidarScan(_Azimuth, _Elevation, false);
ServoHome(_Azimuth, _Elevation); //Serial.println("C4");
break;
} //end switch(command[1])
} //end if(command[0] == 'C') else
{
/*
while(Serial.read() != -1)
{
delay(100);
}
*/ //Serial.println("Error");
}
} //end if(length = Serial.available()) } //end loop() //----------------------------------------------------------------------------- int CommandParse(char *command, String data, char delimit)
{
int index;
char *ptr; index = 0;
ptr = strtok(command, delimit);
data[index] = (String)ptr;
index++; while ((ptr = strtok(NULL, delimit)))
{
data[index] = (String)ptr;
index++;
} //end while ((ptr = strtok(NULL, delimit))) return index;
} //----------------------------------------------------------------------------- void DataParse(String data, char *code)
{
int i; for(i=0; i < data.length(); i++)
code[i] = data.c_str()[i]; code[i] = '\0';
} //-----------------------------------------------------------------------------
//azimuth : 0 - 180. elevation : 0 - 90
void ServoHome(int azimuth, int elevation)
{
int i; //Handle Azimuth
if(azimuth >= Def_standAzimuth) // >= 90
{
for(i = azimuth; i > Def_standAzimuth; i--)
{
_servoX.write(i);
delay(10);
}
}
else
if(azimuth < Def_standAzimuth) // < 90
{
for(i = azimuth; i < Def_standAzimuth; i++)
{
_servoX.write(i);
delay(10);
}
} _Azimuth = i; //Handle Elevation
if(elevation >= Def_standElevation) // <= 0
{
for(i = elevation; i > Def_standElevation; i--)
{
_servoY.write(i + Def_offsetElevation);
delay(10);
}
}
else
if(elevation < Def_standElevation)
{
for(i = elevation; i < Def_standElevation; i++)
{
_servoY.write(i + Def_offsetElevation);
delay(10);
}
} _Elevation = i;
} //end void ServoHome() //-----------------------------------------------------------------------------
//azimuth, elevation, is currently position
void LidarScan(int azimuth, int elevation, bool enable)
{
int i, j;
unsigned int loopcount, distance;
bool bDirection; if(enable) //單點掃描
{
ServoMove(azimuth, elevation + Def_offsetElevation); delay(1000); //少了distance會錯誤 distance = _lidar.distance(false); Serial.println(String(azimuth) + " " + String(elevation) + " " + String(distance)); i = azimuth;
j = elevation;
}
else //全區掃描
{
//initial servo start point x=0, y=0
for(i = azimuth; i > Def_minAzimuth; i--)
{
_servoX.write(i);
delay(10);
} //initial servo start point y=0
for(i = elevation; i > Def_minElevation; i--)
{
_servoY.write(i + Def_offsetElevation);
delay(10);
} loopcount = 0;
bDirection = true;
for(j = Def_minElevation; j < Def_maxElevation; j++) // Scan Y
{
if(bDirection)
{
bDirection = false;
for(i = Def_minAzimuth; i < Def_maxAzimuth; i++)
{
loopcount++;
if (loopcount % 100 == 0)
distance = _lidar.distance();
else
distance = _lidar.distance(false); Serial.println(String(i) + " " + String(j) + " " + String(distance)); ServoMove(i, j + Def_offsetElevation);
} //end for(i = Def_minAzimuth; i < Def_maxAzimuth; i++)
}
else
{
bDirection = true;
for(i = Def_maxAzimuth; i > Def_minAzimuth; i--)
{
loopcount++;
if (loopcount % 100 == 0)
distance = _lidar.distance();
else
distance = _lidar.distance(false); Serial.println(String(i) + " " + String(j) + " " + String(distance)); ServoMove(i, j + Def_offsetElevation);
} //end for(i = Def_maxAzimuth; i > Def_minAzimuth; i--) } /*
for(i = Def_minAzimuth; i < Def_maxAzimuth; i++)
{
loopcount++;
if (loopcount % 100 == 0)
distance = _lidar.distance();
else
distance = _lidar.distance(false); Serial.println(String(i) + " " + String(j) + " " + String(distance)); ServoMove(i, j + Def_offsetElevation);
} //end for(i = Def_minAzimuth; i < Def_maxAzimuth; i++) delay(10); for(i = Def_maxAzimuth; i > Def_minAzimuth; i--)
{
loopcount++;
if (loopcount % 100 == 0)
distance = _lidar.distance();
else
distance = _lidar.distance(false); Serial.println(String(i) + " " + String(j) + " " + String(distance)); ServoMove(i, j + Def_offsetElevation);
} //end for(i = Def_maxAzimuth; i > Def_minAzimuth; i--) delay(10);
*/
} _Azimuth = i;
_Elevation = j;
}
} //----------------------------------------------------------------------------- void ServoX(int px)
{
_servoX.write(px);
delay(10);
} //end void ServoMove() //----------------------------------------------------------------------------- void ServoY(int py)
{
_servoY.write(py);
delay(10);
} //end void ServoMove() //-----------------------------------------------------------------------------
void ServoMove(int px, int py)
{
_servoX.write(px);
_servoY.write(py);
delay(10);
} //end void ServoMove() //----------------------------------------------------------------------------- void LidarON()
{
_scanning = true;
_loopCount =0;
} //end void LidarON() //----------------------------------------------------------------------------- void LidarOFF()
{
_scanning = false;
_loopCount =0;
} //end void LidarOFF() //-----------------------------------------------------------------------------
/*
bool LidarScan()
{
if(_scanning)
{
_loopCount ++;
if (_loopCount % 100 == 0)
_Distance = _lidar.distance();
else
_Distance = _lidar.distance(false); _Azimuth = _posX;
_Elevation = _posY - 90; Serial.println(String(_Azimuth) + " " + String(_Elevation) + " " + String(_Distance)); ServoMove(_posX, _posY); if(_scanDirectionX)
_posX++;
else
_posX--; if(_posX >= _maxPosX || _posX <= _minPosX)
{
_scanDirectionX = !_scanDirectionX; if(_scanDirectionY)
_posY--;
else
_posY++; if(_posY > _maxPosY || _posY < _minPosY)
{
_scanDirectionY = !_scanDirectionY;
LidarOFF();
//ServoHome(); return true;
}
}
} //end if(_scanning) return false;
} //end void Lidar()

雙眼攝影機測距 II – 原理解析

接續昨天所談的，雙眼測距就是簡單的三角測距公式，我導一下公式，稍微解釋就可馬上了解。

@符號定義如下:
P : 待測物。
Z : 相機與待測物的距離。
d: 視差。
f: 攝影機焦距。
T: 左右攝影機投影中心的距離。
Xl: 左影像平面成像座標。
Xr: 右影像平面成像座標。

當然商品有很多know-how，但原理就是如此，為了說明我簡化了儀器，假設
(a) 左右焦距是相同的 fl = fr = f。
(b) 左右相機特性完全相同，也完美矯正過。

除非想製作雷達，否則可不理會(a)、(b)兩項完美假設。

OK, 進入主題，沒多大學問，如圖所示，只要應用幾何三角比例關係；

T / d = Z / f (1)

Z = f . T / d (2)

令 d = Xl - Xr (3)

Z = f . T / Xl - Xr (4)

其中變數f、T、d皆是可從相機求得。

所以Z(距離或稱深度)就可以很容易的由(4)式求出來了。

看是不是很簡單, 希望我沒有誤人子弟^^

[語法簡單明瞭]
Python語法簡單易學，最重要的是還有大量功能強大的免費模組可下載，其強大的應用層面已經發展到令人不可忽視的重要地位，甚至NASA也拿來當作航太人機介面的控制語言。早期接的專案我都是用Assembly、C、C++、C#等設計自動控制系統，這幾年我很多是改用Python來撰寫，好處是取得系統傳回的資料後，可很容易且快速的結合各種海量模組演算法發展出很專業的人工智慧機器。 [Python是跨平台語言]
Python本身沒有支援特定硬體控制的功能，也正因為如此它才能夠跨平台，但這不是原罪，相反的卻是它的優點，換句話說，在x86、Arm、Arduino、PC、手機或平板等不同的作業系統環境下，相同的程式皆可以很容易互相移植過去正常執行，這 print('免費資源 + 免費的模組 + 簡單易學') 根本是一場世界革命，能不紅嗎! 綜合以上講了一大堆，無非就是要說服工程師們是該改變自已接受世界脈動的時候了，我也不例外，以此共勉之! [軟體開發]
接下來進入主題談論如何用Python開發一個多軸的CNC平台，會舉這個CNC專題當作例子是因為在我眼裡，CNC其實就是一個機器人，我的經驗是~只要搞懂CNC軟硬體知識後，無論是自駕車、四軸無人機或是工具機皆是囊中物，至於想要用它做些甚麼，端看你無限的想像力而定! 因為Python沒有直接存取硬體的介面，我的方法是用Python當作主程式，將底層存取硬體的API程式(動態連結程式庫 DLL)封裝成Python可調用的格式即可，如此一來多年使用C++/C#寫的程式庫都可引用了。因為程式很長, 為免洗板我簡略敘說過程如下: (A). 封裝DLL程式庫成PyCNC.py 模組，其中 class __IMC_Pkg()就是封裝成類別的名稱， 將來Python與機器溝通的介面就是依靠這項。 #---------------------------------------------------------------------------
#這是被呼叫的模組 : PyCNC.py
#---------------------------------------------------------------------------
#需引用ctypes
import ctypes
import ctypes.wintypes
#------------------------------------------------------------------------------
class __IMC_Pkg():
def __init__(self):
self.ptr = ctypes.WinDLL('IMC_Pkg.dll') #_stdcall
#---------------------------------------------------------------------------
def Open(self, netcardId, imcId):
self.ptr.PKG_IMC_Open.argtypes = (ctypes.c_int, ctypes.c_int)
self.ptr.PKG_IMC_Open.restype = ctypes.POINTER(ctypes.c_voidp)
return self.ptr.PKG_IMC_Open(netcardId, imcId)
#---------------------------------------------------------------------------
def Close(self, handle):
self.ptr.PKG_IMC_Close.argtypes = (ctypes.POINTER(ctypes.c_voidp), )
self.ptr.PKG_IMC_Close.restype = ctypes.c_int
return self.ptr.PKG_IMC_Close(handle)
#---------------------------------------------------------------------------
def MoveAbs(self, handle, pos, startvel, tgvel, wait, axis):
self.ptr.PKG_IMC_MoveAbs.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_double, ctypes.c_double, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_MoveAbs.restype = ctypes.c_int
return self.ptr.PKG_IMC_MoveAbs(handle, pos, startvel, tgvel, wait, axis)
#---------------------------------------------------------------------------
def MoveAbs_P(self, handle, pos, startvel, tgvel, wait, axis): #P 輔助座標
self.ptr.PKG_IMC_MoveAbs_P.argtypes = (ctypes.POINTER(ctypes.c_voidp), ctypes.c_int, ctypes.c_double, ctypes.c_double, ctypes.c_int, ctypes.c_int) self.ptr.PKG_IMC_MoveAbs_P.restype = ctypes.c_int
return self.ptr.PKG_IMC_MoveAbs_P(handle, pos, startvel, tgvel, wait, axis)
.
.
(B). 主程式motion.py 要引用也很簡單, 只要加上PyCNC.py模組，就可以調用IMC_Pkg所有的功能
#---------------------------------------------------------------------------
#這是主程式 : motion.py
#---------------------------------------------------------------------------
from CNC import PyCNC
IMC_Pkg = PyCNC.__IMC_Pkg()
#---------------------------------------------------------------------------
if(self.gHandle != None):
IMC_Pkg.Close(self.gHandle)
self.gHandle = IMC_Pkg.Open(netcardId, imcId)
if(self.gHandle != None): #//if(IsOpen())
if(IMC_Pkg.InitCfg(self.gHandle) != 0):
self.nAxis = IMC_Pkg.GetNaxis(self.gHandle) #取得設備支援軸數
p, self.Position = self.GetPosition(3) (C). 建議開發環境安裝Anaconda開發包， 在設計Python程式時可少走很多冤枉路，為了相容以前DLL程式庫，我是下載Anaconda3 - 32bit - Python3.7.3版本。 p.s. (1). IMC3xx/IMC4xx系列運動控制卡所有函數(IMC_Pkg.dll)我都有封裝成PyCNC.py以方便Python呼叫，有興趣的朋友可加入我臉書討論。 (2). 圖-1. 八軸運動+I/O+AD/DA控制卡應用圖例 圖-2. motion.py 主程式的視窗介面 影片-1. 使用motion.py 展示CNC軸控，也順便Demo飛行搖桿控制3軸及發射雷射 影片-2. 電腦執行motion.py情形 影片-3. 使用motion.py 執行G-Code，用雷射雕刻一個正圓
(3). 下回有機會再補上 : "使用Python語言做自動控制的方法 - I I 硬體篇"及 "使用Python語言做自動控制的方法 - I I I 通信篇" (UART、USB、TCP/IP)兩篇才算完整，並且教導大家如何組裝一台三軸運動控制平台的知識(機械結構及控制電路的接法)，對於想創業設計自已機器的朋友不要錯過了。在臉書不適合長篇大論所以只能簡單敘述，總感覺見樹不見林，當然我也有開相關的指導課程，有興趣的朋友可上我臉書或私訊我。