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);
}
Exemple #10
0
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);
}
Exemple #15
0
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, &currentCmd, 1);
  ulapi_mutex_give(&statusMutex);
}
Exemple #22
0
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;
  }