int mountFileSystem(char *FileName)
{
FILE *filePointer;
int i, status, type;
filePointer = fopen(FileName, "r");
if (filePointer == NULL)
return -1;
fread(&SB, sizeof(SuperBlock), 1, filePointer);
for (i = 0; i < NO_OF_DIR; i++)
fread(&AllDir[i], sizeof(DIR), 1, filePointer);
for (i = 0; i < NO_OF_DIR; i++)
fread(&DirMap[i], sizeof(DirMapEntry), 1, filePointer);
for (i = 0; i < MAX_FILES; i++) {
char flush[5];
fscanf(filePointer, "%d%u%d%ld%ld%ld", &type,
&DILB[i].file_size,
&status,
&DILB[i].last_modified,
&DILB[i].last_accessed,
&DILB[i].inode_modified);
if (type == 0)
DILB[i].type = Free;
if (type == 2)
DILB[i].type = RegularFile;
if (type == 1)
DILB[i].type = Directory;
if (status == 0)
DILB[i].status = Unlocked;
if (status == 1)
DILB[i].status = Locked;
if (DILB[i].type == RegularFile)
DILB[i].data = (char *)malloc(sizeof(char) * DILB[i].file_size);
else if (DILB[i].type == Directory || DILB[i].type == Free)
DILB[i].data = (char *)malloc(sizeof(char) * 2);
fscanf(filePointer, "%[\nA-Za-z0-9 ., \\\"\'_+?/!@#$%&* ()_+; ]", DILB[i].data);
fscanf(filePointer, "%s", flush);
}
initializeFT();
initializeUArea();
fclose(filePointer);
return 0;
}
int WG06::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
{
if ( ((fw_major_ == 1) && (fw_minor_ >= 1)) || (fw_major_ >= 2) )
{
app_ram_status_ = APP_RAM_PRESENT;
}
int retval = WG0X::initialize(hw, allow_unprogrammed);
if (!retval && use_ros_)
{
bool poor_measured_motor_voltage = false;
double max_pwm_ratio = double(0x2700) / double(PWM_MAX);
double board_resistance = 5.0;
if (!WG0X::initializeMotorModel(hw, "WG006", max_pwm_ratio, board_resistance, poor_measured_motor_voltage))
{
ROS_FATAL("Initializing motor trace failed");
sleep(1); // wait for ros to flush rosconsole output
return -1;
}
// For some versions of software pressure and force/torque sensors can be selectively enabled / disabled
ros::NodeHandle nh(string("~/") + actuator_.name_);
bool pressure_enabled_ ;
if (!nh.getParam("enable_pressure_sensor", enable_pressure_sensor_))
{
pressure_enabled_ = true; //default to to true
}
if (!nh.getParam("enable_ft_sensor", enable_ft_sensor_))
{
enable_ft_sensor_ = false; //default to to false
}
if (enable_ft_sensor_ && (fw_major_ < 2))
{
ROS_WARN("Gripper firmware version %d does not support enabling force/torque sensor", fw_major_);
enable_ft_sensor_ = false;
}
// FW version 2+ supports selectively enabling/disabling pressure and F/T sensor
if (fw_major_ >= 2)
{
static const uint8_t PRESSURE_ENABLE_FLAG = 0x1;
static const uint8_t FT_ENABLE_FLAG = 0x2;
static const unsigned PRESSURE_FT_ENABLE_ADDR = 0xAA;
uint8_t pressure_ft_enable = 0;
if (enable_pressure_sensor_) pressure_ft_enable |= PRESSURE_ENABLE_FLAG;
if (enable_ft_sensor_) pressure_ft_enable |= FT_ENABLE_FLAG;
EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
if (writeMailbox(&com, PRESSURE_FT_ENABLE_ADDR, &pressure_ft_enable, 1) != 0)
{
ROS_FATAL("Could not enable/disable pressure and force/torque sensors");
return -1;
}
}
if (!initializePressure(hw))
{
return -1;
}
// Publish accelerometer data as a ROS topic, if firmware is recent enough
if (fw_major_ >= 1)
{
if (!initializeAccel(hw))
{
return -1;
}
}
// FW version 2 supports Force/Torque sensor.
// Provide Force/Torque data to controllers as an AnalogIn vector
if ((fw_major_ >= 2) && (enable_ft_sensor_))
{
if (!initializeFT(hw))
{
return -1;
}
}
// FW version 2 and 3 uses soft-processors to control certain peripherals.
// Allow the firmware on these soft-processors to be read/write through ROS service calls
if ((fw_major_ >= 2) && enable_soft_processor_access_)
{
if (!initializeSoftProcessor())
{
return -1;
}
}
}
return retval;
}
