示例#1
0
/* The CANbus card must be initialized and called from a realtime thread.
 * The rt_thread is spun off from main() to handle the initial communications.
 */
void can_thd_function(void *thd)
{
   int err;

   /* Probe and initialize the robot actuators */
   err = InitializeSystem();
   if(err) {
      syslog(LOG_ERR, "InitializeSystem returned err = %d", err);
      exit(1);
   }
    
   /* Initialize and get a handle to the robot on the first bus */
   if (ignore_calibration)
      wam = OpenWAM("*../../wam.conf", 0);
   else
      wam = OpenWAM("../../wam.conf", 0);   
   if(!wam)
   {
      syslog(LOG_ERR, "OpenWAM failed");
      exit(1);
   }
   
   /* setSafetyLimits(bus, joint rad/s, tip m/s, elbow m/s);
    * For now, the joint and tip velocities are ignored and
    * the elbow velocity provided is used for all three limits.
    */
   setSafetyLimits(0, 1.5, 1.5, 1.5);  // Limit to 1.5 m/s

   /* Set the puck torque safety limits (TL1 = Warning, TL2 = Critical).
    * Note: The pucks are limited internally to 3441 (see 'MT' in btsystem.c) 
    * Note: btsystem.c bounds the outbound torque to 8191, so entering a
    * value of 9000 for TL2 would tell the safety system to never register a 
    * critical fault.
    */
   setProperty(0, SAFETY_MODULE, TL2, FALSE, 4700);
   setProperty(0, SAFETY_MODULE, TL1, FALSE, 1800);
   
   /* Notify main() thread that the initialization is complete */
   startDone = 1;
   
   /* Spin until we are told to exit */
   while (!btrt_thread_done((btrt_thread_struct*)thd))
      usleep(10000);
   
   /* Close the system */
   CloseSystem();
   
   /* Remove this thread from the realtime scheduler */
   btrt_thread_exit((btrt_thread_struct*)thd);
}
示例#2
0
/* A new rt thread which just gets the encoder positions */
void magenc_thd_function(void *thd)
{
   int i;
   
   /* Detect puck versions */
   for (i=0; i<wam->dof; i++)
      mz_mechset[i] = (GetProp(i,VERS) >= 118 && GetProp(i,ROLE) == 256) ? 1 : 0;
   
   while (!btrt_thread_done((btrt_thread_struct*)thd))
   {
      if (mz_magvals_set)
      {
         for (i=0; i<wam->dof; i++) if (mz_mechset[i])
         {
            mz_magvals[i] = (int)GetProp(i,MECH);
         }
         mz_magvals_set = 0;
      }
      usleep(100000);
   }
}