unsigned int mem_Write(unsigned int addr,unsigned char *data, unsigned int data_len)
{
unsigned int i=0;
unsigned char b;
i2c_Start();
i2c_Write(MEM_ADDRESS);
if (i2c_Timeout()) goto Timeout;
b = addr>>8;
i2c_Write(b);
if (i2c_Timeout()) goto Timeout;
b = addr&0xFF;
i2c_Write(b);
if (i2c_Timeout()) goto Timeout;
for (i=0; i<data_len; i++)
{
i2c_Write(data[i]);
if (i2c_Timeout()) goto Timeout;
}
i2c_Stop();
return i;
Timeout:
return i;
}
void i2c_slave_command(uint8_t slave_address, uint8_t address, uint8_t command)
{
i2c_Start(); // send Start
i2c_Address(slave_address, I2C_WRITE); // Send slave address with write operation
i2c_Write(address); // Servo Speed
i2c_Write(command); // 2 seconds on servo 0
i2c_Stop(); // send Stop
}
void i2c_Start(char addr, char rw){
I2C2CONbits.ACKDT = 0;
int i=0;
for(i=0;i<1000;i++)
Nop();
I2C2CONbits.SEN = 1;
while(I2C2CONbits.SEN){};
i2c_Write(addr | rw);
}
//to-do write in comments what these do.
void init9axis(){
i2c_Init();
i2c_ResetBus();
i2c_Start(acc_i2c_addr,I2C_WRITE);
i2c_Write(0x20);
i2c_Write(0x2F);
i2c_ResetBus();
// i2c_Start(acc_i2c_addr,I2C_WRITE);
// i2c_Write(0x23);
// i2c_Write(0xC0);
// i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(0x00);
i2c_Write(0x08);
i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(0x01);
i2c_Write(0x60);
i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(0x02);
i2c_Write(0x00);
i2c_ResetBus();
i2c_Start(gyr_i2c_addr,I2C_WRITE);
i2c_Write(0x16);
i2c_Write(0x18);
i2c_ResetBus();
//Setup Timer4 to be used as a trigger for information collection
TMR4 =0;
T4CON=0;
T4CONbits.TCKPS = 0b11; // 1:8 for now
_T4IP = 0x01; // Set Timer4 Interrupt Priority Level
IFS1bits.T4IF = 0; // Clear Timer4 Interrupt Flag
IEC1bits.T4IE = 1; // Enable Timer4 interrupt
T4CONbits.TON = 1; // turn on T4
}
unsigned int mem_Read(unsigned int addr,unsigned char *data, unsigned int data_len)
{
unsigned int i=0;
unsigned char b;
i2c_Start();
i2c_Write(MEM_ADDRESS);
if (i2c_Timeout()) goto Timeout;
b = addr>>8;
i2c_Write(b);
if (i2c_Timeout()) goto Timeout;
b = addr&0xFF;
i2c_Write(b);
if (i2c_Timeout()) goto Timeout;
//i2c_Stop();
i2c_Start();
i2c_Write(MEM_ADDRESS | 1);
if (i2c_Timeout()) goto Timeout;
for (i=0; i<data_len; i++)
{
data[i] = i2c_Read();
clear_CPU_operation_detection();
if (i<data_len-1)
i2c_Ack();
else
i2c_Nack();
}
i2c_Stop();
return i;
Timeout:
return i;
}
// Read a char from servo ic
uint8_t i2c_slave_read(uint8_t slave_address, uint8_t address)
{
uint8_t read_byte;
// Read one byte (i.e. servo pos of one servo)
i2c_Start(); // send Start
i2c_Address(slave_address, I2C_WRITE); // Send slave address with write operation
i2c_Write(address); // Set register for servo 0
i2c_Restart(); // Restart
i2c_Address(slave_address, I2C_READ); // Send slave address with read operation
read_byte = i2c_Read(0); // Read one byte
// If more than one byte to be read, (0) should
// be on last byte only
// e.g.3 bytes= i2c_Read(1); i2c_Read(1); i2c_Read(0);
i2c_Stop(); // send Stop
return read_byte; // return byte. If reading more than one byte
// store in an array
}
void readMag(int magArr[3]){
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(mag_xH_addr);
i2c_Restart(mag_i2c_addr,I2C_READ);
unsigned int xH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(mag_xL_addr);
i2c_Restart(mag_i2c_addr,I2C_READ);
unsigned int xL = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(mag_yH_addr);
i2c_Restart(mag_i2c_addr,I2C_READ);
unsigned int yH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(mag_yL_addr);
i2c_Restart(mag_i2c_addr,I2C_READ);
unsigned int yL = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(mag_zH_addr);
i2c_Restart(mag_i2c_addr,I2C_READ);
unsigned int zH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(mag_i2c_addr,I2C_WRITE);
i2c_Write(mag_zL_addr);
i2c_Restart(mag_i2c_addr,I2C_READ);
unsigned int zL = i2c_ReadFinal();
i2c_ResetBus();
magArr[0] = (xH<<8) | xL;
magArr[1] = (yH<<8) | yL;
magArr[2] = (zH<<8) | zL;
}//end read mag
void readGyr(int gyrArr[6]){
i2c_Start(gyr_i2c_addr,I2C_WRITE);
i2c_Write(gyr_xH_addr);
i2c_Restart(gyr_i2c_addr,I2C_READ);
unsigned int xH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(gyr_i2c_addr,I2C_WRITE);
i2c_Write(gyr_xL_addr);
i2c_Restart(gyr_i2c_addr,I2C_READ);
unsigned int xL = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(gyr_i2c_addr,I2C_WRITE);
i2c_Write(gyr_yH_addr);
i2c_Restart(gyr_i2c_addr,I2C_READ);
unsigned int yH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(gyr_i2c_addr,I2C_WRITE);
i2c_Write(gyr_yL_addr);
i2c_Restart(gyr_i2c_addr,I2C_READ);
unsigned int yL = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(gyr_i2c_addr,I2C_WRITE);
i2c_Write(gyr_zH_addr);
i2c_Restart(gyr_i2c_addr,I2C_READ);
unsigned int zH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(gyr_i2c_addr,I2C_WRITE);
i2c_Write(gyr_zL_addr);
i2c_Restart(gyr_i2c_addr,I2C_READ);
unsigned int zL = i2c_ReadFinal();
i2c_ResetBus();
gyrArr[0] = (xH<<8) | xL;
gyrArr[1] = (yH<<8) | yL;
gyrArr[2] = (zH<<8) | zL;
}
void readAcc(int accArr[6]){
i2c_Start(acc_i2c_addr,I2C_WRITE);
i2c_Write(acc_xH_addr);
i2c_Restart(acc_i2c_addr,I2C_READ);
unsigned int xH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(acc_i2c_addr,I2C_WRITE);
i2c_Write(acc_xL_addr);
i2c_Restart(acc_i2c_addr,I2C_READ);
unsigned int xL = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(acc_i2c_addr,I2C_WRITE);
i2c_Write(acc_yH_addr);
i2c_Restart(acc_i2c_addr,I2C_READ);
unsigned int yH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(acc_i2c_addr,I2C_WRITE);
i2c_Write(acc_yL_addr);
i2c_Restart(acc_i2c_addr,I2C_READ);
unsigned int yL = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(acc_i2c_addr,I2C_WRITE);
i2c_Write(acc_zH_addr);
i2c_Restart(acc_i2c_addr,I2C_READ);
unsigned int zH = i2c_ReadFinal();
i2c_ResetBus();
i2c_Start(acc_i2c_addr,I2C_WRITE);
i2c_Write(acc_zL_addr);
i2c_Restart(acc_i2c_addr,I2C_READ);
unsigned int zL = i2c_ReadFinal();
i2c_ResetBus();
accArr[0] = (xH<<8) + xL;
accArr[1] = (yH<<8) + yL;
accArr[2] = (zH<<8) + zL;
}
void SetDAC1(unsigned int value) {
if ((value >= 0) && (value < 4096)) {
i2cdata[0] = value >> 8;
i2cdata[1] = value & 0x00ff;
i2c_Write(DAC1Address, true, i2cdata, 2);
}
