task main() {
byte inputs = 0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Proto");
nxtDisplayCenteredTextLine(3, "Test 2");
nxtDisplayCenteredTextLine(5, "Connect HTPB");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
while(true) {
eraseDisplay();
// Fetch the state of the digital IO pins. When not explicitly
// configured as input or output, they will default to input.
inputs = HTPBreadIO(HTPB, 0x3F);
nxtDisplayTextLine(4, "D: 0x%x", ubyteToInt(inputs));
wait1Msec(100);
}
}
sub NunchukGetParamter(tSensors port, byte i2c_cmd)
{
// Initialize the Nunchuk
SensorType[port] = sensorI2CCustom;
sendI2CMsg(port, CMDNunchukInit[0], 0);
while (nI2CStatus[port] == STAT_COMM_PENDING) ;
CMDNunchukWrite[2] =i2c_cmd;
sendI2CMsg(port, CMDNunchukWrite[0], 0);
while (nI2CStatus[port] == STAT_COMM_PENDING) ;
byte count = 6;
sendI2CMsg(port, CMDNunchukRead[0], count);
while (nI2CStatus[port] == STAT_COMM_PENDING) ; // Wait for I2C bus to be ready
if(nI2CBytesReady[port] == count){
readI2CReply(port, outbuf[0], 6);
for(int i=0; i<count; i++ ){
outbuf1[i]=ubyteToInt(outbuf[i]); //ubyte workaround for RobotC
outbuf1[i]=(outbuf1[i]^0x17) + 0x17;
}
}
else {
memset(outbuf, 0, 0);
}
}
sub NunchukGetParamter(tSensors port, byte i2c_cmd)
{
// Initialize the Nunchuk
SensorType[port] = sensorI2CCustom;
//Ask for 0 bytes
sendI2CMsg(port, CMDNunchukInit[0], 0);
while (nI2CStatus[port] == STAT_COMM_PENDING) ; // ROBOTC Wait for I2C bus to be ready
//Write at 0x00: 6 Bytes of sensor values
CMDNunchukWrite[2] =i2c_cmd;
//Write Nunchuk register address
sendI2CMsg(port, CMDNunchukWrite[0], 0);
while (nI2CStatus[port] == STAT_COMM_PENDING) ; // ROBOTC Wait for I2C bus to be ready
byte count = 6;
//Ask for count bytes
sendI2CMsg(port, CMDNunchukRead[0], count);
while (nI2CStatus[port] == STAT_COMM_PENDING) ; // Wait for I2C bus to be ready
if(nI2CBytesReady[port] == count){
//Read data from buffer
readI2CReply(port, outbuf[0], 6);
for(int i=0; i<count; i++ ){
outbuf1[i]=ubyteToInt(outbuf[i]); //ubyte workaround for ROBOTC
outbuf1[i]=(outbuf1[i]^0x17) + 0x17;
}
}
else {
// error
memset(outbuf, 0, 0);
}
}