RBAPI(bool) i2c_Init2(unsigned baseaddr, unsigned devs, int i2c0irq, int i2c1irq) { int i; if (I2C_ioSection != -1) { err_SetMsg(ERROR_I2C_INUSE, "I2C lib was already opened"); return false; } if ((I2C_ioSection = io_Init()) == -1) return false; //NOTE: base address should be selected carefully to avoid conflicts with other devices! if (baseaddr != 0xffff) i2c_SetBaseAddress(baseaddr); else { if (i2c_SetDefaultBaseAddress() == 0x0000) i2c_SetBaseAddress(0xfb00); } #ifdef ROBOIO switch (roboio_GetRBVer()) { case RB_100b1: case RB_100b2: case RB_100b3: case RB_100: case RB_100RD: case RB_110: case RB_050: devs = devs & I2C_USEMODULE0; i2c1irq = I2CIRQ_DISABLE; break; default: devs = 0; i2c0irq = i2c1irq = I2CIRQ_DISABLE; break; } #endif i2c_SetIRQ(i2c0irq, i2c1irq); I2C_swMode[0] = I2C_swMode[1] = I2CSW_DISABLE; I2C_action[0] = I2C_action[1] = I2CACT_DISABLE; for (i=0; i<2; i++) { if ((i == 0) && ((devs & I2C_USEMODULE0) == 0)) continue; if ((i == 1) && ((devs & I2C_USEMODULE1) == 0)) continue; I2C_action[i] = I2CACT_IDLE; //switch GPIO/I2C pins into GPIO pins OLD_I2CGPIO3FLAG[i] = read_sb_reg(SB_IPFCTRL3_REG) & OLD_I2CGPIO3MASK[i]; //backup GPIO/I2C switch flag write_sb_reg(SB_IPFCTRL3_REG, read_sb_reg(SB_IPFCTRL3_REG) & (~OLD_I2CGPIO3MASK[i])); //send START & STOP signal to reset I2C devices OLD_I2CGPIO3DIR[i] = io_inpb(GPIO3_DIR) & (0x03 << (i*2+4)); //backup GPIO3 DIR OLD_I2CGPIO3VAL[i] = io_inpb(GPIO3_DATA) & (0x03 << (i*2+4)); //backup GPIO3 VAL //set_pins(i, 1, 1); delay_ms(1); //SCL = 1, SDA = 1; START //set_pins(i, 1, 0); delay_ms(1); //SCL = 1, SDA = 0 //set_pins(i, 0, 0); delay_ms(1); //SCL = 0, SDA = 0 //Note: if we send the above START, some I2C sensors, such as ADI ADXL345, may fail to respond //set_pins(i, 0, 0); delay_ms(1); //SCL = 0, SDA = 0; STOP //Note: if we perform the line above, some I2C sensors, such as MEMSIC MXC6202, may fail to respond //set_pins(i, 1, 0); delay_ms(1); //SCL = 1, SDA = 0 //set_pins(i, 1, 1); delay_ms(1); //SCL = 1, SDA = 1 //Note: the above lines work for all sensors we tested, but we disable them to avoid unexpected sensor behaviors if (i2c_Reset(i) == false) // assume the status of GPIO/I2C pins are GPIO "IN" or "OUT 1" { i2c_Close(); err_SetMsg(ERROR_I2C_INITFAIL, "can't reset the I2C modules"); return false; } i2c_DisableINT(i, I2CINT_ALL); i2c_ClearSTAT(i, I2CSTAT_ALL); //Remarks: for DX ver.2, we must disable the noise filter to ensure that 3.3Mbps works in high-speed mode i2c_DisableNoiseFilter(i); //i2c_EnableNoiseFilter(i); i2c_DisableStandardHSM(i); //SCL open-drain in high-speed mode //i2c_EnableStandardHSM(i); i2c_SetSpeed(i, I2CMODE_AUTO, 100000L); //default 100Kbps i2cslave_SetAddr(i, 0x7f); //set slave address 0x7F by default (change this if it collide with external I2C devices) i2cslave_EnableACK(i); //switch GPIO pins into I2C SCL,SDA pins //Remarks: Vortex86DX's H/W I2C has an issue here; if you call i2c_Reset() in case GPIO/SCL pin = GPIO out 0, // then, whenever you switch GPIO/SCL pin to SCL pin, the SCL pin always first send the 10 reset dummy clocks write_sb_reg(SB_IPFCTRL3_REG, read_sb_reg(SB_IPFCTRL3_REG) | OLD_I2CGPIO3MASK[i]); } return true; }
Compass_Sensor::~Compass_Sensor() { i2c_Close(); }
RBAPI(void) i2c_CloseSW(void) { i2c_Close(); }
void GetInput(void) { int c = 0, i; if(key[KEY_B]) { HOLD = ~(HOLD); ADJUST = 0.0; BODY_YAW = 0.0; BODY_PITCH = 0.0; BODY_ROLL = 0.0; POS_GAIN[0] = 0.0; POS_GAIN[1] = 0.0; POS_GAIN[2] = 0.0; if(HOLD != 0) { STATE = BALANCE; printf("enable balance mode !\n"); Prepare(); } else { printf("disable balance mode !\n"); i2c_Close(); } delay_ms(500); } if(key[KEY_1]) { BUF[0] = 150.0; BUF[1] = 0.0; BUF[2] = 2.0; BUF[3] = 0.0; BUF[4] = 2.0; BUF[5] = 0.0; BUF[6] = 2.0; BUF[7] = 25.0; BUF[8] = 25.0; BUF[9] = 25.0; BUF[10] = 20.0; BUF[11] = 20.0; BUF[12] = 30.0; BUF[13] = 5.0; BUF[14] = 5.0; SetVal(); SWING = 0; delay_ms(500); } else if(key[KEY_2]) { BUF[0] = 300.0; BUF[1] = 4.0; BUF[2] = 2.0; BUF[3] = 0.0; BUF[4] = 1.0; BUF[5] = 5.0; BUF[6] = 3.0; BUF[7] = 25.0; BUF[8] = 25.0; BUF[9] = 25.0; BUF[10] = 20.0; BUF[11] = 20.0; BUF[12] = 30.0; BUF[13] = 2.0; BUF[14] = 3.0; SetVal(); SWING = 0; delay_ms(500); } else if(key[KEY_3]) { BUF[0] = 150.0; BUF[1] = 4.0; BUF[2] = 2.0; BUF[3] = 0.0; BUF[4] = 10.0; BUF[5] = 8.0; BUF[6] = 6.0; BUF[7] = 25.0; BUF[8] = 25.0; BUF[9] = 25.0; BUF[10] = 20.0; BUF[11] = 20.0; BUF[12] = 30.0; BUF[13] = 1.0; BUF[14] = 1.0; SetVal(); SWING = 0; delay_ms(500); } else if(key[KEY_4]) { BUF[0] = 300.0; BUF[1] = 0.0; BUF[2] = 2.0; BUF[3] = 0.0; BUF[4] = 2.0; BUF[5] = 0.0; BUF[6] = 2.0; BUF[7] = 15.0; BUF[8] = 15.0; BUF[9] = 15.0; BUF[10] = 15.0; BUF[11] = 10.0; BUF[12] = 20.0; BUF[13] = 5.0; BUF[14] = 5.0; IDLEPLAYTIME = 300.0; SetVal(); SWING = 1; printf("swing!!\n"); delay_ms(500); } if(key[KEY_U]) { ADJUST = ADJUST + 0.2; printf("gain adjust:%f!!\n",ADJUST); } if(key[KEY_D]) { ADJUST = ADJUST - 0.2; printf("gain adjust:%f!!\n",ADJUST); } if(key[KEY_L]) { printf("play slow!!\n"); PLAYTIME = PLAYTIME + 50; printf("playtime:%ld\n",PLAYTIME); printf("starttime:"); for(i = 0; i < 6; i++) { STARTTIME[i] = BUF[i+1]*PLAYTIME; printf("%ld ",STARTTIME[i]); } printf("\n"); delay_ms(500); } if(key[KEY_F]) { printf("play fast!!\n"); PLAYTIME = PLAYTIME - 50; printf("playtime:%ld\n",PLAYTIME); printf("starttime:"); for(i = 0; i < 6; i++) { STARTTIME[i] = BUF[i+1]*PLAYTIME; printf("%ld ",STARTTIME[i]); } printf("\n"); delay_ms(500); } if(key[KEY_K]) { ADJUST = 0.0; PLAYTIME = (long)BUF[0]; for(i = 0; i < 6; i++) STARTTIME[i] = BUF[i+1]*PLAYTIME; BODY_YAW = 0.0; BODY_PITCH = 0.0; BODY_ROLL = 0.0; POS_GAIN[0] = 0.0; POS_GAIN[1] = 0.0; POS_GAIN[2] = 0.0; STATE = IDLE; } if(key[KEY_X] && key[KEY_PGUP]) { if(POS_GAIN[0] < 30.0) POS_GAIN[0] = POS_GAIN[0] + 0.2; printf("pos_x:%f\n",POS_GAIN[0]); } if(key[KEY_X] && key[KEY_PGDN]) { if(POS_GAIN[0] > -30.0) POS_GAIN[0] = POS_GAIN[0] - 0.2; printf("pos_x:%f\n",POS_GAIN[0]); } if(key[KEY_Y] && key[KEY_PGUP]) { if(POS_GAIN[1] < 25.0) POS_GAIN[1] = POS_GAIN[1] + 0.2; printf("pos_y:%f\n",POS_GAIN[1]); } if(key[KEY_Y] && key[KEY_PGDN]) { if(POS_GAIN[1] > -25.0) POS_GAIN[1] = POS_GAIN[1] - 0.2; printf("pos_y:%f\n",POS_GAIN[1]); } if(key[KEY_Z] && key[KEY_PGUP]) { if(POS_GAIN[2] < 32.0) POS_GAIN[2] = POS_GAIN[2] + 0.2; printf("pos_z:%f\n",POS_GAIN[2]); } if(key[KEY_Z] && key[KEY_PGDN]) { if(POS_GAIN[2] > -20.0) POS_GAIN[2] = POS_GAIN[2] - 0.2; printf("pos_z:%f\n",POS_GAIN[2]); } if(key[KEY_V] && key[KEY_PGUP]) { BODY_YAW = BODY_YAW + 0.5*DEG_TO_RAD ; printf("yaw:%f\n",BODY_YAW*RAD_TO_DEG); } if(key[KEY_V] && key[KEY_PGDN]) { BODY_YAW = BODY_YAW -0.5*DEG_TO_RAD; printf("yaw:%f\n",BODY_YAW*RAD_TO_DEG); } if(key[KEY_P] && key[KEY_PGUP]) { BODY_PITCH = BODY_PITCH + 0.2*DEG_TO_RAD ; printf("pitch:%f\n",BODY_PITCH*RAD_TO_DEG); } if(key[KEY_P] && key[KEY_PGDN]) { BODY_PITCH = BODY_PITCH - 0.2*DEG_TO_RAD ; printf("pitch:%f\n",BODY_PITCH*RAD_TO_DEG); } if(key[KEY_R] && key[KEY_PGUP]) { BODY_ROLL = BODY_ROLL + 0.2*DEG_TO_RAD ; printf("roll:%f\n",BODY_ROLL*RAD_TO_DEG); } if(key[KEY_R] && key[KEY_PGDN]) { BODY_ROLL = BODY_ROLL - 0.2*DEG_TO_RAD ; printf("roll:%f\n",BODY_ROLL*RAD_TO_DEG); } if(HOLD != 0) return; c = -key[KEY_UP]; c = c | -(key[KEY_LEFT]<<1); c = c | -(key[KEY_RIGHT]<<2); c = c | -(key[KEY_DOWN]<<3); c = c | -(key[KEY_SPACE]<<4); c = c | -(key[KEY_O]<<5); PRE_STATE = STATE; switch(c) { case 1: STATE = FORWARD; break; case 2: STATE = LEFTWARD; break; case 4: STATE = RIGHTWARD; break; case 8: STATE = BACKWARD; break; case 3: STATE = FLWARD; break; case 5: STATE = FRWARD; break; case 10: STATE = BLWARD; break; case 12: STATE = BRWARD; break; case 34: STATE = LCIRCLE; break; case 36: STATE = RCIRCLE; break; case 16: STATE = EXIT; break; default: STATE = IDLE; break; } if(PRE_STATE != STATE) Prepare(); }
int main() { unsigned long usedchannel = RCSERVO_USECHANNEL0 +RCSERVO_USECHANNEL1 +RCSERVO_USECHANNEL2 + RCSERVO_USECHANNEL4 +RCSERVO_USECHANNEL5 +RCSERVO_USECHANNEL6+ RCSERVO_USECHANNEL9 +RCSERVO_USECHANNEL10+RCSERVO_USECHANNEL11+ RCSERVO_USECHANNEL13+RCSERVO_USECHANNEL14+RCSERVO_USECHANNEL15+ RCSERVO_USECHANNEL16+RCSERVO_USECHANNEL17+RCSERVO_USECHANNEL18+ RCSERVO_USECHANNEL21+RCSERVO_USECHANNEL22+RCSERVO_USECHANNEL23; // for RB-100 if(allegro_init()) { printf("error:initialize allegro library failed!!\n"); return -1; } if(install_keyboard()) { printf("error:initialize keybaord failed!!\n"); return -1; } roboio_SetRBVer(RB_100); if(rcservo_SetServos(usedchannel, RCSERVO_DMP_RS0263) == false) { printf("Set servo fails!%s\n",roboio_GetErrMsg()); return -1; } if(rcservo_Initialize(usedchannel) == false) { printf("RC servo initialize fails!%s\n",roboio_GetErrMsg()); return -1; } rcservo_SetFPS(500); rcservo_EnterPlayMode_NOFB(NORMAL); SetVal(); PRE_STATE = STATE = BALANCE; ADJUST = 0.0; BODY_YAW = 0.0; BODY_PITCH = 0.0; BODY_ROLL = 0.0; POS_GAIN[0] = 0.0; POS_GAIN[1] = 0.0; POS_GAIN[2] = 0.0; Prepare(); while(STATE != EXIT) { GetInput(); switch(STATE) { case FORWARD: case BACKWARD: case RIGHTWARD: case LEFTWARD: case FLWARD: case FRWARD: case BLWARD: case BRWARD: case LCIRCLE: case RCIRCLE: PlayMotion(); break; case IDLE: Idle(); break; case BALANCE: Stable(); break; default: break; } } rcservo_Close(); i2c_Close(); remove_keyboard(); return 0; }
RBAPI(bool) i2c_Initialize2(unsigned devs, int i2c0irq, int i2c1irq) { int i; if (I2C_ioSection != -1) { err_SetMsg(ERROR_I2C_INUSE, "I2C lib was already opened"); return false; } if ((I2C_ioSection = io_Init()) == -1) return false; if (i2c_SetDefaultBaseAddress() == 0x0000) i2c_SetBaseAddress(0xfb00); i2c_SetIRQ(i2c0irq, i2c1irq); I2C_action[0] = I2C_action[1] = I2CACT_DISABLE; for (i=0; i<2; i++) { if ((i == 0) && ((devs & I2C_USEMODULE0) == 0)) continue; if ((i == 1) && ((devs & I2C_USEMODULE1) == 0)) continue; I2C_action[i] = I2CACT_IDLE; //switch GPIO/I2C pins into GPIO pins OLD_I2CGPIO3FLAG[i] = read_sb_reg(SB_IPFCTRL3_REG) & OLD_I2CGPIO3MASK[i]; //backup GPIO/I2C switch flag write_sb_reg(SB_IPFCTRL3_REG, read_sb_reg(SB_IPFCTRL3_REG) & (~OLD_I2CGPIO3MASK[i])); //send START & STOP signal to reset I2C devices OLD_I2CGPIO3DIR[i] = io_inpb(0x9b) & (0x03 << (i*2+4)); //backup GPIO3 DIR OLD_I2CGPIO3VAL[i] = io_inpb(0x7b) & (0x03 << (i*2+4)); //backup GPIO3 VAL //set_pins(i, 1, 1); delay_ms(1); //SCL = 1, SDA = 1; START //set_pins(i, 1, 0); delay_ms(1); //SCL = 1, SDA = 0 //set_pins(i, 0, 0); delay_ms(1); //SCL = 0, SDA = 0 //Note: if we send the above START, some I2C sensors, such as ADI ADXL345, may fail to respond //set_pins(i, 0, 0); delay_ms(1); //SCL = 0, SDA = 0; STOP //Note: if we perform the line above, some I2C sensors, such as MEMSIC MXC6202, may fail to respond set_pins(i, 1, 0); delay_ms(1); //SCL = 1, SDA = 0 set_pins(i, 1, 1); delay_ms(1); //SCL = 1, SDA = 1 if (i2c_Reset(i) == false) { i2c_Close(); err_SetMsg(ERROR_I2C_INITFAIL, "can't reset the I2C modules"); return false; } i2c_DisableINT(i, I2CINT_ALL); i2c_ClearSTAT(i, I2CSTAT_ALL); //Remarks: for DX ver.2, we must disable the noise filter to ensure that 3.3Mbps works in high-speed mode i2c_DisableNoiseFilter(i); //i2c_EnableNoiseFilter(i); i2c_DisableStandardHSM(i); //SCL open-drain in high-speed mode //i2c_EnableStandardHSM(i); i2c_SetSpeed(i, I2CMODE_AUTO, 100000L); //default 100Kbps i2cslave_SetAddr(i, 0x7f); //set slave address 0x7F by default (change this if it collide with external I2C devices) i2cslave_EnableACK(i); //switch GPIO pins into I2C SCL,SDA pins //Remarks: Vortex86DX's H/W I2C has an issue here; if you call i2c_Reset() in case GPIO/SCL pin = GPIO out 0, // then, whenever you switch GPIO/SCL pin to SCL pin, the SCL pin always first send the 10 reset dummy clocks write_sb_reg(SB_IPFCTRL3_REG, read_sb_reg(SB_IPFCTRL3_REG) | OLD_I2CGPIO3MASK[i]); } return true; }
Altimeter_Sensor::~Altimeter_Sensor() { i2c_Close(); }
/** * Perform continuous reading accesses on I2C buses. * @param[in] threadData data needed for I2C processing. */ static void tatl06_03I2C_Process(void *threadData) { DTH_I2C_PROCESS_DATA *i2cData ; int vl_error = 0; int fd = 0; int count = 1; char buf[1024]; i2cData = (DTH_I2C_PROCESS_DATA *) threadData ; memset(buf, 0, 1024); if (strcmp(i2cData->deviceName, I2C3_DEVICE_NAME) == 0) { /* on 8520 no I2C3. Need to check board type */ fd = open("/sys/devices/platform/ab8505-i2c.0/ab8500-usb.0/boot_time_device",O_RDWR); if (fd<0){ /* board 8500 */ count = 6; } else { /* board 8520 : just need to return OK*/ close (fd); while (i2cData->order == START) { i2cData->state = ACTIVATED ; i2cData->error = NO_ERROR ; usleep(1000); } i2cData->state = DEACTIVATED ; pthread_exit(NULL); } } buf[0] = i2cData->registerAddress ; /* register address to be read. */ i2cData->state = ACTIVATED ; fd = i2c_Open(i2cData->deviceName, O_RDWR); if (fd < 0) { i2cData->error = I2cOpenError ; i2cData->state = DEACTIVATED ; pthread_exit(NULL); } if (i2cData->order == START) printf("dev=%s slave_addr=%x reg_addr=%x count=%d\n", i2cData->deviceName, i2cData->deviceAddress, i2cData->registerAddress, count); while (i2cData->order == START) { vl_error = i2c_Read(fd, i2cData->deviceAddress, buf, count); if (vl_error < 0) { i2c_Close(fd); i2cData->error = I2cReadError ; i2cData->state = DEACTIVATED ; pthread_exit(NULL); } i2cData->error = NO_ERROR ; usleep(1000); } i2c_Close(fd); i2cData->error = NO_ERROR ; i2cData->state = DEACTIVATED ; pthread_exit(NULL); }