I2C solution for Tx1 to RPLidar - MDHSRobotics/TeamWiki GitHub Wiki

#include <Wire.h>


#define SLAVE_ADDRESS 0x41
#define STATUS_I2C_DATA_READY 		0x41
#define STATUS_I2C_DATA_NOT_READY 	0x00
#define LOOP_DELAY_DEFAULT 			50       // Wait 50us, need to assess correct loop delay

/********* REGISTER DEFINITIONS **********/
// The lidar will stream a bunch of data points
// each data point consists are 10 bytes in size and consists of the following
#define SYNCH_SIZE 1			//SYNCH is a 1 byte flag that indicates the angle that is the beginning of a scan
#define QUAL_SIZE  1			//QUAL  is a 1 byte flag that indicates whether the lidar was able to make a measurement at the specified angle of the current point
#define ANGLE_SIZE 4			//ANGLE is a 4 byte field that indicates the angle of the current point
#define DIST_SIZE  4			//DIST  is a 4 byte field that indicates distance measurement at the specified angle of the current point
#define I2C_WORD_SIZE 4			//WORD is a general purpose 4 byte field

// the register is optimized for SMBus protocol which defines block reads as 32 byte blocks
// a register can hold up to 3 data points
#define STATUS_SIZE 1			//status is a 1 byte field that indicates the status of the I2C device, e.g. whether data is ready for the host to pick up
#define COUNT_SIZE  1			//count  is a 1 byte field that indicates the number of points sent in this register
								//up to 3 points can be transmitted in each register read
#define POINT_SIZE  10			//point  is a 10 byte field that stores the values associated with a point
#define READ_REGISTER_SIZE  32	//the read register is a 32 byte register optimized for SMBus block read
								//it incorporates the status and the data elements, count and 3 data points
#define TEMP_REGISTER_SIZE REGISTER_SIZE - I2C_WORD_SIZE

//REGISTER ADDRESSES
//FRONT PART is the read register
#define READ_REGISTER      0x00		
#define STATUS_ADDRESS     READ_REGISTER					//1 byte status positioned at the beginning of the READ_REGISTER
#define COUNT_ADDRESS      STATUS_ADDRESS + STATUS_SIZE		//1 byte count positioned after status
#define POINT1_ADDRESS     COUNT_ADDRESS  + COUNT_SIZE		//1 byte count positioned after status
#define POINT2_ADDRESS     POINT1_ADDRESS + POINT_SIZE		//1 byte count positioned after status
#define POINT3_ADDRESS     POINT2_ADDRESS + POINT_SIZE		//1 byte count positioned after status

//After the read register is where the write registers and the address are
#define MODE_ADDRESS        READ_REGISTER  + READ_REGISTER_SIZE	//4 byte word to indicate allow host to configure device
#define CONFIG_ADDRESS      MODE_ADDRESS   + I2C_WORD_SIZE 		//4 byte word to indicate allow host to configure device
#define ID_ADDRESS          CONFIG_ADDRESS + I2C_WORD_SIZE 		//4 byte word to indicate the device address.  least significant bits are used for the address.

#define REGISTER_SIZE READ_REGISTER_SIZE + 3*I2C_WORD_SIZE


/********* I/O PIN DEFINITIONS **********/
#define SLAVE_STATUS_LED 13

/********* COMMANDS *********************/
#define LED_COMMAND 0x4C00

/********* Global  Variables  ***********/

byte registerMap[REGISTER_SIZE];						//the actual interface register
byte registerMapTemp[REGISTER_SIZE - I2C_WORD_SIZE];	//register where data is written to before being published to the interface register
unsigned char deviceStatus = STATUS_I2C_DATA_NOT_READY;
bool newDataAvailable = false; 
bool configModeUpdated = false;

//commands can be sent from the host to the slave
//a read command has a 1 byte argument that indicates where in the register we want to read from
//a write command has an address and a word, it'll either be a mode word or a config word, or a write to both
//so the max data payload size of the command is the size of 2 words + 1 address byte
#define COMMAND_SIZE 1+2*I2C_WORD_SIZE
byte receivedCommands[];	

