NXTDuino beta 1

/* ------------------------------ NXTDUINO.NXC ------------------------------ */


#define PORT    S1
#define ADDR    0x22<<1
#define BUF_LEN 16

enum
{
    LOW = 0,
    HIGH = 1
};

enum
{
    INPUT = 0,
    OUTPUT = 1
};

enum
{
    CMD_PINMODE = 0x11, //{PIN, INPUT/OUTPUT}
    CMD_DIGITALWRITE, //{PIN, HIGH/LOW}
};

//NXTDuino
    void pinMode(int pin, int what);
    void digitalWrite(int pin, int what);
//

byte outBuf[] = {ADDR, 0,0,0};  //NXT --> Arduino
byte inBuf [16];                //Arduino --> NXT

int inBytes  = 0;   //Received bytes
int outBytes = 0;   //Bytes to send
long status = 0;
int bytesReady = 1;


void waitPending();
void printStatus();


task main()
{
    SetSensorLowspeed(PORT, false);
    pinMode(12, OUTPUT);


    while (true) {
        ClearScreen();

        printStatus();

        //TextOut(0, LCD_LINE3, I2CDeviceInfo(PORT, ADDR, I2C_REG_DEVICE_ID));
        //TextOut(0, LCD_LINE4, I2CDeviceInfo(PORT, ADDR, I2C_REG_VENDOR_ID));
        //TextOut(0, LCD_LINE5, I2CDeviceInfo(PORT, ADDR, I2C_REG_VERSION));

        TextOut(0, LCD_LINE7, StrCat("Received: ", NumToStr(bytesReady)));
        TextOut(0, LCD_LINE8, StrCat("inBuf[0]: ", NumToStr(inBuf[0])));

        digitalWrite(12, HIGH);
        Wait(500);
        digitalWrite(12, LOW);
        Wait(500);

    }
}


//NXTDuino
    void pinMode(int pin, int what)
    {
        outBuf[1] = CMD_PINMODE;
        outBuf[2] = pin;
        outBuf[3] = what;
        I2CWrite(PORT, 0x0, outBuf);
    }

    void digitalWrite(int pin, int what)
    {
        outBuf[1] = CMD_DIGITALWRITE;
        outBuf[2] = pin;
        outBuf[3] = what;
        I2CWrite(PORT, 0x0, outBuf);
    }
//

void waitPending()
{
    while(I2CCheckStatus(PORT) == STAT_COMM_PENDING)
    {
        Wait(1);
    }
}
void printStatus()
{
    if(I2CCheckStatus(PORT) == NO_ERR)
    {
        TextOut(0, LCD_LINE1, "CONNECTED");
    }
    else
    {
        TextOut(0, LCD_LINE1, "NOT CONNECTED");
    }
}

/* ------------------------------ NXTDUINO.INO ------------------------------ */

#include <Wire.h>

#define ADDR 0x22 // the double in the NXT (e.g. Arduino: 0x13, NXT: 0x26)

enum
{
    CMD_PINMODE = 0x11, //{PIN, INPUT/OUTPUT}
    CMD_DIGITALWRITE,   //{PIN, HIGH/LOW}
};

byte SensorName[9] = "Arduino ";
byte SensorVersion[9] = "v1.0    ";
byte SensorType[9] = "amazin  ";

void requestEvent(void);
void receiveEvent(int howMany);

byte bufferReceived[16];
byte bufferToSend[16];
byte receivedCount = 0;

void setup()
{
    Serial.begin(115200);
    Wire.begin(ADDR);
    Wire.onRequest(requestEvent);
    Wire.onReceive(receiveEvent);
    Serial.println("NXTDuino Inited");
}

void loop()
{
    delay(100);
}

void requestEvent(void)
{
    Serial.println("Data request!\n");

    if(receivedCount == 0) //What does this guy want?
    {
        Wire.write(0x0);
        return;
    }

    //First byte (aka Register)
        switch(bufferReceived[0])
        {
            //"Sensor" stuff
                case 0x0: //Version
                    Wire.write(SensorVersion, 8);
                    break;
                case 0x08: //Name
                    Wire.write(SensorName, 8);
                    break;
                case 0x10: //Type
                    Wire.write(SensorType, 8);
                    break;


            //What? Nothing!?
                default:
                    Wire.write(0x0);
                    break;
        }
}

void receiveEvent(int howMany)
{
    Serial.print("-->Received: ");
    Serial.print(howMany, HEX);
    Serial.println(" byte(s)\n");

    for(receivedCount = 0; Wire.available() > 0; receivedCount++)
    {
        bufferReceived[receivedCount] = Wire.read();
        //Serial.println(bufferReceived[receivedCount], HEX);
    }

    switch(bufferReceived[0])
    {
        //Commands, from 0x11 to 0xFF

            case 0x11:  //pinMode
                pinMode(bufferReceived[1], bufferReceived[2]);
                Wire.write(0x1);
                break;
            case 0x12:  //digitalWrite
                digitalWrite(bufferReceived[1], bufferReceived[2]);
                Wire.write(0x1);
                break;
    }
}