void run_translatorI2C() {
unsigned char mod, reg, val;
uart_send_static_text("\f\a"
"WELCOME to the RS232 - I2C translator on e-Puck\r\n"
" 1 byte protocol ASL-EPFL 2006\r\n");
e_i2cp_init();
while (1) {
e_getchar_uart1(&mod); // #module
mod=(unsigned char) (mod<<1);
e_getchar_uart1(®); // #register
if (reg>127) { // read
e_i2cp_enable();
val= e_i2cp_read(mod, reg&0x7f); // read I2C
e_i2cp_disable();
e_send_uart1_char(&val, 1);
} else { // write
e_getchar_uart1(&val); // #value
e_i2cp_enable();
e_i2cp_write(mod, reg, val); // write I2C
e_i2cp_disable();
}
}
}
/*!\brief Configure and turn on the device.
*/
void initAccAndGyro(void) {
e_i2cp_enable();
// REG5_A (0x20) = ODR (4) | BDU (1) | Axes enabling (3)
// 0 1 1 0 | 1 | 1 1 1 => 0x6F => 100 Hz | BDU | x, y, z axes enabled
// 0 1 1 1 | 1 | 1 1 1 => 0x7F => 400 Hz | BDU | x, y, z axes enabled
// 1 0 0 0 | 1 | 1 1 1 => 0x8F => 800 Hz | BDU | x, y, z axes enabled
// 1 0 0 1 | 1 | 1 1 1 => 0x9F => 1600 Hz | BDU | x, y, z axes enabled
e_i2cp_write(ACC_ADDR, 0x20, 0x9F);
// REG7_A (0x25) = 0 1 (enable fifo, needed?) 0 1 (auto increment address on multi read) 0 0 0 0 => 0x50
e_i2cp_write(ACC_ADDR, 0x25, 0x50);
// REG6_A (0x24) => default +- 2g
// FIFO_CTRL_REG_A (0x2E) => default bypass mode
// CTRL_REG1_G (0x20) = ODR and cut-off (4) | mode (1) | Axes enabling (3)
// 0 0 1 1 | 1 | 1 1 1 => 0x3F => odr=95Hz, cut-off=25Hz | normal | x, y, z axes enabled
// 1 1 0 0 | 1 | 1 1 1 => 0xCF => odr=760Hz, cut-off=30Hz | normal | x, y, z axes enabled
e_i2cp_write(GYRO_ADDR, 0x20, 0xCF);
// CTRL_REG2_G (0x21) => normal mode; HPF=51.4 Hz (not used anyway)
e_i2cp_write(GYRO_ADDR, 0x21, 0x20);
// CTRL_REG4_G (0x23) => 250 dps (degrees per second) and continuous update
e_i2cp_write(GYRO_ADDR, 0x23, 0x00);
// CTRL_REG5_G (0x24) => enable fifo (needed?)
//e_i2cp_write(GYRO_ADDR, 0x24, 0x40);
//e_i2cp_write(GYRO_ADDR, 0x24, 0x50); // LPF1
//e_i2cp_write(GYRO_ADDR, 0x24, 0x51); // LPF1 + HPF
//e_i2cp_write(GYRO_ADDR, 0x24, 0x42); // LPF1 + LPF2
//e_i2cp_write(GYRO_ADDR, 0x24, 0x52); // LPF1 + HPF + LPF2
e_i2cp_disable();
}
// start ranging, wait & return the distance in cm
unsigned int e_get_dist_devantech(char device_add)
{
unsigned int dist;
unsigned char byte1, byte0;
unsigned char revision =255;
e_i2cp_write (device_add, 0, 81); // start ranging comand - cm
while ((revision==255)) // Cecking for completion of ranging (according datasheet)
{
revision = e_i2cp_read(device_add, 0);
}
byte1=e_i2cp_read(device_add, 2);
byte0=e_i2cp_read(device_add, 3);
dist=byte1;
dist=(dist<<8)+byte0;
return dist;
}
/*!\brief Set the register reg to value
* \param addr The device address
* \param reg The register address
* \param value The value to write
*/
void writeReg(unsigned char addr, unsigned char reg, unsigned char value) {
e_i2cp_enable();
e_i2cp_write(addr, reg, value);
e_i2cp_disable();
}
void WriteRegister(unsigned char registeraddr, unsigned char value) {
e_i2cp_write(COM_MODULE_I2C_ADDR, registeraddr, value);
}
static void el_cam_register_write_uint8(el_uint8 address,el_uint8 b){
e_i2cp_write(EL_CAM_I2C_ID,address,b);
}
