double CRCLStatus::getCycleTime()
{
double cycleTimeOut;
ulapi_mutex_take(&statusMutex);
cycleTimeOut = cycleTime;
ulapi_mutex_give(&statusMutex);
return cycleTimeOut;
}
static void emove_client_write_task_code(emove_client_write_task_args *args)
{
ulapi_task_struct *task;
int id;
int period_nsecs;
enum {OUTBUF_SIZE = 1024};
char outbuf[OUTBUF_SIZE];
int nchars;
nist_kitting::emove_stat emove_stat;
task = args->task;
id = args->id;
period_nsecs = args->period_nsecs;
free(args);
while (true) {
ros::spinOnce();
ulapi_mutex_take(&emove_stat_mutex);
emove_stat = emove_stat_buf;
ulapi_mutex_give(&emove_stat_mutex);
/*
uint8 type
uint8 serial_number
uint8 state
uint8 status
uint8 heartbeat
float32 period
float32 cycle
float32 duration
extern char *kitting_cmd_to_string(int s);
extern char *rcs_state_to_string(int s);
extern char *rcs_status_to_string(int s);
*/
ulapi_snprintf(outbuf, sizeof(outbuf), "%s %d %s %s %d %s %s %s\n",
kitting_cmd_to_string(emove_stat.stat.type),
emove_stat.stat.serial_number,
rcs_state_to_string(emove_stat.stat.state),
rcs_status_to_string(emove_stat.stat.status),
emove_stat.stat.heartbeat,
emove_stat.name.c_str(), emove_stat.line.c_str(), emove_stat.crcl.c_str());
outbuf[sizeof(outbuf)-1] = 0;
nchars = ulapi_socket_write(id, outbuf, strlen(outbuf));
if (nchars <= 0) break;
ulapi_wait(period_nsecs);
}
ulapi_socket_close(id);
if (debug) printf("emove client write handler %d done\n", id);
ulapi_task_delete(task);
return;
}
double CRCLStatus::getMaxAccel(MovementType mType)
{
double retValue;
ulapi_mutex_take(&statusMutex);
retValue = maxAccel[mType];
ulapi_mutex_give(&statusMutex);
return retValue;
}
GripperStatus CRCLStatus::getGripperStatus()
{
GripperStatus retValue;
ulapi_mutex_take(&statusMutex);
retValue = gripStatus;
ulapi_mutex_give(&statusMutex);
return retValue;
}
CRCLState CRCLStatus::getCurrentState()
{
CRCLState retValue;
ulapi_mutex_take(&statusMutex);
retValue = currentState;
ulapi_mutex_give(&statusMutex);
return retValue;
}
CRCLCmdUnion CRCLStatus::getCurrentCmd()
{
CRCLCmdUnion cmd;
ulapi_mutex_take(&statusMutex);
cmd = currentCmd;
ulapi_mutex_give(&statusMutex);
return cmd;
}
RobotStatus CRCLStatus::getRobotStatus()
{
RobotStatus retValue;
ulapi_mutex_take(&statusMutex);
retValue = robotStatus;
ulapi_mutex_give(&statusMutex);
return retValue;
}
CRCLCmdStatus CRCLStatus::getCurrentStatus()
{
CRCLCmdStatus retValue;
ulapi_mutex_take(&statusMutex);
retValue = currentCmd.status;
ulapi_mutex_give(&statusMutex);
return retValue;
}
CRCLStatus::CRCLStatus()
{
ulapi_mutex_init(&statusMutex, 1);
ulapi_mutex_give(&statusMutex);
ulapi_mutex_take(&statusMutex);
currentCmd.cmd = CRCL_NOOP;
currentCmd.status = CRCL_DONE;
currentState = CRCL_UNINITIALIZED;
cycleTime = DEFAULT_LOOP_CYCLE;
maxAccel[MOVE_DEFAULT] = 0;
maxVel[MOVE_DEFAULT] = 0;
maxAccel[MOVE_CARTESIAN] = KUKA_DEFAULT_CART_MAX_ACCEL;
maxVel[MOVE_CARTESIAN] = KUKA_DEFAULT_CART_MAX_VEL;
maxAccel[MOVE_JOINT] = KUKA_DEFAULT_JOINT_MAX_ACCEL;
maxVel[MOVE_JOINT] = KUKA_DEFAULT_JOINT_MAX_VEL;
ulapi_mutex_give(&statusMutex);
}
static void state_connection_thread_code(state_connection_thread_args *args)
{
void *thread;
int id;
double period;
joint_traj_pt_state_message jsmsg;
robot_status_message rsmsg;
object_state_message obj_state(3);
enum {OUTBUF_LEN = 1024};
char outbuf[OUTBUF_LEN];
int nchars, len;
int which = 0;
thread = args->thread;
id = args->id;
period = args->period;
free(args);
for (int t = 0; t < obj_state.object_number(); t++) {
obj_state.objects[t].id = t + 1;
obj_state.objects[t].x = t + 1.2;
obj_state.objects[t].y = t - 3.4;
obj_state.objects[t].z = t + 5.6;
obj_state.objects[t].qx = 0;
obj_state.objects[t].qy = 0;
obj_state.objects[t].qz = sin(0.1*t);
obj_state.objects[t].qw = cos(0.1*t);
}
while (true) {
float p;
int i;
switch (which) {
case 0:
ulapi_mutex_take(&robot_mutex);
for (int t = 0; t < JOINT_MAX; t++) {
if (the_robot.get_robot_joint_pos(&p, t)) jsmsg.set_pos(p, t);
}
ulapi_mutex_give(&robot_mutex);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&jsmsg), sizeof(jsmsg));
if (nchars < 0) break;
break;
case 1:
ulapi_mutex_take(&robot_mutex);
the_robot.get_drives_powered(&i);
rsmsg.set_drives_powered(i);
the_robot.get_e_stopped(&i);
rsmsg.set_e_stopped(i);
the_robot.get_in_error(&i);
rsmsg.set_in_error(i);
the_robot.get_in_motion(&i);
rsmsg.set_in_motion(i);
the_robot.get_mode(&i);
rsmsg.set_mode(i);
the_robot.get_motion_possible(&i);
rsmsg.set_motion_possible(i);
ulapi_mutex_give(&robot_mutex);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&rsmsg), sizeof(rsmsg));
if (nchars < 0) break;
break;
case 2:
#undef DO_OBJECTS
#ifdef DO_OBJECTS
len = obj_state.size();
if (len <= sizeof(outbuf)) {
obj_state.write_object_state(outbuf, sizeof(outbuf));
nchars = ulapi_socket_write(id, outbuf, len);
}
#endif // DO_OBJECTS
break;
} // switch (which)
if (++which > 2) which = 0;
ulapi_wait(period * 1e9);
} // while (true)
ulapi_socket_close(id);
if (debug) printf("joint state connection handler %d done\n", id);
free(thread);
return;
}
void CRCLStatus::setRobotStatus(RobotStatus status)
{
ulapi_mutex_take(&statusMutex);
robotStatus = status;
ulapi_mutex_give(&statusMutex);
}
void CRCLStatus::setMaxVel(double VelIn, MovementType mType)
{
ulapi_mutex_take(&statusMutex);
maxVel[mType] = VelIn;
ulapi_mutex_give(&statusMutex);
}
void CRCLStatus::setGripperStatus(GripperStatus status)
{
ulapi_mutex_take(&statusMutex);
gripStatus = status;
ulapi_mutex_give(&statusMutex);
}
void CRCLStatus::setCurrentStatus(CRCLCmdStatus status)
{
ulapi_mutex_take(&statusMutex);
currentCmd.status = status;
ulapi_mutex_give(&statusMutex);
}
int main(int argc, char *argv[])
{
int option;
int ival;
double dval;
int message_port = MESSAGE_PORT_DEFAULT;
int state_port = STATE_PORT_DEFAULT;
int request_server_id;
int state_server_id;
ulapi_task_struct request_server_thread;
ulapi_task_struct state_server_thread;
request_server_thread_args request_server_args;
state_server_thread_args state_server_args;
double period = 0.2; // ROS recommends 2 Hz at least
enum {INBUF_LEN = 1024};
char inbuf[INBUF_LEN];
opterr = 0;
while (true) {
option = getopt(argc, argv, ":m:s:t:d?");
if (option == -1) break;
switch (option) {
case 'm':
ival = atoi(optarg);
message_port = ival;
break;
case 's':
ival = atoi(optarg);
state_port = ival;
break;
case 't':
dval = atof(optarg);
period = dval;
break;
case 'd':
debug = true;
break;
case '?':
print_help();
return 0;
break;
case ':':
fprintf(stderr, "missing value for -%c\n", optopt);
return 1;
break;
default:
fprintf (stderr, "unrecognized option -%c\n", optopt);
return 1;
break;
} // switch (option)
} // while (true) for getopt
ulapi_mutex_init(&robot_mutex, 1);
the_robot.print_robot_status();
request_server_id = ulapi_socket_get_server_id(message_port);
if (request_server_id < 0) {
fprintf(stderr, "can't serve port %d\n", message_port);
return 1;
}
ulapi_task_init(&request_server_thread);
request_server_args.id = request_server_id;
ulapi_task_start(&request_server_thread, reinterpret_cast<ulapi_task_code>(request_server_thread_code), reinterpret_cast<void *>(&request_server_args), ulapi_prio_highest(), 0);
state_server_id = ulapi_socket_get_server_id(state_port);
if (state_server_id < 0) {
fprintf(stderr, "can't serve port %d\n", state_port);
return 1;
}
ulapi_task_init(&state_server_thread);
state_server_args.id = state_server_id;
state_server_args.period = period;
ulapi_task_start(&state_server_thread, reinterpret_cast<ulapi_task_code>(state_server_thread_code), reinterpret_cast<void *>(&state_server_args), ulapi_prio_highest(), 0);
bool done = false;
bool print_robot_info = true;
while (! done) {
char *ptr;
char *endptr;
int ival;
float fval;
int ret;
int joint;
// print prompt
printf("> ");
fflush(stdout);
// get input line
if (NULL == fgets(inbuf, sizeof(inbuf), stdin)) break;
// skip leading whitespace
ptr = inbuf;
while (isspace(*ptr)) ptr++;
// strip off trailing whitespace
endptr = inbuf + strlen(inbuf);
while ((isspace(*endptr) || 0 == *endptr) && endptr >= ptr) *endptr-- = 0;
while (endptr >= ptr) *endptr = tolower(*endptr--);
// now 'ptr' is the stripped input string
do {
if (0 == *ptr) { // blank line, print state
ulapi_mutex_take(&robot_mutex);
if (print_robot_info) {
the_robot.print_robot_info();
} else {
the_robot.print_robot_status();
}
ulapi_mutex_give(&robot_mutex);
break;
}
if ('q' == *ptr) { // quit
done = true;
break;
}
if ('d' == *ptr) { // quit
debug = ! debug;
break;
}
if ('i' == *ptr) {
print_robot_info = true;
}
if ('s' == *ptr) {
print_robot_info = false;
}
if ('?' == *ptr) { // help
print_help();
break;
}
if (! strncmp(ptr, "set", strlen("set"))) {
ptr += strlen("set");
if (! isspace(*ptr)) {
printf("need a joint and value to set\n");
break;
}
ival = strtol(ptr, &endptr, 0);
if (endptr == ptr || isgraph(*endptr)) {
printf("need a joint to set\n");
break;
}
joint = ival;
ptr = endptr;
while (isspace(*ptr)) ptr++;
if (! strncmp(ptr, "pos", strlen("pos"))) {
ptr += strlen("pos");
if (! isspace(*ptr)) {
printf("missing position value\n");
break;
}
fval = strtod(ptr, &endptr);
if (endptr == ptr || isgraph(*endptr)) {
printf("bad position value\n");
break;
}
ulapi_mutex_take(&robot_mutex);
ret = the_robot.set_robot_joint_pos(fval, joint-1);
ulapi_mutex_give(&robot_mutex);
if (! ret) {
printf("can't set joint %d position to %f\n", joint, dval);
}
break;
}
if (! strncmp(ptr, "pmin", strlen("pmin"))) {
ptr += strlen("pmin");
if (! isspace(*ptr)) {
printf("missing min value\n");
break;
}
fval = strtod(ptr, &endptr);
if (endptr == ptr || isgraph(*endptr)) {
printf("bad min value\n");
break;
}
ulapi_mutex_take(&robot_mutex);
ret = the_robot.set_robot_joint_min(fval, joint-1);
ulapi_mutex_give(&robot_mutex);
if (! ret) {
printf("can't set joint %d min value to %f\n", joint, dval);
}
break;
}
if (! strncmp(ptr, "pmax", strlen("pmax"))) {
ptr += strlen("pmax");
if (! isspace(*ptr)) {
printf("missing max value\n");
break;
}
fval = strtod(ptr, &endptr);
if (endptr == ptr || isgraph(*endptr)) {
printf("bad max value\n");
break;
}
ulapi_mutex_take(&robot_mutex);
ret = the_robot.set_robot_joint_max(fval, joint-1);
ulapi_mutex_give(&robot_mutex);
if (! ret) {
printf("can't set joint %d max value to %f\n", joint, dval);
}
break;
}
printf("need 'pos', 'pmin', or 'pmax' to set\n");
break;
} // matches "set"
printf("?\n");
} while (false); // do ... wrapper
} // while (! done)
return 0;
}
rtapi_result rtapi_mutex_take(rtapi_mutex_struct *mutex)
{
return ulapi_mutex_take(mutex);
}
void client_code(void *args)
{
void *client_task;
ulapi_integer client_id;
client_db_struct *client_db_ptr;
ulapi_integer debug;
enum {BUFFERLEN = 256};
char inbuf[BUFFERLEN];
char outbuf[BUFFERLEN];
ulapi_integer nchars;
int number;
int lastnumber = 0;
client_task = ((client_args *) args)->client_task;
client_id = ((client_args *) args)->client_id;
client_db_ptr = ((client_args *) args)->client_db_ptr;
debug = ((client_args *) args)->debug;
free(args);
/*
The client thread asks for an update every second, blocks until
it gets a response, prints it, and loops again.
*/
for (;;) {
ulapi_mutex_take(client_db_ptr->mutex);
number = client_db_ptr->number;
ulapi_mutex_give(client_db_ptr->mutex);
if (number != lastnumber) {
ulapi_snprintf(outbuf, sizeof(outbuf), "write %d", number);
lastnumber = number;
} else {
ulapi_snprintf(outbuf, sizeof(outbuf), "read");
}
ulapi_socket_write(client_id, outbuf, strlen(outbuf)+1);
nchars = ulapi_socket_read(client_id, inbuf, sizeof(inbuf)-1);
if (-1 == nchars) {
if (debug) printf("connection closed\n");
break;
}
if (0 == nchars) {
if (debug) printf("end of file\n");
break;
}
inbuf[nchars] = 0;
/*
Parse and handle the message here as your application requires.
*/
printf("%s\n", inbuf);
ulapi_wait(1000000000);
}
ulapi_socket_close(client_id);
}
int main(int argc, char *argv[])
{
int option;
ulapi_integer port = SAMPLE_APP_DEFAULT_PORT;
enum {BUFFERLEN = 256};
char host[BUFFERLEN] = "localhost";
ulapi_integer debug = 0;
ulapi_integer client_id;
ulapi_task_struct *client_task;
client_args *client_args_ptr;
client_db_struct client_db;
char buffer[BUFFERLEN];
char *ptr;
int number;
ulapi_opterr = 0;
for (;;) {
option = ulapi_getopt(argc, argv, ":p:h:d");
if (option == -1)
break;
switch (option) {
case 'p':
port = atoi(ulapi_optarg);
break;
case 'h':
strncpy(host, ulapi_optarg, sizeof(host));
host[sizeof(host) - 1] = 0;
break;
case 'd':
debug = 1;
break;
case ':':
fprintf(stderr, "missing value for -%c\n", ulapi_optopt);
return 1;
break;
default: /* '?' */
fprintf(stderr, "unrecognized option -%c\n", ulapi_optopt);
return 1;
break;
}
}
if (ulapi_optind < argc) {
fprintf(stderr, "extra non-option characters: %s\n", argv[ulapi_optind]);
return 1;
}
if (ULAPI_OK != ulapi_init()) {
fprintf(stderr, "ulapi_init error\n");
return 1;
}
if (debug) ulapi_set_debug(ULAPI_DEBUG_ALL);
if (0 != sample_app_init()) {
fprintf(stderr, "can't init the sample app\n");
return 1;
}
client_id = ulapi_socket_get_client_id(port, host);
if (client_id < 0) {
fprintf(stderr, "can't connect to port %d\n", (int) port);
ulapi_exit();
return 1;
}
if (debug) {
printf("serving port %d\n", (int) port);
}
client_db.mutex = ulapi_mutex_new(0);
client_db.number = 0;
client_task = ulapi_task_new();
client_args_ptr = reinterpret_cast<client_args *>(malloc(sizeof(client_args)));
client_args_ptr->client_task = client_task;
client_args_ptr->client_id = client_id;
client_args_ptr->client_db_ptr = &client_db;
client_args_ptr->debug = debug;
ulapi_task_start(client_task, client_code, client_args_ptr, ulapi_prio_lowest(), 0);
/* enter application main loop */
while (!feof(stdin)) {
if (NULL == fgets(buffer, sizeof(buffer), stdin)) {
break;
}
ptr = buffer;
while (isspace(*ptr)) ptr++;
if ('q' == *ptr) break;
if (0 == *ptr) {
ulapi_mutex_take(client_db.mutex);
number = client_db.number;
ulapi_mutex_give(client_db.mutex);
printf("%d\n", number);
continue;
}
if (1 == sscanf(ptr, "%d", &number)) {
ulapi_mutex_take(client_db.mutex);
client_db.number = number;
ulapi_mutex_give(client_db.mutex);
continue;
}
}
ulapi_socket_close(client_id);
ulapi_exit();
sample_app_exit();
return 0;
}
static void emove_stat_callback(const nist_kitting::emove_stat::ConstPtr& msg)
{
ulapi_mutex_take(&emove_stat_mutex);
emove_stat_buf = *msg;
ulapi_mutex_give(&emove_stat_mutex);
}
static void ws_stat_callback(const nist_kitting::ws_stat::ConstPtr& msg)
{
ulapi_mutex_take(&ws_stat_mutex);
ws_stat_buf = *msg;
ulapi_mutex_give(&ws_stat_mutex);
}
void CRCLStatus::setCurrentCmd(CRCLCmdUnion *cmdIn)
{
ulapi_mutex_take(&statusMutex);
crclCmdUnionCopy(cmdIn, ¤tCmd, 1);
ulapi_mutex_give(&statusMutex);
}
static void request_connection_thread_code(request_connection_thread_args *args)
{
void *thread;
int id;
enum {INBUF_LEN = 1024};
char inbuf[INBUF_LEN];
enum {OUTBUF_LEN = 1024};
char outbuf[OUTBUF_LEN];
char *ptr;
int nchars;
int nleft;
int length;
int message_type;
ping_reply_message pingrep;
joint_traj_pt_request_message jtreq;
joint_traj_pt_reply_message jtrep;
joint_info jinfo;
cart_traj_pt_request_message ctreq;
cart_traj_pt_reply_message ctrep;
float f1, f2, f3, f4, f5, f6, f7;
thread = args->thread;
id = args->id;
free(args);
bool done = false;
while (! done) {
nchars = ulapi_socket_read(id, inbuf, sizeof(inbuf));
if (nchars <= 0) break;
inbuf[sizeof(inbuf)-1] = 0;
ptr = inbuf;
nleft = nchars;
while (nleft > 0) {
// get the length and type of the message
memcpy(&length, ptr, sizeof(length));
memcpy(&message_type, ptr + sizeof(length), sizeof(message_type));
// switch on the message type and handle it
switch (message_type) {
case MESSAGE_PING:
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&pingrep), sizeof(pingrep));
if (nchars < 0) done = true;
break;
case MESSAGE_JOINT_TRAJ_PT:
jtreq.read_joint_traj_pt_request(ptr);
if (debug) {
printf("connection %d requested:\n", id);
jtreq.print_joint_traj_pt_request();
}
ulapi_mutex_take(&robot_mutex);
for (int t = 0; t < JOINT_MAX; t++) {
if (! jtreq.get_pos(&f1, t)) break;
the_robot.set_robot_joint_pos(f1, t);
}
// FIXME -- testing with a delay, should thread this off
ulapi_sleep(3);
ulapi_mutex_give(&robot_mutex);
jtrep.set_joint_traj_pt_reply(REPLY_SUCCESS);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&jtrep), sizeof(jtrep));
if (debug) {
printf("replied to connection %d:\n", id);
jtrep.print_joint_traj_pt_reply();
}
if (nchars < 0) done = true;
break;
case MESSAGE_CART_TRAJ_PT:
ctreq.read_cart_traj_pt_request(ptr);
if (debug) {
printf("connection %d requested:\n", id);
ctreq.print_cart_traj_pt_request();
}
// FIXME -- ditto
ulapi_sleep(3);
ulapi_mutex_take(&robot_mutex);
if (! ctreq.get_pos(&f1, &f2, &f3, &f4, &f5, &f6, &f7)) break;
the_robot.set_robot_cart_pos(f1, f2, f3, f4, f5, f6, f7);
ulapi_mutex_give(&robot_mutex);
ctrep.set_seq_number(ctreq.get_seq_number());
ctrep.set_cart_traj_pt_reply(REPLY_SUCCESS);
nchars = ulapi_socket_write(id, reinterpret_cast<char *>(&ctrep), sizeof(ctrep));
if (debug) {
printf("replied to connection %d:\n", id);
ctrep.print_cart_traj_pt_reply();
}
if (nchars < 0) done = true;
break;
default:
// unknown message
if (debug) printf("unknown message type: %d\n", message_type);
break;
} // switch (message type)
nleft -= (sizeof(length) + length);
ptr += (sizeof(length) + length);
} // while (nleft)
} // while (true)
ulapi_socket_close(id);
if (debug) printf("simple message connection handler %d done\n", id);
free(thread);
return;
}
void CRCLStatus::setCurrentState(CRCLState state)
{
ulapi_mutex_take(&statusMutex);
currentState = state;
ulapi_mutex_give(&statusMutex);
}
LIBRARY_API CanonReturn CrpiRobotiq::SetParameter (const char *paramName, void *paramVal)
{
int *temp_int = (int*) paramVal;
//WaitForSingleObject (ka_.handle, INFINITE);
ulapi_mutex_take(ka_.handle);
if ((strcmp (paramName, "ACTIVATE") == 0))
{
iqGrip->setParameter(1, *temp_int);
}
else if (strcmp (paramName, "GRIP_TYPE") == 0)
{
iqGrip->setParameter(2, *temp_int);
}
else if (strcmp (paramName, "AUTO_CENTER") == 0)
{
iqGrip->setParameter(4, *temp_int);
}
else if (strcmp (paramName, "AUTO_RELEASE") == 0)
{
iqGrip->setParameter(5, *temp_int);
}
else if (strcmp (paramName, "ADVANCED_CONTROL") == 0)
{
iqGrip->setParameter(6, *temp_int);
}
else if (strcmp (paramName, "SCISSOR_CONTROL") == 0)
{
iqGrip->setParameter(7, *temp_int);
}
else if (strcmp (paramName, "POSITION_FINGER_A") == 0)
{
iqGrip->setPositionFingerA(*temp_int);
}
else if (strcmp (paramName, "SPEED_FINGER_A") == 0)
{
iqGrip->setSpeedFingerA(*temp_int);
}
else if (strcmp (paramName, "FORCE_FINGER_A") == 0)
{
iqGrip->setForceFingerA(*temp_int);
}
else if (strcmp (paramName, "POSITION_FINGER_B") == 0)
{
iqGrip->setPositionFingerB(*temp_int);
}
else if (strcmp (paramName, "SPEED_FINGER_B") == 0)
{
iqGrip->setSpeedFingerB(*temp_int);
}
else if (strcmp (paramName, "FORCE_FINGER_B") == 0)
{
iqGrip->setForceFingerB(*temp_int);
}
else if (strcmp (paramName, "POSITION_FINGER_C") == 0)
{
iqGrip->setPositionFingerC(*temp_int);
}
else if (strcmp (paramName, "SPEED_FINGER_C") == 0)
{
iqGrip->setSpeedFingerC(*temp_int);
}
else if (strcmp (paramName, "FORCE_FINGER_C") == 0)
{
iqGrip->setForceFingerC(*temp_int);
}
else if (strcmp (paramName, "POSITION_SCISSOR") == 0)
{
iqGrip->setPositionScissor(*temp_int);
}
else if (strcmp (paramName, "SPEED_SCISSOR") == 0)
{
iqGrip->setSpeedScissor(*temp_int);
}
else if (strcmp (paramName, "FORCE_SCISSOR") == 0)
{
iqGrip->setForceScissor(*temp_int);
}
else if (strcmp (paramName, "GRIP") == 0)
{
iqGrip->setGrip(*temp_int);
}
else if (strcmp (paramName, "STATUS") == 0)
{
iqGrip->getStatusRegisters();
}
else
{
return CANON_FAILURE;
}
//ReleaseMutex (ka_.handle);
ulapi_mutex_give(ka_.handle);
return CANON_SUCCESS;
}