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