任天堂革命性的產品,
自己實作了一次,
這也算是窮人的陀螺儀吧..
//========= Arduino Code ========= #include <Wire.h> byte data[6]; //six data bytes int yaw, pitch, roll; //three axes int yaw0, pitch0, roll0; //calibration zeroes int startTag=0xDEAD; void wmpOn(){ Wire.beginTransmission(0x53); //WM+ starts out deactivated at address 0x53 Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+ Wire.send(0x04); Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active } void wmpSendZero(){ Wire.beginTransmission(0x52); //now at address 0x52 Wire.send(0x00); //send zero to signal we want info Wire.endTransmission(); } void calibrateZeroes(){ for (int i=0;i<10;i++){ wmpSendZero(); Wire.requestFrom(0x52,6); for (int i=0;i<6;i++){ data[i]=Wire.receive(); } yaw0+=(((data[3]>>2)<<8)+data[0])/10; //average 10 readings for each zero pitch0+=(((data[4]>>2)<<8)+data[1])/10; roll0+=(((data[5]>>2)<<8)+data[2])/10; } /*Serial.print("Yaw0:"); Serial.print(yaw0); Serial.print(" Pitch0:"); Serial.print(pitch0); Serial.print(" Roll0:"); Serial.println(roll0);*/ } void receiveData(){ wmpSendZero(); //send zero before each request (same as nunchuck) Wire.requestFrom(0x52,6); //request the six bytes from the WM+ for (int i=0;i<6;i++){ data[i]=Wire.receive(); } yaw=(((data[3]>>2)<<8)+data[0]); //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus pitch=(((data[4]>>2)<<8)+data[1]); //for info on what each byte represents roll=(((data[5]>>2)<<8)+data[2]); } void setup(){ Serial.begin(115200); //Serial.println("WM+ tester"); Wire.begin(); wmpOn(); //turn WM+ on calibrateZeroes(); //calibrate zeroes delay(1000); } void loop(){ receiveData(); //receive data and calculate yaw pitch and roll Serial.write((unsigned byte*)&startTag, 2); //see diagram on randomhacksofboredom.blogger.com Serial.write((unsigned byte*)&pitch0, 2); //for info on which axis is which Serial.write((unsigned byte*)&yaw0, 2); Serial.write((unsigned byte*)&roll0, 2); Serial.write((unsigned byte*)&pitch, 2); Serial.write((unsigned byte*)&yaw, 2); Serial.write((unsigned byte*)&roll, 2); delay(10); } //================= End
//========= Processing Code =========
// 劃出波形
import processing.serial.*;
// Globals
int g_winW = 820; // Window Width
int g_winH = 600; // Window Height
boolean g_dumpToFile = false; // Dumps data to c:\\output.txt in a comma seperated format (easy to import into Excel)
boolean g_enableFilter = false; // Enables simple filter to help smooth out data.
cDataArray pitch0 = new cDataArray(200);
cDataArray yaw0 = new cDataArray(200);
cDataArray roll0 = new cDataArray(200);
cDataArray pitch = new cDataArray(200);
cDataArray yaw = new cDataArray(200);
cDataArray roll = new cDataArray(200);
cGraph g_graph = new cGraph(10, 190, 800, 400);
Serial g_serial;
PFont g_font;
void setup()
{
size(g_winW, g_winH, P2D);
println(Serial.list());
g_serial = new Serial(this, "COM3", 115200, 'N', 8, 1.0); //enter COM port of
g_font = loadFont("AVGmdBU-30.vlw"); //of arduino here
textFont(g_font, 20);
// This draws the graph key info
strokeWeight(1.5);
stroke(255, 0, 0); line(20, 440, 35, 440);
stroke(0, 255, 0); line(20, 460, 35, 460);
stroke(0, 0, 255); line(20, 480, 35, 480);
stroke(255, 0, 0); line(20, 500, 35, 500);
stroke(0, 255, 0); line(20, 520, 35, 520);
stroke(0, 0, 255); line(20, 540, 35, 540);
fill(0, 0, 0);
text("pitch0", 40, 450);
text("yaw0", 40, 470);
text("roll0", 40, 490);
text("pitch", 40, 510);
text("yaw", 40, 530);
text("roll", 40, 550);
//text("current raw", 180, 430);
//text("current deg/s", 320, 430);
if (g_dumpToFile)
{
// This clears deletes the old file each time the app restarts
byte[] tmpChars = {'\r', '\n'};
saveBytes("c:\\output.txt", tmpChars);
}
}
void draw()
{
// We need to read in all the avilable data so graphing doesn't lag behind
while (g_serial.available() >= 2*6+2)
{
processSerialData();
}
strokeWeight(1);
fill(255, 255, 255);
g_graph.drawGraphBox();
strokeWeight(1.5);
stroke(255, 0, 0);
g_graph.drawLine(pitch0, 0, 16384);
stroke(0, 255, 0);
g_graph.drawLine(yaw0, 0, 16384);
stroke(0, 0, 255);
g_graph.drawLine(roll0, 0, 16384);
stroke(255, 0, 0);
g_graph.drawLine(pitch, 0, 16384);
stroke(0, 255, 0);
g_graph.drawLine(yaw, 0, 16384);
stroke(0, 0, 255);
g_graph.drawLine(roll, 0, 16384);
}
// This reads in one set of the data from the serial port
void processSerialData()
{
int inByte = 0;
int curMatchPos = 0;
int[] intBuf = new int[2];
intBuf[0] = 0xAD;
intBuf[1] = 0xDE;
while (g_serial.available() < 2); // Loop until we have enough bytes
inByte = g_serial.read();
// This while look looks for two bytes sent by the client 0xDEAD
// This allows us to resync the server and client if they ever
// loose sync. In my testing I haven't seen them loose sync so
// this could be removed if you need to, but it is a good way to
// prevent catastrophic failure.
while(curMatchPos < 2)
{
if (inByte == intBuf[curMatchPos])
{
++curMatchPos;
if (curMatchPos == 2)
break;
while (g_serial.available() < 2); // Loop until we have enough bytes
inByte = g_serial.read();
}
else
{
if (curMatchPos == 0)
{
while (g_serial.available() < 2); // Loop until we have enough bytes
inByte = g_serial.read();
}
else
{
curMatchPos = 0;
}
}
}
while (g_serial.available() < 2*6); // Loop until we have a full set of data
// This reads in one set of data
{
byte[] inBuf = new byte[2];
int pitch0_cur, yaw0_cur, roll0_cur, pitch_cur, yaw_cur, roll_cur;
g_serial.readBytes(inBuf);
// Had to do some type conversion since Java doesn't support unsigned bytes
pitch0_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
g_serial.readBytes(inBuf);
yaw0_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
g_serial.readBytes(inBuf);
roll0_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
g_serial.readBytes(inBuf);
pitch_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
g_serial.readBytes(inBuf);
yaw_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
g_serial.readBytes(inBuf);
roll_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
pitch0.addVal(pitch0_cur);
yaw0.addVal(yaw0_cur);
roll0.addVal(roll0_cur);
pitch.addVal(pitch_cur);
yaw.addVal(yaw_cur);
roll.addVal(roll_cur);
if (g_dumpToFile) // Dump data to a file if needed
{
String tempStr;
tempStr = pitch0_cur + "," + yaw0_cur + "," + roll0_cur + "," + pitch_cur + "," + yaw_cur + "," + roll_cur + "\r\n";
FileWriter file;
try
{
file = new FileWriter("c:\\output.txt", true); //bool tells to append
file.write(tempStr, 0, tempStr.length()); //(string, start char, end char)
file.close();
}
catch(Exception e)
{
println("Error: Can't open file!");
}
}
/*
print(pitch0_cur); print(" "); print(yaw0_cur); print(" "); print(roll0_cur); print(" ");
print(pitch_cur); print(" "); print(yaw_cur); print(" "); println(roll_cur);
*/
}
}
// This class helps mangage the arrays of data I need to keep around for graphing.
class cDataArray
{
float[] m_data;
int m_maxSize;
int m_startIndex = 0;
int m_endIndex = 0;
int m_curSize;
cDataArray(int maxSize)
{
m_maxSize = maxSize;
m_data = new float[maxSize];
}
void addVal(float val)
{
if (g_enableFilter && (m_curSize != 0))
{
int indx;
if (m_endIndex == 0)
indx = m_maxSize-1;
else
indx = m_endIndex - 1;
m_data[m_endIndex] = getVal(indx)*.5 + val*.5;
}
else
{
m_data[m_endIndex] = val;
}
m_endIndex = (m_endIndex+1)%m_maxSize;
if (m_curSize == m_maxSize)
{
m_startIndex = (m_startIndex+1)%m_maxSize;
}
else
{
m_curSize++;
}
}
float getVal(int index)
{
return m_data[(m_startIndex+index)%m_maxSize];
}
int getCurSize()
{
return m_curSize;
}
int getMaxSize()
{
return m_maxSize;
}
}
// This class takes the data and helps graph it
class cGraph
{
float m_gWidth, m_gHeight;
float m_gLeft, m_gBottom, m_gRight, m_gTop;
cGraph(float x, float y, float w, float h)
{
m_gWidth = w;
m_gHeight = h;
m_gLeft = x;
m_gBottom = g_winH - y;
m_gRight = x + w;
m_gTop = g_winH - y - h;
}
void drawGraphBox()
{
stroke(0, 0, 0);
rectMode(CORNERS);
rect(m_gLeft, m_gBottom, m_gRight, m_gTop);
}
void drawLine(cDataArray data, float minRange, float maxRange)
{
float graphMultX = m_gWidth/data.getMaxSize();
float graphMultY = m_gHeight/(maxRange-minRange);
for(int i=0; i<data.getCurSize()-1; ++i)
{
float x0 = i*graphMultX+m_gLeft;
float y0 = m_gBottom-((data.getVal(i)-minRange)*graphMultY);
float x1 = (i+1)*graphMultX+m_gLeft;
float y1 = m_gBottom-((data.getVal(i+1)-minRange)*graphMultY);
line(x0, y0, x1, y1);
}
}
}
//================= End
沒有留言:
張貼留言