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

}