CAVEDU教育團隊技術部落格 / 科技趨勢與新知

Catcan smartsensor Lite 讀值畫面

✍ CAVEDU 阿吉 - 雜工📅 2009-12-26👁 595

本NXC 程式示範了如何用Catcan Smartsensor Lite 來讀取 Roll, Pitch, Yaw三軸向角度。 並附加溫度(攝氏), 以及三軸向的加速度raw值。

Roll, Pitch, Yaw這三個名詞是在航空學中指X, Y, Z軸的旋轉, 見下圖會比較好懂。

NXC程式碼有點長, 有興趣的朋友再慢慢看好了。

============================================

unsigned int  tmp=0;    //Define temporal return variable
int count = 2;
byte outBuf[], cmdBuf[];

void checkI2C()
{
byte status;
do{
status= I2CCheckStatus(IN_1);
}while(status == STAT_COMM_PENDING);
}

void  cali()
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0xC0);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif

}

void  setFilterON()
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0xf0);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
}

void  setFilterOFF()
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0xf1);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
}
//Get Servo Postion
int getPitch()
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0x00);  //Build command array
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready

tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

int getRoll()
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0x01);  //Build command array
#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
// TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

int getHeading() //Yaw
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0x02);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

int getTemp()   //Temp
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0x03);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

int getGyro()   //Temp
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 0x04);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

int getAXRaw() //Acelator Raw Data Y
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 10);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

int getAYRaw() //Acelator Raw Data Y
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 11);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

int getAZRaw() //Acelator Raw Data Y
{
outBuf[0] = 0x00;
outBuf[1] = 0x00;

ArrayBuild(cmdBuf,  0x30, 12);  //Build command array

#if 1
checkI2C();    //Wait I2C bus ready
if ( !I2CBytes(IN_1, cmdBuf, count, outBuf) )   //Send first command and check result
//TextOut(0, LCD_LINE7, "ReadBuf Error");  //Display error if failed
checkI2C();    //Wait I2C bus ready
#endif
tmp=outBuf[1];   //Parse High Byte
tmp=(tmp<<8)|outBuf[0];  //Parse Low Byte and merge with High Byte
return tmp;   //Return Position (type 16Bit int)
}

 

 

task main()
{
int a = 0, b =0, c =0,t=0;
int axr, ayr, azr, mxr, myr, mzr;

string msg,msg1,msg2,msg3;
SetSensorLowspeed(IN_1); //設定輸出模式為RAW, 9V Lowspeed
//Precedes(get);

ClearScreen();
while(true){

a = getPitch();
Wait(20);

b = getRoll();
Wait(20);

c = getHeading();
Wait(20);

t = getTemp();
Wait(20);

axr = getAXRaw();
Wait(20);

ayr = getAYRaw();
Wait(20);

azr = getAZRaw();
Wait(20);

msg = NumToStr(a);
TextOut(0, LCD_LINE1, "                 ");
TextOut(0, LCD_LINE1, "Pitch Deg: "+msg);

msg = NumToStr(b);
TextOut(0, LCD_LINE2, "                 ");
TextOut(0, LCD_LINE2, "Roll Deg: "+msg);

msg = NumToStr(c);
TextOut(0, LCD_LINE3, "                 ");
TextOut(0, LCD_LINE3, "Yaw Deg: "+msg);

msg = NumToStr(t);
TextOut(0, LCD_LINE4, "                 ");
TextOut(0, LCD_LINE4, "Temp: "+msg);
Wait(20);

msg = NumToStr(axr);
TextOut(0, LCD_LINE5, "                 ");
TextOut(0, LCD_LINE5, "Ax Raw: "+msg);
Wait(20);

msg = NumToStr(ayr);
TextOut(0, LCD_LINE6, "                 ");
TextOut(0, LCD_LINE6, "Ay Raw: "+msg);
Wait(20);

msg = NumToStr(azr);
TextOut(0, LCD_LINE7, "                 ");
TextOut(0, LCD_LINE7, "Az RAW: "+msg);
Wait(20);
}

}

本文發表於科技趨勢與新知。

相關文章 📎

建立不上網的私人大型AI模型,規劃工作站時要注意的事

2026-11-16

[simReady 系列文章1] 從 CAD/URDF 到 Isaac Sim:5 個最常見的 Physics 踩坑點|軸向、單位、Inertia、Collision 完整解析

2026-11-14

Why Can’t My High-Frequency Communication Device Transmit or Receive Signals?

2026-10-16

Jetson AGX Orin LangChain 教學:MIC-733 遠端連線與 AI Agent 實作

2026-10-06

留言 💬 (0)

還沒有留言,來當第一個。