Catcan smartsensor Lite 讀值畫面

本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); } }