Mindboards

Code: Select all

byte CMDNunchukInit[] = {0xA4, 0x40, 0x00};
byte CMDNunchukWrite[] = {0xA4, 0x00};
byte CMDNunchukRead[] = {0xA4};

byte nbytes;
byte outbuf[];

// port is the NXT port
// i2c_cmd is the nunchuk internal address register
// at i2c_cmd=0...5 are 6 Byte of sensor values
// at i2c_cmd=0x20 ... 0x2F  you get 16 Byte of calibration data
sub NunchukGetParamter(byte port, byte i2c_cmd)
{
  // Initialize the Nunchuk
  SetSensorLowspeed(port);
  LowspeedWrite(port, 0, CMDNunchukInit); //ask for 0 bytes
  while(LowspeedStatus(port, nbytes)>0);

  // Wait(1) is needless, the NXT command interpreter delays next i2c access 3ms

  CMDNunchukWrite[1] =i2c_cmd; //at 0x00: 6 Bytes of sensor values
  LowspeedWrite(port, 0, CMDNunchukWrite);   // write Nunchuk register address
  while(LowspeedStatus(port, nbytes)>0);

  byte count = 6;
  LowspeedWrite(port, count, CMDNunchukRead); //ask for count bytes
  while(LowspeedStatus(port, nbytes)>0);

  if(nbytes == count){
    LowspeedRead(port, count, outbuf)  //read data from buffer
    for(int i=0; i<count; i++ ){
      outbuf[i]=(outbuf[i]^0x17) + 0x17;
    }
  }
  else {
    ArrayInit(outbuf,0,0);    //error
  }
}
int angleX, angleY;
task main()
{
// Initialize the 3D engine...
glInit();
// Create a box, this is the first object which will be object id 0...
glBox(GL_POLYGON, 10, 20, 30);
// Create a box, this is the second object which will be object id 1...
glBox(GL_POLYGON, 30,40,50);
//glSetAngleX(0);//100
//glSetAngleY(0);//125
angleX = 0;
angleY = 0;
while (true)
{
    /*
    // Setup for rendering...
    glBeginRender();
    // Call the object with id 0...
    glCallObject(0);

    glObjectAction(0, GL_ROTATE_X, angleX);
    
    // Call the object with id 1...
    glCallObject(1);
    glObjectAction(1, GL_ROTATE_Y, angleY);
    // Finish, clear the screen, rotate and render the called objects...
    glFinishRender();
    */
    NunchukGetParamter(S1, 0x00);
    if (ArrayLen(outbuf)==6)
    {
        glBeginRender();
        glCallObject(0);
        //NumOut(0,0,(outbuf[0]),false);
        //NumOut(0,10,(outbuf[1]),false);
        glObjectAction(0, GL_ROTATE_Y, (225-outbuf[0]));
        glObjectAction(0, GL_ROTATE_X, (outbuf[1]));

        glCallObject(1);
        glObjectAction(1, GL_ROTATE_Y, ((225-outbuf[2])*3));
        glObjectAction(1, GL_ROTATE_X, (outbuf[3])*3);
    
        glFinishRender();
        //Wait(1000);
    }
}
}