// mode structure  4 bytes
// 0 - command , //2 byte mode setting
// 1 - data 1 ,  //2 byt mode value
unsigned int mode[I2C_WORD_SIZE / sizeof(unsigned int)];

// config structure
// 0 - command , //2 byte config setting
// 1 - data 1 ,  //2 byt config value
unsigned int configuration[I2C_WORD_SIZE / sizeof(unsigned int)];


void setup() {
  //Set up the Serial Monitor
  //The serial monitor is used to dosplay log messages and enable us to see what is happening inside the Arduini.  It is the main debugging tool
  //This can be disabled or commented out when debugging is no longer needed
  Serial.begin(9600); // Open serial monitor at 9600 baud to see ping results.

  Serial.println("Initializing subsystems ...");
  setupI2C();
  setupPins();
}

void  setupPins(){
  Serial.println("Setting up pins");
  pinMode(SLAVE_STATUS_LED,OUTPUT);
  digitalWrite(SLAVE_STATUS_LED,LOW);
}


void setupI2C(){
  Serial.println("setting up I2C");
  Wire.begin(SLAVE_ADDRESS);
  Wire.onRequest(requestEvent);
  Wire.onReceive(receiveEvent);
}

void requestEvent(){
  if(newDataAvailable) memcpy(registerMap, registerMapTemp, TEMP_REGISTER_SIZE);
  newDataAvailable = false;
  Wire.write(registerMap+receivedCommands[0], REGISTER_SIZE - receivedCommands[0]);  //Set the buffer up to send all 17 bytes of data
}

void receiveEvent(int bytesReceived){
  for (uint8_t a = 0; a < bytesReceived; a++){
    if ( a < MAX_COMMAND_SIZE){
         receivedCommands[a] = Wire.read();
    }
    else{
         Wire.read();  // if we receive more data then allowed just throw it away
    }
  }
  if(bytesReceived == 1 && (receivedCommands[0] < REGISTER_SIZE))
  {
  	  //request is a read from the specified address
      return; 
  }
  if(bytesReceived == 1 && (receivedCommands[0] >= REGISTER_SIZE))
  {
  	  //request is a read to an invalid address, reset the address to 0
      receivedCommands[0] = 0x00;
      return;
  }

  byte address = receivedCommands[0];
//  Serial.print("received more than 1 byte"); //meaning that the master is writing to the slave.
//  Serial.print(", address writing to is: 0x");
//  Serial.print(address,HEX);
//  Serial.println();
  if(address < CONFIG_ADDRESS || address >= ID_ADDRESS){
//  we can only write to either the config or mode registers
//  Serial.println('address is not a valid write address');
    return;
  }

  if(address == CONFIG_ADDRESS){
  	//we are receiving a config word to write into the config register
	memcpy(configuration, receivedCommands+1, I2C_WORD_SIZE);
  }
  else if(address == MODE_ADDRESS){
  	//we are either receiving a mode word or a mode word and a config word to write into the register
	memcpy(mode, receivedCommands+1, bytesReceived-1);
  }
  else{
//  we can only write to either the config or mode registers
//  Serial.println('address is not a valid write address');
    return;
  }
  configModeUpdated = true;  //we had a valid request to either update configuration or mode
}



void loop() {
  //Serial.println("looping...");

  if(configModeUpdated){
    configModeUpdate();
  }
  delayMicroseconds(LOOP_DELAY_DEFAULT);                      
}

void configModeUpdate(){
    configModeUpdated = false;
    switch(mode[0]){
      case LED_COMMAND:
        switch(mode[1]){
          case 0x4800:
          digitalWrite(SLAVE_STATUS_LED,HIGH);
          break;
          case 0x4C00:
          digitalWrite(SLAVE_STATUS_LED,LOW);
          break;
        }
        break;
    }

//    printMode();
}

void printMode(){
  char tmp[85];
  sprintf(tmp,"mode: [%.4X %.4X %.4X]",mode[0],mode[1],mode[2]);
  Serial.print(tmp);
}