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);
}
