#pragma config(Sensor, S1,TIR,sensorI2CCustom)
/*
How to connect your LEGO MINDSTORMS NXT with an Arduino.
http://dexterind.wpengine.com/howto/
*/

// First, define the Arduino Address
// Address is 0x0A on the Arduino: (Binary) 1010
// Bit shifted out with one 0, that becomes: (Binary) 10100
// Which is 0x14
#define ARDUINO_ADDRESS 0x14
#define ARDUINO_PORT S1

ubyte I2Cmessage[22];
char I2Creply[20];
task disbatch()
{
	motor[motorB] = 20;
	Sleep(500);
	motor[motorB] = 0;
	Sleep(1000);
	motor[motorB] = -20;
	Sleep(500);
	motor[motorB] = 0;
	nMotorEncoder[motorB] = 0;
}
task advanceBrakes()
{
	motor[motorC] = 20;
	Sleep(500);
	motor[motorC] = 0;
	Sleep(1000);
	motor[motorC] = -20;
	Sleep(500);
	motor[motorC] = 0;
}
void i2c_read_registers_text(ubyte register_2_read, int message_size, int return_size){
memset(I2Creply, 0, sizeof(I2Creply));
message_size = message_size+3;

I2Cmessage[0] = message_size; // Messsage Size
I2Cmessage[1] = ARDUINO_ADDRESS;
I2Cmessage[2] = register_2_read; // Register
sendI2CMsg(S1, &I2Cmessage[0], return_size);
wait1Msec(20);

readI2CReply(ARDUINO_PORT, &I2Creply[0], return_size);

if(I2Creply[0] == 'a')
{
	nxtDisplayString(1, "Motor = 0  ");
	motor[motorA] = 0;
}
if(I2Creply[0] == 'f')
{
	nxtDisplayString(1, "Motor = 5 ");
	motor[motorA] = 5;
}
if(I2Creply[0] == 'b')
{
	nxtDisplayString(1, "Motor = 10 ");
	motor[motorA] = 10;
}
if(I2Creply[0] == 'c')
{
	nxtDisplayString(1, "Motor = 30  ");
	motor[motorA] = 30;
}
if(I2Creply[0] == 'd')
{
	nxtDisplayString(1, "Motor = -30  ");
	motor[motorA] = -30;
}
if(I2Creply[0] == 'e')
{
	nxtDisplayString(1, "Motor = -15");
	motor[motorA] = -15;
}
if(I2Creply[0] == 'g')
{
	StartTask(disbatch);
}
if(I2Creply[0] == 'h')
{
	StartTask(advanceBrakes);
}
}

task main()
{
while(true){
// i2c_write_registers(0x01, 0x01, 0x00, 0x0A, 0, 0, 0);
i2c_read_registers_text(0x01, 0, 10); // Here we’re going to get back “Dexter” because we’re writing to the 0x01 register.

wait1Msec(1000);
}

}