/*********************************************************************** Process: dex_driver/dex_driver.cpp Description: This process enables communication with the robot. The process handles all I/O interface and is the main servo loop rate generator. Notes: Handles : S2G, Fingertips, Init, Calib, Forces Input : do calib, do init, do force init, T Output: q, dq, link info, forces History: When Who What/Why __________|______|_______________ 02/17/00 wbg Start ********************************************************************/ //#define DEBUG_STATE //#define TOGGLE_DAC4 //------------------------ include files --------------------------- #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "dex_config.h" #include "servo2go_dex.h" #include "gather_forces.h" //----------------------- function prototypes --------------------- static void sigTermHandler( int ); static void dexDriverInitialization( int argc, const char * argv[]); static void setUpServo (); static void startADConversion(); static void startUpInit(); static int calibrateRoutine(); static int calibrateTactileRoutine(); static void zeroTorques(); static void outputCmdTorques(); static void readState(); static void computeForwardKinematics ( vecLftRht & xPos ); //----------------------- process global variables ---------------- static dexClientShm *dexDriver = NULL; static dexMatrixShm *dexMatrix = NULL; static dexControlShm *dexControl = NULL; static dexCommShm *dexComm = NULL; static dexObjectShm *dexObject = NULL; //added only so that it can be destoryed static adcStructShm *adc = NULL; const char *currentConfigFile = NULL; static servo2goDex s2go(0x300); DML_timer servoLoopTimer(FREQ1000Hz); // child isr process defs volatile unsigned dummy; static pid_t irq5pid; static pid_t isrProxy; #pragma off(check_stack); pid_t far irqHandler () { return (isrProxy); } #pragma on(check_stack); static DML_child isr("isr_process"); static DML_child control("dex_control"); static DML_child dexinterface("dex_interface"); static DML_child matrix("dex_matrix"); static DML_child tactile("dex_tactile"); static DML_child objest("dex_objest"); static DML_master master; static int imAChild = 0; //----------------------- main process ---------------------------- void main ( int argc, const char *argv[]) { int firstTime = 1; float toggleMe = 5.0; servoLoopTimer.stop(); //stop to prevent proxy buildup dexDriverInitialization(argc, argv); servoLoopTimer.restart(); //printf("Starting Servo Loop\n"); while(1) { Receive(servoLoopTimer, 0, 0); if (dexDriver->calibrateFlag()) { servoLoopTimer.stop(); calibrateRoutine(); servoLoopTimer.restart(); } // end calibrate if (dexDriver->calibrateTactileFlag()) { servoLoopTimer.stop(); calibrateTactileRoutine(); servoLoopTimer.restart(); } // end calibrate tactile if (dexDriver->encInitFlag()) { servoLoopTimer.stop(); startUpInit(); //run encoder set up // printf("%% dex_driver - starting children - matrix, tactile, objest\n"); // matrix.sendInfoMsg(); //start matrix child // tactile.sendInfoMsg(); // start the tactile code but wait for kick // objest.sendInfoMsg(); servoLoopTimer.restart(); } //end encInit if (dexDriver->sensorInitFlag()) { /* if (setUpForceSensor()) { printf("%% dex_driver - sensor initialization complete, starting interface\n"); dexDriver->clearSensorInitFlag(); //once the sensor is initialized then start dex_interface dexinterface.kick(); } */ servoLoopTimer.stop(); tactile.sendInfoMsg(); // start the tactile code DML_timer delay1(500, ONE_SHOT); //give the tactile a chance to print to screen delay1.wait(); delay1.~DML_timer(); printf("%% dex_driver - force sensor intializing in progress..."); readState(); // the sensor need to know where we are setUpForceSensor(); //assumes that the current torques remain applied to motors dexDriver->clearSensorInitFlag(); printf("%% dex_driver - starting children - matrix, tactile, objest\n"); matrix.sendInfoMsg(); //start matrix child objest.sendInfoMsg(); printf("%% dex_driver - sensor initialization complete, starting interface\n"); DML_timer delay2(1000, ONE_SHOT); delay2.wait(); delay2.~DML_timer(); servoLoopTimer.restart(); dexinterface.kick(); if (imAChild) { master.sendInfoMsg(); } } else { //update forces (previous) computeForces(); } // end sensorInit if (dexDriver->shutDownFlag()) { // printf("%% dex_driver - shutDownFlag called, invoking sigHandler\n"); // sigTermHandler(SIGTERM); } //-----> start force conversion startADConversion(); //-----> output torques outputCmdTorques(); //-----> read position readState(); //-----> update state - velocity calculation //-----> set dexdriver shmem q, qdot control.kick(); objest.kick(); #ifdef TOGGLE_DAC4 toggleMe < 0 ? toggleMe = 5.0 : toggleMe = -0.01; s2go.DAC(DAC_4, toggleMe); #endif //TOGGLE_DAC4 } // end while loop printf("%% dex_driver - kicked out of while loop, invoking sigHandler\n"); sigTermHandler(0); } // end main /*************************************************************** Fucntion: dexDriverInitialization Parameters: Returns: Notes: This function sets up the dexDriverRoutine. ***************************************************************/ static void dexDriverInitialization( int argc, const char * argv[]) { printf("%% dex_driver - initializing\n"); setprio (0, dexDriverPrio); signal(SIGTERM, sigTermHandler); dexDriver = openDexClientShm; dexMatrix = openDexMatrixShm; dexControl = openDexControlShm; dexComm = openDexCommShm; dexObject = openDexObjectShm; adc = openAdcStructShm; currentConfigFile = "dexter.cfg"; dexConfig dexInfo(currentConfigFile); dexInfo.readConfig(); printf("%% dex_driver - starting children - control, interface\n"); control.sendInfoMsg(); //start control child dexinterface.sendInfoMsg(); //start interface child, interface waits for initial kick so that the // prompt does not come up before the startup messages //with children started and interface awaiting a kick //fill the shared memory with data from the config file dexInfo.fillShmem(); // set the flags so that encoders and sensors are initialize in // the first run through of the main loop dexDriver->encInit(); dexDriver->sensorInit(); // set up servo and isr process Note: must be called after shmem is filled // because isr_process initialization needs channel and conversion data setUpServo(); // if dex_driver was invoke manually then the tick size must be set // otherwise it is setup in jedi // if dex_driver was started by teleteach then send message to teleteach // and let process know that dex_driver is ready if (argc > 1) { if ( strcmp (argv[1], "-s")==0 ) { long oldTickSize; oldTickSize = qnx_ticksize(1000000, _TICKSIZE_STANDARD); //adjust ticksize to 1ms if (oldTickSize == -1) { printf("****************************************************************************\n"); printf("**** dex_driver - error: unable to set ticksize in dex_driver, shutting down\n"); printf("****************************************************************************\n"); sigTermHandler(0); } // if self also set the desX position so that dexter does not accidently try // to go to zero dexComm->xDes.left.values (-0.085, -0.130); dexComm->xDes.right.values ( 0.085, -0.130); } } else { // master.sendInfoMsg(); imAChild = 1; } printf("%% dex_driver - initialization complete, starting servo loop\n"); } /*************************************************************** Fucntion: setUpServo Parameters: Returns: Notes: This function sets up the Servo-to-go card. Also sets up the child process and enables the EOC interrupt. First conversion is started ***************************************************************/ static void setUpServo ( void ) { printf("%% dex_driver - initializing servo card\n"); s2go.init(); s2go.setADCautoZero(0); isr.sendInfoMsg(); isrProxy = isr.proxy; if ( (irq5pid = qnx_hint_attach (5 , &irqHandler, FP_SEG(&dummy) ) ) == -1 ) { printf("**************************************************************\n"); printf("**** dex_driver - warning: unable to attach hardware interrupt\n"); printf("**************************************************************\n"); } s2go.enableInterrupt(S2G_EOC_INT); // note delay was added because with out it the initial force value is off // this is most likely due to the fact that you are doing a hardware write to the // EOC bit and then immediately using it by starting a conversion DML_timer delay(100, ONE_SHOT); delay.wait(); delay.~DML_timer(); //start the first conversion startADConversion(); } /*************************************************************** Fucntion: startADConversion Parameters: Returns: Notes: This function starts the analog to digital conversion for each cycle. The interrupt service routine takes care of the rest. ***************************************************************/ static void startADConversion (void ) { adc->channelCount = 0; s2go.startConversion(ADCport[adc->channelCount+dexDriver->adcChannelOffset]); } /*************************************************************** Fucntion: startUpInit Parameters: Returns: Notes: This fucntion initialized the links encoders. Basic process is as follows- --------> step up ramp timer --------> slowly increase torque --------> delete ramp timer --------> set encoder preset value --------> latch encoders --------> set control mode ***************************************************************/ static void startUpInit ( void ) { short count, finger, axis; static float initTorque [FINGERS][AXES]; printf("%% dex_driver - encoder intializing in progress..."); // set up a timer with 50ms period DML_timer rampTimer(FREQ20Hz); for (finger=0;finger < FINGERS;finger++) { for (axis=0; axis< AXES; axis ++) { initTorque[finger][axis] = 0.0; } } for (count = 0; count < 100; count++) // do this for 100 cycles i.e., 5 secs { // printf("waiting for timer in enc int loop\n"); Receive(rampTimer,0,0); //stay receive block until kicked by timer //exponential rise in torque for (finger=0;finger < FINGERS;finger++) { for (axis=0; axis< AXES; axis ++) { initTorque[finger][axis] = 0.95*initTorque[finger][axis] + 0.05*dexDriver->initTorqueMax[finger][axis]; } } if (!(count%5) ) { printf("."); fflush(stdout); } dexDriver->setTorques(initTorque); outputCmdTorques(); } printf("done! \n"); rampTimer.~DML_timer(); s2go.zeroEncoders(); //loads preset value into encoder // printf("q Init = %5.3f, %5.3f, %5.3f, %5.3f\n", dexDriver->qInit[0][0], dexDriver->qInit[0][1], dexDriver->qInit[1][0], dexDriver->qInit[1][1]); printf("\a"); } // startUpInit /*************************************************************** Fucntion: calibrateRoutine Parameters: Returns: Notes: This fucntion calibrates the links encoders. Basic process is as follows- --------> stop servo loop timer & force stuff --------> set current position to qInitCalibration --------> step up ramp timer --------> slowly increase torque --------> read new positon --------> currentTip.updateConfig(currentConfigFile); --------> kill program or set control mode ***************************************************************/ static int calibrateRoutine (void) { char yesno[10]; char answer[10]; int axisNumber[AXES]; float angle[AXES]; float newqInit[FINGERS][AXES]; int offset, finger, axis; int c; zeroTorques(); printf("Calibration Routine Called \n This routine will allow you recalibrate the robot if necessary.\n"); printf(" You must have the following items to complete the calibration routine\n"); printf(" - height guge\n"); printf(" - dial indicator\n"); printf(" - adjustable arm mount for dial indicator\n"); printf(" - level mounting block for the robot\n"); printf(" - 12\" square\n"); printf(" Do you want to perform this routine (yes/no)? "); scanf("%[^'\n']", &yesno); //scan in message until \n if (strstr(yesno,"no" )) { printf("Returning to main loop\n"); return(0); } yesno[0] = NULL; printf(" This process can only be performed one side at a time\n"); printf(" The following step will repeat for each side\n"); printf(" The first motor set to be calibrated will be 12 followed by 34\n"); for (finger=0; finger < FINGERS;finger ++) { offset = finger * 2; if (finger == LEFT) { printf("\nPerforming calibration for motor set 12 (left finger)\n"); } else { printf("\nPerforming calibration for motor set 34 (right finger)\n"); } printf(" Step 1: Place robot on leveling block\n"); printf(" Step 2: Move height gage to bottom of travel and set dial to zero\n"); printf(" Step 3: Set height gage to 8.4755\"\n"); printf(" Step 4: Move first link of motor set approximately level\n"); printf(" and hold in place using height gage\n"); printf(" Step 5: Make second link of motor set vertical using square\n"); printf(" Step 6: Press RETURN once you have completed the steps above\n"); fflush(stdin); c = getche(); s2go.zeroEncoders(); printf(" Step 7: Move second link to joint limit and then press RETURN\n"); printf(" Make sure that the first link remains level\n"); fflush(stdin); c = getchar(); angle[AXIS2] = s2go.readEnc(offset + AXIS2) * LinkCountsToDeg[finger][AXIS2]; // printf("angle[%d] = %5.3f\n", AXIS2, angle[AXIS2]); printf(" Step 8: Move first link to joint limit and then press RETURN\n"); printf(" Orientation of the second link does not matter\n"); fflush(stdin); c = getchar(); angle[AXIS1] = s2go.readEnc(offset+ AXIS1) * LinkCountsToDeg[finger][AXIS1]; // printf("angle[%d] = %5.3f\n", AXIS1, angle[AXIS1]); printf(" Step 9: Until I figure out how to write this to the config file \n"); printf(" record the following values\n"); newqInit[finger][AXIS1] = -180.0 - angle[AXIS1]; newqInit[finger][AXIS2] = -180 + (90 - angle[AXIS2] - angle[AXIS1]); printf("q%d = %5.3f\n", 1+ AXIS1 + offset, newqInit[finger][AXIS1]); printf("q%d = %5.3f\n", 1+ AXIS2 + offset, newqInit[finger][AXIS2]); } fflush(stdin); printf("Modify config file for future runs and then press RETURN\n"); c = getchar(); printf("Updating initial position in memory, calling link initialization\n"); for (finger = 0;finger < FINGERS; finger++) { for (axis = 0;axis < AXES; axis++) { dexDriver->qInit[finger][axis] = newqInit[finger][axis]; } } dexDriver->encInit(); return(1); } /*************************************************************** Fucntion: calibrateTactileRoutine Parameters: Returns: Notes: This fucntion calibrates the tacile sensors The basic process is as follows: --------> nothing yet ***************************************************************/ static int calibrateTactileRoutine( void ) { char yesno[10]; char answer[10]; zeroTorques(); printf(" Tactile Calibration Routine Called \n This routine will allow you recalibrate the tactile sensors if necessary.\n"); printf(" Do you want to perform this routine (yes/no)? "); scanf("%[^'\n']", &yesno); //scan in message until \n if (strstr(yesno,"no" )) { printf("Returning to main loop\n"); return(0); } printf("Will needs to write this code first!\n Returning to main loop\n"); return(1); } /*************************************************************** Fucntion: readState Parameters: Returns: Notes: this function reads the encoders and updates the shmem position value ***************************************************************/ static void readState ( void ) { short encAxis = 0; int finger, axis; static float qPosition[FINGERS][AXES]; static float qVelocity[FINGERS][AXES]; static float qPositionOld[FINGERS][AXES] = {{dexDriver->qInit[0][0],dexDriver->qInit[0][1]},{dexDriver->qInit[1][0],dexDriver->qInit[1][1]}}; static float qVelocityOld[FINGERS][AXES] = {{0.0,0.0},{0.0,0.0}}; // static float xPosition[FINGERS][AXES]; static vecLftRht xPosition; static vecLftRht qVelocityVector; static vecLftRht xVelocityVector; //calculate angular position (degrees) s2go.latchEncoders(); for (finger = 0; finger < FINGERS; finger++) { for (axis = 0; axis < AXES; axis++) { qPosition[finger][axis] = dexDriver->qInit[finger][axis] - s2go.readEnc(encAxis) * LinkCountsToDeg[finger][axis]; encAxis++; } } dexDriver->setQ(qPosition); // UPDATE SHMEM //calculate angular velocity (degrees/sec) for (finger = 0; finger < FINGERS; finger++) { for (axis = 0; axis < AXES; axis++) { qVelocity[finger][axis] = 0.95 * qVelocityOld[finger][axis] + 0.05 * (qPosition[finger][axis] - qPositionOld[finger][axis]) * 1000.0; // printf("\nqPosition[%d][%d] = %5.2f, qPositionOld[%d][%d] = %5.2f\n",finger, axis, qPosition[finger][axis],finger, axis, qPositionOld[finger][axis] ); // printf("qVelocity[%d][%d] = %5.2f, qVelocityOld[%d][%d] = %5.2f\n",finger, axis, qVelocity[finger][axis],finger, axis, qVelocityOld[finger][axis] ); qVelocityOld[finger][axis] = qVelocity[finger][axis]; qPositionOld[finger][axis] = qPosition[finger][axis]; } } dexDriver->setDQ(qVelocity); // UPDATE SHMEM /* xPosition[LEFT][AXISX] = - ( 0.5 * base.width ) + dexDriver->link.length[AXIS1] * cos ( qPosition[LEFT][AXIS1] DEG ) + dexDriver->link.length[AXIS2] * cos ( qPosition[LEFT][AXIS2] DEG ); xPosition[LEFT][AXISY] = dexDriver->link.length[AXIS1] * sin ( qPosition[LEFT][AXIS1] DEG ) + dexDriver->link.length[AXIS2] * sin ( qPosition[LEFT][AXIS2] DEG ); xPosition[RIGHT][AXISX] = ( 0.5 * base.width ) - dexDriver->link.length[AXIS1] * cos ( qPosition[RIGHT][AXIS1] DEG ) - dexDriver->link.length[AXIS2] * cos ( qPosition[RIGHT][AXIS2] DEG ); xPosition[RIGHT][AXISY] = dexDriver->link.length[AXIS1] * sin ( qPosition[RIGHT][AXIS1] DEG ) + dexDriver->link.length[AXIS2] * sin (qPosition[RIGHT][AXIS2] DEG ); dexDriver->setX(xPosition); // UPDATE SHMEM */ computeForwardKinematics(xPosition); dexDriver->setX(xPosition); //calculate operational space velocity // using vector notation because of matrix multiplication dexDriver->getDQ(qVelocityVector); qVelocityVector.left *= PI/180.0; qVelocityVector.right *= PI/180.0; dexMatrix->Jacobian.left.multiply( qVelocityVector.left, xVelocityVector.left); dexMatrix->Jacobian.right.multiply( qVelocityVector.right, xVelocityVector.right); dexDriver->setDX(xVelocityVector); // UPDATE SHMEM #ifdef DEBUG_STATE static float testqPosition[FINGERS][AXES]; static float testqVelocity[FINGERS][AXES]; static float testxPosition[FINGERS][AXES]; static float testxVelocity[FINGERS][AXES]; dexDriver->getQ (testqPosition); dexDriver->getDQ(testqVelocity); dexDriver->getX (testxPosition); dexDriver->getDX(testxVelocity); // printf(" q position = %5.3f, %5.3f, %5.3f, %5.3f", testqPosition[0][0],testqPosition[0][1],testqPosition[1][0],testqPosition[1][1]); // printf(" q velocity = %5.3f, %5.3f, %5.3f, %5.3f", testqVelocity[0][0],testqVelocity[0][1],testqVelocity[1][0],testqVelocity[1][1]); printf(" x position = %5.3f, %5.3f, %5.3f, %5.3f", testxPosition[0][0],testxPosition[0][1],testxPosition[1][0],testxPosition[1][1]); printf(" x velocity = %5.3f, %5.3f, %5.3f, %5.3f", testxVelocity[0][0],testxVelocity[0][1],testxVelocity[1][0],testxVelocity[1][1]); printf("\r"); fflush(stdout); #endif //DEBUG_STATE } /*************************************************************** Fucntion: computeForwardKinematics Parameters: Returns: Notes: **************************************************************/ static void computeForwardKinematics ( vecLftRht & xPos ) { static vecLftRht xPosFingerTipCenter; static vecLftRht rContactF; static vecLftRht rContactW; static matLftRht RotationMatrix; static vecLftRht xPosFingerTipContact; static float qPosition[FINGERS][AXES]; dexDriver->getQ(qPosition); //calculate operational space position (meters) // note that the link.length[AXIS2] must be to the fingertip center xPosFingerTipCenter.left[AXISX] = - ( 0.5 * base.width ) + dexDriver->link.length[LEFT][AXIS1] * cos ( qPosition[LEFT][AXIS1] DEG ) + dexDriver->link.length[LEFT][AXIS2] * cos ( qPosition[LEFT][AXIS2] DEG ); xPosFingerTipCenter.left[AXISY] = dexDriver->link.length[LEFT][AXIS1] * sin ( qPosition[LEFT][AXIS1] DEG ) + dexDriver->link.length[LEFT][AXIS2] * sin ( qPosition[LEFT][AXIS2] DEG ); xPosFingerTipCenter.right[AXISX] = ( 0.5 * base.width ) - dexDriver->link.length[RIGHT][AXIS1] * cos ( qPosition[RIGHT][AXIS1] DEG ) - dexDriver->link.length[RIGHT][AXIS2] * cos ( qPosition[RIGHT][AXIS2] DEG ); xPosFingerTipCenter.right[AXISY] = dexDriver->link.length[RIGHT][AXIS1] * sin ( qPosition[RIGHT][AXIS1] DEG ) + dexDriver->link.length[RIGHT][AXIS2] * sin (qPosition[RIGHT][AXIS2] DEG ); // now lets compute the vector to contact location in the fingertip frame dexDriver->getContactVec( rContactF ); dexDriver->getRotMatFTip2W( RotationMatrix ); RotationMatrix.left.multiply ( rContactF.left, rContactW.left ); RotationMatrix.right.multiply ( rContactF.right, rContactW.right ); xPosFingerTipContact.left = xPosFingerTipCenter.left + rContactW.left; xPosFingerTipContact.right = xPosFingerTipCenter.right + rContactW.right; xPos.left = xPosFingerTipContact.left; xPos.right = xPosFingerTipContact.right; // dexDriver->setX(xPosFingerTipCenter); // Update the shared memory } /*************************************************************** Fucntion: outputCmdTorques Parameters: torques Returns: Notes: This function updates the actual D/A values based on shared mem torques ---------o---------------------------------o----------- / | | \ / \ / / \ \ / \ | / neg torque neg torque \ | \ / (pos current) (pos current) \ / -- / <--- ---> \ -- / \ o ----------- -------------- o / \ \ neg torque neg torque / / \ | \ | | / | \-- \ <- / \ -> / --/ \ (pos current) (pos current) / \ / \ / \ / / \ ***************************************************************/ static void outputCmdTorques( void ) { int finger,axis; int channelNum = 0; static float torques[FINGERS][AXES]; static float current[FINGERS][AXES]; static double voltage[FINGERS][AXES]; dexDriver->getTorques(torques); for (finger = 0; finger < FINGERS; finger++) { for (axis = 0; axis < AXES; axis++) { current[finger][axis] = - (torques [finger][axis] / motorConstant [finger][axis]); if ( current [finger][axis] < currentMin ) current [finger][axis] = currentMin; if ( current [finger][axis] > currentMax ) current [finger][axis] = currentMax; /**** current = KAMP*voltage + AMP_OFFSET ****/ voltage [finger][axis] = (current [finger][axis] - ampOffset[finger][axis]) / ampConstant [finger][axis]; s2go.DAC (DACport[channelNum], voltage[finger][axis]); //driver requires board axis 0-3 // printf(" channelNum = %d\n", channelNum); channelNum++; //printf(" Torque cmd Finger %d Axes %d -- %4.3f N-m \r",finger,axis,torques[finger][axis]); //printf("AXIS1_RATIO = %4.3f \n",AxisRatio[finger][axis]); //printf(" KMOT Finger %d Axes %d -- %4.3f N-m/A \n",finger,axis,motorConstant[finger][axis]); //printf(" Current Finger %d Axes %d -- %4.3f A \n",finger,axis,current[figners][axis]); //printf(" Voltage Finger %d Axes %d -- %4.3f V \n",finger,axis,voltage[finger][axis]); fflush(stdout); } } //end double for loop } //outputCmdTorques /*************************************************************** Fucntion: zeroTorques Parameters: Returns: Notes: This function zeros the torques (accounts for amp and DAC offsets) ***************************************************************/ static void zeroTorques( void ) { int finger,axis; int channelNum = 0; static float torques[FINGERS][AXES]; static float current[FINGERS][AXES]; static double voltage[FINGERS][AXES]; dexDriver->getTorques(torques); for (finger = 0; finger < FINGERS; finger++) { for (axis = 0; axis < AXES; axis++) { voltage [finger][axis] = (0.0 - ampOffset[finger][axis]) / ampConstant [finger][axis]; s2go.DAC (DACport[channelNum], voltage[finger][axis]); //driver requires board axis 0-3 channelNum++; } } //end double for loop } //zeroTorques /************************************************************** Fucntion: sigTermHandler Parameters: Returns: Notes: when program is terminated this function closes all the necessary things (shmem, servo2go, etc) ***************************************************************/ static void sigTermHandler( int ) { printf("\nclosing dex_driver\n"); destroy_shm(dexClientShmName); destroy_shm(dexMatrixShmName); destroy_shm(dexControlShmName); destroy_shm(adcStructShmName); destroy_shm(dexObjectShmName); qnx_hint_detach (irq5pid); isr.kill(); matrix.kill(); control.kill(); dexinterface.kill(); master.kill(); zeroTorques(); s2go.~servo2goDex(); servoLoopTimer.~DML_timer(); fflush(stdout); exit(0); }