Wii Motion Plus

原文出處 http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
任天堂革命性的產品,
自己實作了一次,
這也算是窮人的陀螺儀吧..
//========= 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

沒有留言:

張貼留言