float gryo_axis_reading_linked(ubyte axis){
// ubyte axis definitions
// 0x00 - x-axis
// 0x01 - y-axis
// 0x02 - z-axis
ubyte I2Csnd[3]; //
I2Csnd[0] = 2; // Sending address, register.
I2Csnd[1] = 0xD2; // I2C Address of gyro.
ubyte I2Crec[2]; // We are looking for a single byte returned.
ubyte hb = 0;
ubyte lb = 0;
// We will set the value of divisor. This is important to get a scaled output in dps.
int divisor = 128;
if(full_scale_range == 0x10) divisor = 64;
if(full_scale_range == 0x30) divisor = 16;
if(axis == 0x00) { // x-axis
I2Csnd[2] = 0x28; // Register of the data we're requesting.
sendI2CMsg(S1, I2Csnd[0], 2); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 2); // (Port 1, Reply Array, Bytes to Read)
// hb = gyro_axis_reading_byte(0x29);
// lb = gyro_axis_reading_byte(0x28);
lb = I2Crec[0];
hb = I2Crec[1];
}
if(axis == 0x01) { // y-axis
I2Csnd[2] = 0x2B; // Register of the data we're requesting.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 2); // (Port 1, Reply Array, Bytes to Read)
// ubyte result = I2Crec[0];
// hb = gyro_axis_reading_byte(0x2B);
// lb = gyro_axis_reading_byte(0x2A);
hb = I2Crec[0];
lb = I2Crec[1];
}
if(axis == 0x02) { // z-axis
I2Csnd[2] = 0x2D; // Register of the data we're requesting.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 2); // (Port 1, Reply Array, Bytes to Read)
// hb = gyro_axis_reading_byte(0x2D);
// lb = gyro_axis_reading_byte(0x2C);
hb = I2Crec[0];
lb = I2Crec[1];
}
float val = (lb+((long)(hb<<8)))/divisor; // Assemble the final number by assembling the two bytes,
// and dividing it by the divisor (defined in the gyro startup,
// to get a properly scaled long.
return val;
}
void start_gyro(ubyte range){
ubyte I2Csnd[4];
I2Csnd[0] = 3; // Sending address, register, value.
I2Csnd[1] = 0xD2; // I2C Address of gyro.
ubyte I2Crec[1]; // We are looking for a single byte returned.
// full_scale_range = + 0x80;
//Write CTRL_REG1
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x20; // Register address of CTRL_REG1
I2Csnd[3] = 0x0F; // Enable all axes. Disable power down.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG2
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x21; // Register address of CTRL_REG2
I2Csnd[3] = 0x00; // No High Pass Filter
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG3
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x22; // Register address of CTRL_REG3
I2Csnd[3] = 0x08; // No interrupts. Date ready.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG4
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x23; // Register address of CTRL_REG4
I2Csnd[3] = range+0x80; // Full scale range.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG5
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x24; // Register address of CTRL_REG5
I2Csnd[3] = 0x00; // Enable all axes. Disable power down.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
// Set divisor so that the output of our gyro axis readings can be turned
// into scaled values.
///////////////////////////////////////////////////////////////////////////
if(full_scale_range == 0) divisor = 128; // Full scale range is 250 dps.
if(full_scale_range == 0x10) divisor = 64; // Full scale range is 500 dps.
if(full_scale_range == 0x30) divisor = 16; // Full scale range is 2000 dps.
}
// Set the sensor to the selected frequency
void setHz(tSensors link, byte hertz) {
byte msg[4] = { 3, 0x02, 0x41, 0x00 };
if (hertz == 50) {
msg[3] = 0x35;
} else if (hertz == 60) {
msg[3] = 0x36;
} else {
playSound(soundException);
eraseDisplay();
displayCenteredTextLine(3, "Wrong freq.");
displayCenteredTextLine(3, "specified");
sleep(5000);
stopAllTasks();
}
while (nI2CStatus[link] == STAT_COMM_PENDING){
sleep(5);
}
sendI2CMsg(link, &msg[0], 0);
if (nI2CStatus[link] == ERR_COMM_BUS_ERR) {
playSound(soundException);
eraseDisplay();
displayCenteredTextLine(3, "Error setting");
displayCenteredTextLine(3, "sensor to 50Hz");
sleep(5000);
stopAllTasks();
}
}
void start_gyro(){
ubyte I2Csnd[4];
I2Csnd[0] = 3; // Sending address, register, value.
I2Csnd[1] = 0xD2; // I2C Address of gyro.
ubyte I2Crec[1]; // We are looking for a single byte returned.
//Write CTRL_REG1
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x20; // Register address of CTRL_REG1
I2Csnd[3] = 0x0F; // Enable all axes. Disable power down.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG2
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x21; // Register address of CTRL_REG2
I2Csnd[3] = 0x00; // No High Pass Filter
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG3
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x22; // Register address of CTRL_REG3
I2Csnd[3] = 0x08; // No interrupts. Date ready.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG4
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x23; // Register address of CTRL_REG4
I2Csnd[3] = 0x00; // Full scale range.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
//Write CTRL_REG5
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x24; // Register address of CTRL_REG5
I2Csnd[3] = 0x00; // Enable all axes. Disable power down.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
}
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);
}
}
//IR SENSOR TASK
task IR_Sense(){
ubyte request[3]; // I2C request message
ubyte reply[6]; // accelerometer returns 7 values
int replyLen; // length of reply array.
tSensors port; // port the sensor is attached to.
// which port is the accelerator hooked up to?
port = S4;
// Ensure the sensor is configured correctly
SensorType[port] = sensorI2CCustom;
eraseDisplay();
while (true) {
// Read the data from the sensor
// setup the request message....
request[0] = 2; // Message size is 2 bytes..
request[1] = I2C_ADDR; // I2C Address of sensor(0x02)
request[2] = REG_OFFSET; // location of registers to read 0x42
replyLen = 6; // how many registers to read
// send request message to sensor.
sendI2CMsg(port, &request[0], replyLen);
// wait for a reply on the sensor port.
// also check for sensor error...
if (!waitForI2C(port)) {
displayBigTextLine(4, "Error 1.");
wait1Msec(2000);
stopAllTasks();
}
// read the reply from the sensor.
readI2CReply(port, &reply[0], replyLen);
// make sure the bus is clear...
// check for error.
if (!waitForI2C(port)){
displayBigTextLine(4, "Error 2.");
wait1Msec(2000);
stopAllTasks();
}
//Waits, then updates BrightLight value
wait1Msec(5);
BrightLight = reply[0];
}
}
// Accel axis reading gets 10 bits of axis reading data.
long accl_axis_reading_10(ubyte reg){
byte LSB = 0x00; // Least significant bit of reading.
byte MSB = 0x00; // Most significant bit of reading.
long axis_reading = 0; // This will be the assembled reading.
ubyte I2Csnd[3]; //
I2Csnd[0] = 2; // Sending address, register.
I2Csnd[1] = accl_read; // I2C Address of accl.
I2Csnd[2] = reg; // First Register of the data we're requesting.
byte I2Crec[1]; // We are looking for a single byte returned each time.
// Get Least Signifcant Bit of reading.
sendI2CMsg(S1, I2Csnd[0], 1); // Port 1, Message Array, Reply Size
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // Port 1, Reply Array, Bytes to Read
LSB = I2Crec[0]; // & 0xFF; // Save the LSB to record.
wait1Msec(10); // Wait some time for clean I2C.
I2Csnd[2] = reg+1; // Get the second register information.
// Get Most Signifcant Bit of reading.
sendI2CMsg(S1, I2Csnd[0], 1); // Port 1, Message Array, Reply Size
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // Port 1, Reply Array, Bytes to Read
MSB = I2Crec[0]; // & 0xFF; // Save the MSB to record.
int iLSB = LSB;
int iMSB = (MSB)<<8;
axis_reading = LSB + (MSB<<8 );
if(axis_reading > 512){
axis_reading >>= 8;
axis_reading <<= 8;
//axis_reading = ~axis_reading;
axis_reading *= -1;
}
//Retrieve data from the sound sensor
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);
int i = 0,top=0;
//Pitch
if(cmd==1)
writeDebugStreamLine("%d", (int)I2Creply[0]+(int)I2Creply[1]*256);
//Sound Power Level
else if(cmd==2)
//For detecting clap
//if((int)I2Creply>85)
writeDebugStreamLine("%i", (int)I2Creply[0]);
//FFT
else if(cmd==3)
{
for(int x = 0; x <return_size; x++)
{
writeDebugStream("%i", I2Creply[x]);
writeDebugStream(" ");
}
writeDebugStreamLine(" ");
//Graphic Equalizer
nxtEraseRect(6,62,99,6);
nxtDrawLine(5,2,5,61);
nxtDrawLine(2,5,95,5);
for(int j=0;j<return_size;j++)
{
top=I2Creply[j]+5;
if(top>61)
top=61;
nxtDrawRect(5+6*j,top,5+6*(j+1),5);
}
}
}
//Send command to the processor
void i2c_write_registers(ubyte register_2_write, int message_size, int return_size, ubyte byte1, ubyte byte2){
memset(I2Creply, 0, sizeof(I2Creply));
message_size = message_size+3;
I2Cmessage[0] = message_size; // Messsage Size
I2Cmessage[1] = ARDUINO_ADDRESS;
I2Cmessage[2] = register_2_write; // Register
I2Cmessage[3] = byte1;
I2Cmessage[4] = byte2;
//I2Cmessage[5] = byte3;
sendI2CMsg(ARDUINO_PORT, &I2Cmessage[0], return_size);
wait1Msec(20);
}
// Acell axis_reading gets a byte of axis reading data.
byte accl_axis_reading_8(ubyte reg){
ubyte I2Csnd[3]; //
I2Csnd[0] = 2; // Sending address, register.
I2Csnd[1] = accl_read; // I2C Address of accl.
I2Csnd[2] = reg; // Register of the data we're requesting.
byte I2Crec[1]; // We are looking for a single byte returned.
// Must be signed. We're looking for a number between -128 and 128
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
wait1Msec(10);
return I2Crec[0];
}
// Acell axis_reading gets a byte of axis reading data
ubyte accl_axis_reading(ubyte reg){
ubyte I2Csnd[3]; //
I2Csnd[0] = 2; // Sending address, register.
I2Csnd[1] = 0x3A; // I2C Address of accl.
I2Csnd[2] = reg; // Register of the data we're requesting.
ubyte I2Crec[1]; // We are looking for a single byte returned.
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
ubyte result = I2Crec[0];
wait1Msec(10);
return I2Crec[0];
}
void start_accl(){
ubyte I2Csnd[4];
I2Csnd[0] = 3; // Sending address, register, value.
I2Csnd[1] = 0x3A; // I2C Address of Accelerometer.
ubyte I2Crec[1]; // We are looking for a single byte returned.
//Set the Mode Control
////////////////////////////////////////////////////////////////////////////
I2Csnd[2] = 0x16; // Register address of Mode Control
I2Csnd[3] = 0x09; // 0x05 For 2G
// 0x09 For 4G
// 0x01 For 8G
sendI2CMsg(S1, I2Csnd[0], 1); // (Port 1, Message Array, Reply Size)
wait1Msec(10);
readI2CReply(S1, I2Crec[0], 1); // (Port 1, Reply Array, Bytes to Read)
}
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);
int i = 0;
while(true){
writeDebugStream("%c", I2Creply[i]);
i++;
if(I2Creply[i] == 0) break;
}
writeDebugStreamLine(" ");
}
task SonarDeviceDriver()
{
//
// Message constants for Lego Sonar (Ultra Sonic) sensor
//
const ubyte kSonarID = 2;
const ubyte kContinuousMeasurement = 0x02; // 0x01 for single shot mode, 0x02 for continuous measurement
const ubyte kInitializeSonar = 0x41;
const ubyte kReadSonarResult = 0x42;
const ubyte kReset = 0x04; // for a warm reset
const ubyte kSetInterval = 0x40; // sets the interval at continuous measurement
const ubyte kInterval = 0x01; // The interval
//
// I2C Message definition for SONAR sensor
//
static const byte kSonarInitialize[4] = {3, kSonarID, kInitializeSonar, kContinuousMeasurement};
static const byte kSonarReset[4] = {3, kSonarID, kInitializeSonar, kReset};
static const byte kSonarInterval[4] = {3, kSonarID, kSetInterval, kInterval};
static const byte kSonarRead[3] = {2, kSonarID, kReadSonarResult};
//
// state machine for SONAR "device driver"
//
typedef enum
{
stateInitializing,
stateWaitForInitDone,
stateSendPollMessage,
stateWaitingForReply,
stateResetting,
stateWaitForReset,
stateSettingInterval,
stateWaitForSettingInterval
} TSonarState;
static TSonarState nSonarState;
long lastTime;
long interval;
//
// Set up port 'kTestPort' as user defined I2C sensor
//
nI2CRetries = 0;
SensorType[kTestPort] = sensorI2CCustom9V;
nSonarState = stateResetting;
while(true)
{
//
// Loop forever, polling the SONAR sensor
//
switch (nSonarState)
{
case stateResetting:
//
// Send rest message to Sonar sensor
//
sendI2CMsg(kTestPort, kSonarReset[0], 0);
nSonarState = stateWaitForReset;
break;
case stateWaitForReset:
//
// Wait for Sonar sensor warm reset to complete
//
switch (nI2CStatus[kTestPort])
{
case NO_ERR:
nSonarState = stateInitializing;
wait1Msec(kDelayBForHWStartup);
break;
case STAT_COMM_PENDING:
// Keep waiting for reply. I2C messaging is not complete
wait1Msec(1);
break;
default:
case (TI2CStatus) ERR_COMM_BUS_ERR:
// re-initialize sensor. An I2C messaging error occurred.
PlaySound(soundException);
nSonarState = stateResetting;
break;
}
break;
case stateSettingInterval:
//
// Send initialize message to Sonar sensor
//
sendI2CMsg(kTestPort, kSonarInterval[0], 0);
nSonarState = stateWaitForSettingInterval;
break;
case stateWaitForSettingInterval:
//
// Wait for Sonar sensor initialization to complete
//
switch (nI2CStatus[kTestPort])
{
case NO_ERR:
nSonarState = stateSendPollMessage;
wait1Msec(kDelayBForHWStartup);
break;
case STAT_COMM_PENDING:
// Keep waiting for reply. I2C messaging is not complete
wait1Msec(1);
break;
default:
case (TI2CStatus) ERR_COMM_BUS_ERR:
// re-initialize sensor. An I2C messaging error occurred.
PlaySound(soundException);
nSonarState = stateResetting;
break;
}
break;
case stateInitializing:
//
// Send initialize message to Sonar sensor
//
sendI2CMsg(kTestPort, kSonarInitialize[0], 0);
nSonarState = stateWaitForInitDone;
break;
case stateWaitForInitDone:
//
// Wait for Sonar sensor initialization to complete
//
switch (nI2CStatus[kTestPort])
{
case NO_ERR:
nSonarState = stateSettingInterval;
wait1Msec(kDelayBForHWStartup);
break;
case STAT_COMM_PENDING:
// Keep waiting for reply. I2C messaging is not complete
wait1Msec(1);
break;
default:
case (TI2CStatus) ERR_COMM_BUS_ERR:
// re-initialize sensor. An I2C messaging error occurred.
PlaySound(soundException);
nSonarState = stateResetting;
break;
}
break;
case stateSendPollMessage:
//
// Ready to send next polling message to Sensor
//
nI2CBytesReady[kTestPort] = 0; // Clear any ending bytes
sendI2CMsg(kTestPort, kSonarRead[0], kSonarReplySize);
nSonarState = stateWaitingForReply;
break;
case stateWaitingForReply:
//
// Wait for reply from SONAR sensor
//
switch (nI2CStatus[kTestPort])
{
case NO_ERR:
// Reply from SONAR sensor is ready to be read.
// - Update sensor value
// - Setup to send next polling message.
//
// Read the SONAR value and store it in "SensorValue" array so that it can be used
// by other tasks as a standard sensor.
//
memset(replyMsg,0x00,kSonarReplySize);
readI2CReply(kTestPort, replyMsg[0], kSonarReplySize);
SensorValue[kTestPort]=replyMsg[0];
nSonarState = stateSendPollMessage;
if (resetAfterEveryRead) nSonarState= stateResetting;
interval=nSysTime-lastTime;
lastTime=nSysTime;
//
// Sonar hardware delay. If the next poll request is immediately sent to
// the SONAR sensor, then the message gets corrupted and generates a
// 'ERR_COMM_BUS_ERR' error. So we'll put a little delay in here. I'm not
// certain if this is a problem in the sonar sensor or a problem in the
// bit-banged firmware trying to do too many things in one timeslice or
// a problem in the ROBOTC firmware. The error message is that when
// transmitting the next polling I2C message an ack is not received by the
// the ARM processor after transmitting a byte of the message. Suspicion
// is that this is bug in SONAR sensor.
//
// We really only need to wait a few msec to fix above problem. But might as
// well go ahead and wait longer because sonar pulses are only sent out on a
// 50 to 80 period.
//
if (kDelayBForHWStartup != 0)
wait1Msec(kDelayBetweenPolls);
break;
case STAT_COMM_PENDING:
// Insert short wait for reply. I2C messaging xmit/rcv is not complete
wait1Msec(1);
break;
default:
case (TI2CStatus) ERR_COMM_BUS_ERR:
// re-initialize sensor. An I2C messaging error occurred.
PlaySound(soundException);
nSonarState = stateResetting;
wait1Msec(2);
break;
}
break;
}
}
return;
}
/*---------------------------------------------------------------------------*/
i2cStatus
i2cWriteData( char address, char *buf, int buflen, int replylen = 0 )
{
static bool init = true;
i2cStatus status = kMessageSuccess;
char txbuf[34];
// 32 bytes max buffer is determined by the firmware
if( buflen > MAX_I2C_MESSAGE )
return( kMessageTooLong );
// We need to send one message to disable IME polling
// Or enable termination
if( init ) {
// Dummy message
txbuf[0] = 1;
txbuf[1] = 0;
txbuf[2] = 0;
sendI2CMsg( txbuf, 0 );
// We must wait for the I2C state machine to timeout
// or the disable termination message to be sent to an IME
wait1Msec(100);
init = false;
}
// Loop waiting for last message to be sent
int timeout = DEFAULT_I2C_TIMEOUT;
while( nI2CStatus != i2cStatusDone && timeout > 0) {
timeout--;
wait1Msec(1);
}
// We waited too long for the message to be sent
if(timeout == 0) {
// Check and see if we are stuck on Start Bit or Address send
if( nI2CStatus == i2cStatusStart || nI2CStatus == i2cStatusAddress )
status = kMessageError;
else
status = kMessageSending;
}
// Setup message, first byte is data length, second is address
txbuf[0] = buflen;
txbuf[1] = address;
// Copy transmit data
for(int i=0;i<buflen;i++)
txbuf[i+2] = buf[i];
// Transmit
sendI2CMsg( txbuf, replylen );
// look at status, sort of meaningless
// The message will either be sent or block waiting for ACK
// There is no timeout on the low level code we can use
if( nI2CStatus == i2cStatusStart )
status = kMessageError;
return( status );
}
task main()
{
int hUpper, hLower, heading;
TButtons nBtn;
SensorType[CS] = sensorI2CCustom;
wait10Msec(5);
nI2CBytesReady[CS]=0;
while(nI2CStatus[CS]== STAT_COMM_PENDING)
wait1Msec(2);
byte replyMessage[6];
TI2C_Output sOutput;
while(true)
{
nxtDisplayTextLine(1, "Mode = Read");
nI2CBytesReady[CS] = 0;
sOutput.nMsgSize = 2; //read the value from the sensor
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x42;
sendI2CMsg(CS, sOutput.nMsgSize, 6);
while (nI2CStatus[CS] == STAT_COMM_PENDING)
wait1Msec(2);
if (nI2CStatus[CS] == NO_ERR)
{
readI2CReply(CS, replyMessage[0], 6); //get the compass heading
hUpper = (replyMessage[0] & 0x00FF);
hLower = (replyMessage[1] & 0x00FF);
heading = (hUpper << 1) | hLower ; //convert contents of 0x44 0x45
nxtDisplayTextLine(4, "U-val %d", hUpper);
nxtDisplayTextLine(5, "L-val %d", hLower);
nxtDisplayTextLine(6, "Heading = %d", heading);
}
else
{
nxtDisplayTextLine(1, "i2c err %d", nI2CStatus[CS]);
}
//check if a button has been pressed.
nBtn = nNxtButtonPressed; // check for button press
switch (nBtn)
{
case kLeftButton:
{
PlayTone(440,1000);
sOutput.nMsgSize = 3;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x41; //Calibration mode command
sOutput.nCompassMode = 0x43;
nI2CBytesReady[CS] = 0;
sendI2CMsg(CS, sOutput.nMsgSize, 0);
wait1Msec(40);
while ((nBtn = nNxtButtonPressed) != kRightButton)
{ //wait for the right button to be pressed before leaving cal mode;
nxtDisplayTextLine(1, "Mode = Cal ");
wait1Msec(1000);
PlaySound(soundBlip);
}
break;
}
case kRightButton:
{
PlaySound(soundBeepBeep);
sOutput.nMsgSize = 3;
sOutput.nDeviceAddress = 0x02;
sOutput.nLocationPtr = 0x41;
sOutput.nCompassMode = 0x00; //Read mode command
nI2CBytesReady[CS] = 0;
sendI2CMsg(CS, sOutput.nMsgSize, 0);
wait1Msec(1250); //wait for the cal info to be saved
break;
}
}
}
}
bool getI2CInfo(const tSensors nIntPort, string& sManufacturer, string& sType)
{
tSensors nPort = nIntPort;
TSensorTypes nSaveSensorType;
ubyte nTemp[9];
bool bI2CType;
static const ubyte kReadManufacturer[] = {2, 0x02, 0x08};
static const ubyte kReadSensorType[] = {2, 0x02, 0x10};
nSaveSensorType = SensorType[nPort];
SensorType[nPort] = sensorI2CCustom;
wait1Msec(50);
while (true)
{
switch (nI2CStatus[nPort])
{
case NO_ERR:
case ERR_COMM_BUS_ERR:
break;
default:
case STAT_COMM_PENDING:
wait1Msec(1);
// Keep waiting for reply. I2C messaging is not complete
continue;
}
break;
}
bI2CType = false;
sendI2CMsg(nPort, kReadManufacturer[0], 8);
while (true)
{
switch (nI2CStatus[nPort])
{
case NO_ERR:
case ERR_COMM_BUS_ERR:
bI2CType = true;
break;
default:
case STAT_COMM_PENDING:
wait1Msec(1);
// Keep waiting for reply. I2C messaging is not complete
continue;
}
break;
}
if (!bI2CType)
{
sManufacturer = "";
sType = "";
SensorType[nPort] = nSaveSensorType;
return false;
}
readI2CReply(nPort, nTemp[0], 8);
nTemp[8] = 0;
strcpy(sManufacturer, nTemp);
sendI2CMsg(nPort, kReadSensorType[0], 8);
while (true)
{
switch (nI2CStatus[nPort])
{
case NO_ERR:
case ERR_COMM_BUS_ERR:
break;
default:
case STAT_COMM_PENDING:
wait1Msec(1);
// Keep waiting for reply. I2C messaging is not complete
continue;
}
break;
}
if (!bI2CType)
{
sManufacturer = "";
sType = "";
SensorType[nPort] = nSaveSensorType;
return false;
}
readI2CReply(nPort, nTemp[0], 8);
nTemp[8] = 0;
strcpy(sType, nTemp);
SensorType[nPort] = nSaveSensorType;
return true;
}