/*
* IIC Read 1
*/
int iic_read1(XIicPs *IicPs, u8 Address, u8 *Data) {
int Status;
/*
* Wait until bus is idle to start another transfer.
*/
while (XIicPs_BusIsBusy(IicPs)) {
/* NOP */
}
/*
* Receive the data.
*/
Status = XIicPs_MasterRecvPolled(IicPs, Data, 1, Address);
if (Status != XST_SUCCESS) {
myprintf("XIicPs_MasterRecvPolled error!\n\r");
return XST_FAILURE;
}
// myprintf("[iic_read1] 0x%02X=0x%02X\n\r", Address, *Data);
return XST_SUCCESS;
}
int i2c_recv(XIicPs* i2c_dev, uint8_t dev_id, uint8_t* buffer, int buf_max)
{
while(XIicPs_BusIsBusy(i2c_dev));
return XIicPs_MasterRecvPolled(i2c_dev, buffer, buf_max, dev_id);
}
int i2c_recv(XIicPs* i2c_dev, uint8_t dev_id, uint8_t* buffer, int buf_max)
{
return XIicPs_MasterRecvPolled(i2c_dev, buffer, buf_max, dev_id);
}
/*
* IIC Read 2
*/
int iic_read2(XIicPs *IicPs, u8 Address, u8 Register, u8 *Data, int ByteCount) {
int Status;
int cnt = 0;
/*
* Wait until bus is idle to start another transfer.
*/
while (XIicPs_BusIsBusy(IicPs)) {
usleep(100);
//Timeout
cnt++;
if (cnt > IIC_TIMEOUT) {
busy = BOOL_TRUE;
return XST_DEVICE_BUSY;
}
}
/*
* Set the IIC Repeated Start option.
*/
Status = XIicPs_SetOptions(IicPs, XIICPS_REP_START_OPTION);
if (Status != XST_SUCCESS) {
return Status;
}
/*
* Send the buffer using the IIC and check for errors.
*/
Status = XIicPs_MasterSendPolled(IicPs, &Register, 1, Address);
if (Status != XST_SUCCESS) {
#ifdef DEBUG
//Get Timestamp
unsigned long timestamp_ms = getElapsedRuntimeUS()/1000;
myprintf("XIicPs_MasterSendPolled error at %d ms!\n\r", timestamp_ms);
#endif
return Status;
}
/*
* Receive the data.
*/
Status = XIicPs_MasterRecvPolled(IicPs, Data, ByteCount, Address);
if (Status != XST_SUCCESS) {
#ifdef DEBUG
//Get Timestamp
unsigned long timestamp_ms = getElapsedRuntimeUS()/1000;
myprintf("XIicPs_MasterRecvPolled error at %d ms!\n\r", timestamp_ms);
#endif
return Status;
}
/*
* Clear the IIC Repeated Start option.
*/
Status = XIicPs_ClearOptions(IicPs, XIICPS_REP_START_OPTION);
if (Status != XST_SUCCESS) {
return Status;
}
//myprintf("[iic_read2] 0x%02X(0x%02X)=0x%X\n\r", Address, Register, *Data);
return XST_SUCCESS;
}
/**
*
* This function simply initializes the iic component of the board
*
*******************************************************************************/
int IicInit()
{
xil_printf("Initialize iic component\r\n");
int Status;
XIicPs_Config *Config;
int Index;
/*
* Initialize the IIC driver so that it's ready to use
* Look up the configuration in the config table,
* then initialize it.
*/
Config = XIicPs_LookupConfig(IIC_DEVICE_ID);
if (NULL == Config)
{
return XST_FAILURE;
}
Status = XIicPs_CfgInitialize(&Iic, Config, Config->BaseAddress);
if (Status != XST_SUCCESS)
{
return XST_FAILURE;
}
/*
* Perform a self-test to ensure that the hardware was built correctly.
*/
Status = XIicPs_SelfTest(&Iic);
if (Status != XST_SUCCESS)
{
return XST_FAILURE;
}
/*
* Set the IIC serial clock rate.
*/
XIicPs_SetSClk(&Iic, IIC_SCLK_RATE);
/*
* Initialize the send buffer bytes with a pattern to send and the
* the receive buffer bytes to zero to allow the receive data to be
* verified.
*/
for (Index = 0; Index < RECV_BUFFER_SIZE; Index++)
{
RecvBuffer[Index] = 0;
}
/*
* Wait until bus is idle to start another transfer.
*/
while (XIicPs_BusIsBusy(&Iic)) {
/* NOP */
}
// Wait until slave is ready to communicate
Status = XIicPs_MasterRecvPolled(&Iic, RecvBuffer, RECV_BUFFER_SIZE, IIC_SLAVE_ADDR);
while (Status != XST_SUCCESS)
{
Status = XIicPs_MasterRecvPolled(&Iic, RecvBuffer, RECV_BUFFER_SIZE, IIC_SLAVE_ADDR);
usleep(100000);
}
xil_printf("iic component initialized and ready to request data!\r\n");
return XST_SUCCESS;
}
/**
* Returns a structure containing the position, heading and data read from sensors
* of the robot. The structure contains the parsed data requested by IIC from the
* slave board.
*/
robot_state IicGetRobotState()
{
int Status, Index;
// phase used for parsing the input taken from iic buffer
// 0 - positionX
// 1 - positionY
// 2 - headingAngle
// 3 - leftSensorX
// 4 - leftSensorY
// 5 - frontSensorX
// 6 - frontSensorY
// 7 - rightSensorX
// 8 - rightSensorY
int Phase = 0;
int Sign = 1;
robot_state Result = IicGetInvalidRobotState();
while (XIicPs_BusIsBusy(&Iic)) {
/* NOP */
}
Status = XIicPs_MasterRecvPolled(&Iic, RecvBuffer, RECV_BUFFER_SIZE, IIC_SLAVE_ADDR);
if (Status != XST_SUCCESS) {
// failed at this point..
// do something about it
return Result;
}
/*
* Verify received data is correct.
*/
char CurrentChar;
int CurrentNumber = 0;
int StoreData = 0;
for (Index = 0; Index < RECV_BUFFER_SIZE; Index++)
{
StoreData = 0;
CurrentChar = RecvBuffer[Index];
switch (CurrentChar)
{
case '-':
Sign = -1;
break;
case ',':
CurrentNumber *= Sign;
StoreData = 1;
break;
default:
CurrentNumber = CurrentNumber * 10 + (CurrentChar - '0');
break;
}
if (StoreData == 1)
{
switch(Phase)
{
case 0:
Result.numberOfCommands = CurrentNumber;
break;
case 1:
Result.vacuumStatus = CurrentNumber;
break;
case 2:
Result.sensorsServoStatus = CurrentNumber;
break;
case 3:
Result.position.x = CurrentNumber;
break;
case 4:
Result.position.y = CurrentNumber;
break;
case 5:
Result.headingAngle = CurrentNumber;
break;
case 6:
Result.leftSensor.x = CurrentNumber;
break;
case 7:
Result.leftSensor.y = CurrentNumber;
break;
case 8:
Result.frontSensor.x = CurrentNumber;
break;
case 9:
Result.frontSensor.y = CurrentNumber;
break;
case 10:
Result.rightSensor.x = CurrentNumber;
break;
case 11:
Result.rightSensor.y = CurrentNumber;
return Result;
break;
default:
break;
}
CurrentNumber = 0;
Phase ++;
Sign = 1;
}
}
return Result;
}
