// mskccRobot.cpp: implementation of the mskccRobot class. // $Id: mskccRobot.cpp,v 1.19 2006/08/02 15:54:20 pkaz Exp $ ////////////////////////////////////////////////////////////////////// #include "mskccRobot.h" // The class is registered with a default level of detail 5 for the logs. // Levels of details are 1: Errors duting init, 2: Warnings during init, // 3: Messages during init, 5: Error during normal operations, 6: Warnings // during normal operations, 7: Messages during normal operations, 8: Very // verbose, i.e. log all connections to the Galil controller. #ifdef CMN_DECLARE_SERVICES_INSTANTIATION CMN_IMPLEMENT_SERVICES(mskccRobot); // cisst-0.3.1 #else CMN_IMPLEMENT_SERVICES(mskccRobot,5); // cisst-0.3.0 #endif using namespace std; // The following program is downloaded to the Galil controller. // Note that the line continuation character (\) is used at the end of // each line so that they are concatenated together into a single string. // This program always executes on the Galil controller. There is a // main loop (#LOOP) that always reads the force feedback and subtracts // the bias values. It also checks the forces against threshold limits // and stops the robot if the threshold is exceeded, setting "fstop" to 1 // to indicate this condition. It is the PC's responsibility to clear // "fstop" and to re-enable motor power. // // There is some conditional code that is executed based on the values of the // variables "do_bias" and "fstate": // // - If "do_bias" is 1, then the bias values are updated. Note that this // code averages 4 readings. From my experimentation, it seems that // 4 consecutive reads of an analog input channel produces 4 different // values, but I am not certain if this is always true. // // - The "fstate" variable implements a finite state machine: // // 0 --> idle // 1 --> initialize force control (note that do_bias is set to 1) // 2 --> force control active // 3 --> force control terminating (cleanup) // // The PC is responsible for specifying the transition 0->1 (by setting // fstate to 1) and 2->3 (by setting fstate to 3). // // Note also the PC can change the force control gains and can retrieve the // values of any parameters of interest (such as the force and bias values). // The force controller uses a nonlinear gain to achieve fine control at low // forces while allowing fast motions at high forces. The nonlinearity was // implemented using the built-in cosine function. Since the maximum analog // input is +/-10V, we multiply the analog force feedback value by 9 to get it // in the range -90 to +90. The nonlinear factor is 1-cos(force*9), which is // fairly flat for low forces but has a higher slope for large forces. // // Using the timing variables (curtime, oldtime, period), it seems that the force // control loop takes about 24 clock ticks, which is 24/1024 = 23 msec. // // We set the force control travel limits (poslim, neglim) to be a little // less than the forward and reverse software limits (FL, BL). Specifically, // we reduce them by gain*10/4, which corresponds to the distance that the // axis would travel in 0.25 seconds at maximum velocity; i.e., when the // feedback force is full-scale (10). // // "dt" is the time parameter that determines the margin within which the // robot speed is reduced; i.e., if we are within "dt" seconds (or less) of // hitting the travel limit at the current speed, then we reduce speed by // the scale factor "fact". // // PROGRAMMER BEWARE: The Galil software does not define operator precedence as in C. // For example, a-b*c is interpreted by Galil as (a-b)*c, whereas a C // compiler would produce a-(b*c). char force_controller[] = "#CONT\n\ DM force[3]\n\ DM bias[3]\n\ DM gain[3]\n\ DM vel[3]\n\ DM poslim[3]\n\ DM neglim[3]\n\ gain[0]=6000\n\ gain[1]=6000\n\ gain[2]=45000\n\ dt=2\n\ fstop=0\n\ fstate=0\n\ do_bias=1\n\ curtime=TIME\n\ oldtime=curtime\n\ period=0\n\ #LOOP\n\ IF (fstate=1)\n\ ST ABCD\n\ poslim[0] = _FLA - (gain[0]*2.5)\n\ poslim[1] = _FLB - (gain[1]*2.5)\n\ poslim[2] = _FLC - (gain[2]*2.5)\n\ neglim[0] = _BLA + (gain[0]*2.5)\n\ neglim[1] = _BLB + (gain[1]*2.5)\n\ neglim[2] = _BLC + (gain[2]*2.5)\n\ JG 0,0,0\n\ BG XYZ\n\ do_bias=1\n\ fstate=2\n\ ENDIF\n\ IF (do_bias=1)\n\ bias[0]=(@AN[1]+@AN[1]+@AN[1]+@AN[1])/4\n\ bias[1]=(@AN[2]+@AN[2]+@AN[2]+@AN[2])/4\n\ bias[2]=(@AN[3]+@AN[3]+@AN[3]+@AN[3])/4\n\ do_bias=0\n\ ENDIF\n\ force[0]=bias[0]-@AN[1]\n\ force[1]=bias[1]-@AN[2]\n\ force[2]=@AN[3]-bias[2]\n\ IF ((fstate=2) & (fstop=0))\n\ index=0\n\ #FLOOP\n\ ~a=index\n\ fact = 0\n\ IF (force[index]<-0.1)\n\ vel[index]=(force[index]+0.1)*gain[index]*(1-@COS[force[index]*9])\n\ fact = (neglim[index] - _TP~a)/(dt*vel[index])\n\ ELSE\n\ IF (force[index]>0.1)\n\ vel[index]=(force[index]-0.1)*gain[index]*(1-@COS[force[index]*9])\n\ fact = (poslim[index] - _TP~a)/(dt*vel[index])\n\ ENDIF\n\ ENDIF\n\ IF (fact <= 0)\n\ vel[index] = 0;\n\ ELSE\n\ IF (fact < 1)\n\ vel[index] = vel[index]*fact\n\ ENDIF\n\ ENDIF\n\ index=index+1\n\ JP #FLOOP,index<3\n\ JG vel[0],vel[1],vel[2]\n\ curtime=TIME\n\ period=curtime-oldtime\n\ oldtime=curtime\n\ ENDIF\n\ IF (fstate=3)\n\ ST ABCD\n\ fstate=0\n\ ENDIF\n\ JP #LOOP\n\ EN\n"; ////////////////////////////////////////////////////////////////////// // Private (internal) member functions ////////////////////////////////////////////////////////////////////// // ReadConfigFile: read the hardware-dependent configuration information // For now, values are hard-coded in this function //bool mskccRobot::ReadConfigFile(const char *cfg_file) bool mskccRobot::ReadConfigFile() { // Scale factors (encoder counts per mm) cnts_per_mm[X] = 1161.42; cnts_per_mm[Y] = 1161.42; cnts_per_mm[Z1] = 4645.67; cnts_per_mm[Z2] = 141537.6/25.4; // 5572.35 counts per mm // Amplifier loop gain (Amps/Volt) amp_type[X] = amp_type[Y] = amp_type[Z1] = 1; // 0.7 A/V amp_type[Z2] = 0; // 0.4 A/V // PID controller gains pid[X].kp = 133.38; pid[X].ki = 52.352; pid[X].kd = 996.25; pid[Y].kp = 157.50; pid[Y].ki = 60.617; pid[Y].kd = 1176.38; pid[Z1].kp = 118.00; pid[Z1].ki = 45.430; pid[Z1].kd = 881.75; pid[Z2].kp = 56.38; pid[Z2].ki = 18.10; pid[Z2].kd = 420.88; // Parameters for replacement motor, with amp_type[Z2] = 0 // pid[Z2].kp = 74.13; pid[Z2].ki = 28.531; pid[Z2].kd = 553.75; // Average current limit (Amps). The Galil board averages the // current over a 0.25 second window. curlim_avg[X] = curlim_avg[Y] = curlim_avg[Z1] = 2.0; curlim_avg[Z2] = 1.0; // Home positions in mm // (Encoder values for X,Y,Z1 obtained by measuring range of motion // between limit switches and dividing by 2) jt_home[X] = 122.2; jt_home[Y] = 117.7; jt_home[Z1] = 119.2; jt_home[Z2] = 5.0; // Joint forward (upper) software limit, in mm // For X,Y,Z1 home position is at forward hardware limit, so set software // limit to be 5 mm before hardware limit. // For Z2, home position is at reverse hardware limit, so set software // limit to be 5 mm before hardware limit. jt_ulim[X] = jt_home[X] - 5; jt_ulim[Y] = jt_home[Y] - 5; jt_ulim[Z1] = jt_home[Z1] - 5; jt_ulim[Z2] = jt_home[Z2] - 5; // Joint reverse (lower) software limit, in mm // For X,Z1 we are symmetric about the origin. // For Y, reduce limit to avoid collision between rodent bed and Z1 axis // For Z2, set software limit based on measurements. jt_llim[X] = -jt_ulim[X]; jt_llim[Y] = -85.0; jt_llim[Z1] = -jt_ulim[Z1]; jt_llim[Z2] = -92.2; return true; } // VerifyStatus: check the robot status and throw an exception for any set status bit // that matches the specified mask (statusMask). // // Note: This function will silently attempt to enable the motor power if it is not // already on and statusMask has the MOTOR_OFF_MASK bit set. long mskccRobot::VerifyStatus(const char *callerName, long statusMask) { long status = GetStatus(); long testMask = status & statusMask; if (testMask & ESTOP_MASK) throw ExcpPowerOff(string(callerName) + ": Motor power is off"); if (testMask & MOTOR_OFF_MASK) { EnableMotorPower(); status = GetStatus(); testMask = status & statusMask; if (testMask & MOTOR_OFF_MASK) { CMN_LOG_CLASS(1) << "VerifyStatus: failed to enable motor power" << endl; throw ExcpMotorOff(string(callerName) + ": Motor is off"); } } if (testMask & AMP_ERROR_MASK) throw ExcpAmpError(string(callerName) + ": Amplifier error"); if (testMask & NOT_HOMED_MASK) throw ExcpNotHomed(string(callerName) + ": Robot not homed"); if (testMask & PROG_ERROR_MASK) throw ExcpForceProg(string(callerName) + ": Galil force control program not running"); return status; } // GetForceState: Get value of "fstate" variable, which indicates the state of // the force control finite state machine. int mskccRobot::GetForceState() { const char *retbuf = SendCommand("fstate="); int fstate = -1; if (sscanf(retbuf,"%d", &fstate) != 1) { CMN_LOG_CLASS(5) << "GetForceState: error sscanf of \"" << retbuf << "\"" << endl; throw ExcpSystemError("GetForceState: error sscanf"); } return fstate; } ////////////////////////////////////////////////////////////////////// // Construction/Destruction ////////////////////////////////////////////////////////////////////// mskccRobot::mskccRobot() { hDmc = 0; speed = accel = decel = 0; homed = false; for (int i=0; i < MAX_AXES; i++) { cnts_per_mm[i] = 1.0; jt_home[i] = jt_ulim[i] = jt_llim[i] = 0.0; amp_type[i] = 1; pid[i].kp = 6; pid[i].ki = 0; pid[i].kd = 64; curlim_avg[i] = 9.998; } amp_gain[0] = 0.4; // 0.4 A/V amp_gain[1] = 0.7; // 0.7 A/V amp_gain[2] = 1.0; // 1.0 A/V } mskccRobot::mskccRobot(const mskccRobot& rob) { *this = rob; } mskccRobot::~mskccRobot() { DMCClose(hDmc); } mskccRobot &mskccRobot::operator =(const mskccRobot& rob) { hDmc = rob.hDmc; speed = rob.speed; accel = rob.accel; decel = rob.decel; homed = rob.homed; for (int i=0; i < MAX_AXES; i++) { cnts_per_mm[i] = rob.cnts_per_mm[i]; jt_home[i] = rob.jt_home[i]; jt_ulim[i] = rob.jt_ulim[i]; jt_llim[i] = rob.jt_llim[i]; } return *this; } ////////////////////////////////////////////////////////////////////// // Public member functions ////////////////////////////////////////////////////////////////////// // Init: initialize robot (must be called) // This function establishes communication with the Galil controller, // configures it, downloads the application program and then starts execution. bool mskccRobot::Init() { char buffer[sizeof(force_controller)+1]; if (hDmc) DMCClose(hDmc); long rc = DMCOpen(1, 0, &hDmc); if (rc != DMCNOERROR) { CMN_LOG_CLASS(1) << "Could not open controller #1, rc = " << rc << endl; return false; } // Reset the controller, in case we're trying to escape a problem DMCReset(hDmc); // Clear the buffers DMCClear(hDmc); // Unhome robot UnHome(); // Read configuration file if (!ReadConfigFile()) return false; // Download force control program strcpy(buffer, force_controller); rc = DMCDownloadFromBuffer(hDmc, buffer, 0); if (rc != DMCNOERROR) { CMN_LOG_CLASS(1) << "Error downloading force controller from buffer, rc = " << rc << endl; return false; } // Set for brush motor operation SendCommand("BR 1,1,1,1"); // Set position error limit to 2mm sprintf(buffer,"ER %ld,%ld,%ld,%ld", (long)(2*cnts_per_mm[X]), (long)(2*cnts_per_mm[Y]), (long)(2*cnts_per_mm[Z1]), (long)(2*cnts_per_mm[Z2])); SendCommand(buffer); // Set axes to turn off on error SendCommand("OE 1,1,1,1"); // Configure home/limit switches // -1 -> limit switches active low (N/O switches) // 1 -> home switch drives motor in forward direction when high // -1 -> latch input is active low (not used) // 0 -> inputs for general use SendCommand("CN -1,1,-1,0"); // Set amplifier loop gain. sprintf(buffer, "AG %d,%d,%d,%d", amp_type[X], amp_type[Y], amp_type[Z1], amp_type[Z2]); SendCommand(buffer); // Set PID gains sprintf(buffer, "KP %lf,%lf,%lf,%lf", pid[X].kp, pid[Y].kp, pid[Z1].kp, pid[Z2].kp); SendCommand(buffer); sprintf(buffer, "KI %lf,%lf,%lf,%lf", pid[X].ki, pid[Y].ki, pid[Z1].ki, pid[Z2].ki); SendCommand(buffer); sprintf(buffer, "KD %lf,%lf,%lf,%lf", pid[X].kd, pid[Y].kd, pid[Z1].kd, pid[Z2].kd); SendCommand(buffer); // Set continuous torque limit sprintf(buffer, "TL %lf,%lf,%lf,%lf", curlim_avg[X]/amp_gain[amp_type[X]], curlim_avg[Y]/amp_gain[amp_type[Y]], curlim_avg[Z1]/amp_gain[amp_type[Z1]], curlim_avg[Z2]/amp_gain[amp_type[Z2]]); SendCommand(buffer); // Set maximum torque limit SendCommand("TK 9.998,9.998,9.998,9.998"); // Set initial values for speed, acceleration, deceleration SetSpeed(12.0); // 12 mm/sec SetAccel(25.0); // 25 mm/sec**2 SetDecel(25.0); // 25 mm/sec**2 // Start application program on Galil SendCommand("XQ #CONT,0"); // Enable motor power. This will fail if the Stop switch is pressed, // but that isn't considered an error at this point. SendCommand("SH"); return true; } // EnableMotorPower: turns on the motor servos // Note that "fstop" is cleared just in case we are recovering from a shutdown // due to a force threshold being exceeded. void mskccRobot::EnableMotorPower() { VerifyStatus("EnableMotorPower", ESTOP_MASK); SendCommand("fstop=0"); SendCommand("SH"); } // DisableMotorPower: turns off the motor servos void mskccRobot::DisableMotorPower() { SendCommand("MO"); } // Stop: stop robot motion (do not disable motor power) void mskccRobot::Stop() { SendCommand("ST ABCD"); } // Home: Home robot (initialize encoders) // Note that we home the axes in the reverse order so that the Z axes retract to // their highest position before we start moving the X and Y axes. This is necessary // to avoid the possibility of collision. bool mskccRobot::Home() { char buffer[256]; // Make sure robot is unhomed before we begin; otherwise, some of the homing moves will be // outside the software travel limits and the robot won't home properly. UnHome(); // Get the status (we are interested in the initial state of the home switches). By calling // VerifyStatus, we also ensure that the motor power is on. int status = VerifyStatus("Home", ESTOP_MASK | AMP_ERROR_MASK | MOTOR_OFF_MASK); // Home Z2,Z1,Y,X motors in sequence for (char axis='D'; axis >= 'A'; axis--) { if (axis == 'D') { // Configure home switch to drive motors in reverse direction when high (Z2) SendCommand("CN ,-1"); } else { // Configure home switch to drive motors in forward direction when high (X,Y,Z1) SendCommand("CN ,1"); } // Move motor to home switch (forward limit for X,Y,Z1 and reverse limit for Z2) sprintf(buffer, "FE %c", axis); SendCommand(buffer); sprintf(buffer, "BG%c", axis); SendCommand(buffer); // Wait for motion to finish WaitMotion(); // If we were originally on the home switch, then the first FE moved us off the switch; send the // command again to move off the switch. if ((status>>(8+6*(axis-'A'))) & JT_HOME_MASK) { SendCommand(buffer); WaitMotion(); } // Now, move backwards to find the first index pulse double saved_speed = GetSpeed(); const double HOME_SPEED = 1.0; // 1 mm/sec sprintf(buffer, "JG %ld,%ld,%ld,%ld", -(long)(HOME_SPEED*cnts_per_mm[X]), -(long)(HOME_SPEED*cnts_per_mm[Y]), -(long)(HOME_SPEED*cnts_per_mm[Z1]), (long)(HOME_SPEED*cnts_per_mm[Z2])); SendCommand(buffer); sprintf(buffer,"FI %c", axis); SendCommand(buffer); sprintf(buffer,"BG%c", axis); SendCommand(buffer); WaitMotion(); SetSpeed(saved_speed); } // Restore default setting SendCommand("CN ,1"); // Now, set the initial position // Set home position so that 0,0,0 is at center of travel for X,Y,Z1 sprintf(buffer, "DP %ld,%ld,%ld,%ld", (long)(jt_home[X]*cnts_per_mm[X]), (long)(jt_home[Y]*cnts_per_mm[Y]), (long)(jt_home[Z1]*cnts_per_mm[Z1]), (long) (-jt_home[Z2]*cnts_per_mm[Z2])); SendCommand(buffer); // Now set software limits. Note that the X,Y,Z1 home position is at the forward limits while the Z2 home // position is at the reverse limit. sprintf(buffer,"FL %ld,%ld,%ld,%ld", (long)(jt_ulim[X]*cnts_per_mm[X]),(long)(jt_ulim[Y]*cnts_per_mm[Y]), (long)(jt_ulim[Z1]*cnts_per_mm[Z1]),(long)(-jt_llim[Z2]*cnts_per_mm[Z2])); SendCommand(buffer); sprintf(buffer,"BL %ld,%ld,%ld,%ld", (long)(jt_llim[X]*cnts_per_mm[X]),(long)(jt_llim[Y]*cnts_per_mm[Y]), (long)(jt_llim[Z1]*cnts_per_mm[Z1]),(long)(-jt_ulim[Z2]*cnts_per_mm[Z2])); SendCommand(buffer); homed = true; // Move robot to software joint limits MoveJoints(jt_ulim[X], jt_ulim[Y], jt_ulim[Z1], jt_ulim[Z2]); WaitMotion(); return true; } // UnHome: Unhome the robot. Besides clearing the homed flag, this function turns off the forward and reverse // software travel limits. void mskccRobot::UnHome() { homed = false; SendCommand("FL 2147483647, 2147483647, 2147483647, 2147483647"); SendCommand("BL -2147483648, -2147483648, -2147483648, -2147483648"); } // MoveXYZ1: Move the first 3 axes (X, Y, Z1) to the specified xyz position void mskccRobot::MoveXYZ1(double x, double y, double z1) { char buffer[256]; VerifyStatus("MoveXYZ1", ANY_ERROR_MASK & ~PROG_ERROR_MASK); if (!(InsideLimits(X, x) && InsideLimits(Y, y) && InsideLimits(Z1, z1))) { CMN_LOG_CLASS(5) << "MoveXYZ1: (" << x << ", " << y << ", " << z1 << ") is out of limits" << endl; throw ExcpLimitError("MoveXYZ1: out of limits"); } long xgoal = (long)(x*cnts_per_mm[X]); long ygoal = (long)(y*cnts_per_mm[Y]); long z1goal = (long)(z1*cnts_per_mm[Z1]); sprintf(buffer, "PA %ld,%ld,%ld", xgoal, ygoal, z1goal); SendCommand(buffer); SendCommand("BGABC"); } // MoveZ2: Move the 4th axis (Z2) to the specified goal position void mskccRobot::MoveZ2(double goal) { char buffer[256]; VerifyStatus("MoveZ2", ANY_ERROR_MASK & ~PROG_ERROR_MASK); if (!InsideLimits(Z2, goal)) { CMN_LOG_CLASS(5) << "MoveNeedle: " << goal << " is out of limits" << endl; throw ExcpLimitError("MoveZ2: out of limits"); } long z2goal = (long)(-goal*cnts_per_mm[Z2]); sprintf(buffer, "PAD=%ld", z2goal); SendCommand(buffer); SendCommand("BGD"); } // AdvanceZ2: Advance the Z2 axis by the specified incremental distance (negative is down) // Technically, the robot does not have to be homed for this move, but we // enforce that anyway. We do not currently check the software travel // limits in this function (they are checked in the lower-level Galil software) void mskccRobot::AdvanceZ2(double incr) { char buffer[256]; VerifyStatus("AdvanceZ2", ANY_ERROR_MASK & ~PROG_ERROR_MASK); long z2goal = (long)(-incr*cnts_per_mm[Z2]); sprintf(buffer, "PR 0,0,0,%ld", z2goal); SendCommand(buffer); SendCommand("BGD"); } // MoveJoints: Move all joints (not in world coordinate system) void mskccRobot::MoveJoints(double x, double y, double z1, double z2) { char buffer[256]; VerifyStatus("MoveJoints", ANY_ERROR_MASK & ~PROG_ERROR_MASK); if (!(InsideLimits(X, x) && InsideLimits(Y, y) && InsideLimits(Z1, z1) && InsideLimits(Z2, z2))) { CMN_LOG_CLASS(5) << "MoveJoints: (" << x << ", " << y << ", " << z1 << ", " << z2 << ") is out of limits" << endl; throw ExcpLimitError("MoveJoints: out of limits"); } long xgoal = (long)(x*cnts_per_mm[X]); long ygoal = (long)(y*cnts_per_mm[Y]); long z1goal = (long)(z1*cnts_per_mm[Z1]); long z2goal = (long)(-z2*cnts_per_mm[Z2]); sprintf(buffer, "PA %ld,%ld,%ld,%ld", xgoal, ygoal, z1goal, z2goal); SendCommand(buffer); SendCommand("BG"); } // GetPositionXYZ1: Get position of X,Y,Z1 axes. vctDouble3 mskccRobot::GetPositionXYZ1() { if (!IsHomed()) throw ExcpNotHomed("GetPositionXYZ1: Robot not homed"); const char *retbuf = SendCommand("TPABC"); long xpos = 0, ypos = 0, z1pos = 0; if (sscanf(retbuf, "%ld,%ld,%ld", &xpos, &ypos, &z1pos) != 3) { CMN_LOG_CLASS(5) << "GetPositionXYZ1: error sscanf of \"" << retbuf << "\"" << endl; throw ExcpSystemError("GetPositionXYZ1: error sscanf"); } return vctDouble3(xpos/cnts_per_mm[X], ypos/cnts_per_mm[Y], z1pos/cnts_per_mm[Z1]); } // GetPositionZ2: Get position of Z2 axis double mskccRobot::GetPositionZ2() { if (!IsHomed()) throw ExcpNotHomed("GetPositionZ2: Robot not homed"); const char *retbuf = SendCommand("TPD"); long z2pos = 0; if (sscanf(retbuf,"%ld", &z2pos) != 1) { CMN_LOG_CLASS(5) << "GetPositionZ2: error sscanf of \"" << retbuf << "\"" << endl; throw ExcpSystemError("GetPositionZ2: error sscanf"); } // Note: negate z2 feedback return -z2pos/cnts_per_mm[Z2]; } // GetJointPosition: Returns joint positions (x,y,z1,z2) vctDouble4 mskccRobot::GetJointPosition() { vctDouble3 xyz1 = GetPositionXYZ1(); double z2 = GetPositionZ2(); return vctDouble4(xyz1[0], xyz1[1], xyz1[2], z2); } // SetSpeed: Set the motion speed void mskccRobot::SetSpeed(double spd) { char buffer[256]; speed = spd; long xspeed = (long)(spd*cnts_per_mm[X]); long yspeed = (long)(spd*cnts_per_mm[Y]); long z1speed = (long)(spd*cnts_per_mm[Z1]); long z2speed = (long)(spd*cnts_per_mm[Z2]); sprintf(buffer, "SP %ld,%ld,%ld,%ld", xspeed, yspeed, z1speed, z2speed); SendCommand(buffer); } // SetAccel: set the motion acceleration void mskccRobot::SetAccel(double acc) { char buffer[256]; accel = acc; long xaccel = (long)(acc*cnts_per_mm[X]); long yaccel = (long)(acc*cnts_per_mm[Y]); long z1accel = (long)(acc*cnts_per_mm[Z1]); long z2accel = (long)(acc*cnts_per_mm[Z2]); sprintf(buffer, "AC %ld,%ld,%ld,%ld", xaccel, yaccel, z1accel, z2accel); SendCommand(buffer); } // SetDecel: set the motion deceleration void mskccRobot::SetDecel(double dec) { char buffer[256]; decel = dec; long xdecel = (long)(dec*cnts_per_mm[X]); long ydecel = (long)(dec*cnts_per_mm[Y]); long z1decel = (long)(dec*cnts_per_mm[Z1]); long z2decel = (long)(dec*cnts_per_mm[Z2]); sprintf(buffer, "DC %ld,%ld,%ld,%ld", xdecel, ydecel, z1decel, z2decel); SendCommand(buffer); } // GetSpeed: get the motion speed // Note that this returns the value of the shadow variable (speed), rather // than querying the Galil board. An alternative would be to use the speed // interrogation command (SP ?,?,?,?). double mskccRobot::GetSpeed() const { return speed; } // GetAccel: get the motion acceleration // Note that this returns the value of the shadow variable (accel), rather // than querying the Galil board. An alternative would be to use the acceleration // interrogation command (AC ?,?,?,?). double mskccRobot::GetAccel() const { return accel; } // GetDecel: get the motion deceleration // Note that this returns the value of the shadow variable (decel), rather // than querying the Galil board. An alternative would be to use the deceleration // interrogation command (DC ?,?,?,?). double mskccRobot::GetDecel() const { return decel; } // StartForceCompliance: start the force compliance mode // Note that the robot remains in force compliance mode until the StopForceCompliance // function is called (or the robot is stopped or powered off). void mskccRobot::StartForceCompliance() { // Check if currently idle or already running int fstate = GetForceState(); if ((fstate != 0) && (fstate != 2)) { CMN_LOG_CLASS(1) << "StartForceCompliance, fstate = " << fstate << endl; throw ExcpForceProg("StartForceCompliance: fstate != 0"); } // Check if robot is ready for motion VerifyStatus("StartForceCompliance", ANY_ERROR_MASK); // Start force controller if (fstate == 0) { SendCommand("fstate=1"); // Should we wait for fstate == 2? } } // StopForceCompliance: stop the force compliance mode void mskccRobot::StopForceCompliance() { int i; int fstate = GetForceState(); if ((fstate == 0) || (fstate == 3)) return; if (fstate != 2) { // Need to wait for fstate == 2 // We use a loop up to 20 so that we cannot hang the system by waiting indefinitely for (i=0; (i < 20) && (fstate == 1); i++) fstate = GetForceState(); if (fstate != 2) { CMN_LOG_CLASS(1) << "StopForceCompliance, fstate = " << fstate << endl; throw ExcpForceProg("StopForceCompliance: fstate != 2"); } } // Stop force controller SendCommand("fstate=3"); // Wait for fstate == 0 (idle) fstate = 3; for (i=0; (i < 20) && fstate; i++) fstate = GetForceState(); if (fstate) { CMN_LOG_CLASS(1) << "StopForceCompliance failed, fstate = " << fstate << endl; throw ExcpForceProg("StopForceCompliance: fstate != 0"); } // Restore speed, acceleration and deceleration parameters SetSpeed(speed); SetAccel(accel); SetDecel(decel); } // GetForces: Returns the current measured forces (from the analog inputs) // Note that the units are in volts -- the force sensor has not been // calibrated to force units such as Newtons or lbs. vctDouble3 mskccRobot::GetForces() { return GetGalilVector("force"); } // BiasForces: Zero the force values // Calling BiasForces causes the system to measure the current force levels and // then subtract them from subsequent readings. void mskccRobot::BiasForces() { SendCommand("do_bias=1"); } // GetForceBias: Return the current force bias values vctDouble3 mskccRobot::GetForceBias() { return GetGalilVector("bias"); } // GetForceGains: Return the current force controller gains vctDouble3 mskccRobot::GetForceGains() { return GetGalilVector("gain"); } // SetForceGains: Set the force controller gains void mskccRobot::SetForceGains(const vctDouble3& fgains) { char buffer[256]; sprintf(buffer, "gain[0]=%lf; gain[1]=%lf; gain[2]=%lf", fgains[0], fgains[1], fgains[2]); SendCommand(buffer); } // WaitMotion: Wait for the robot to stop the current motion void mskccRobot::WaitMotion() { // First, check if there is motion int status = GetStatus(); if (!(status & MOTOR_MOVING_MASK)) return; // Next, check if force control is running. If it is, we don't wait because otherwise it would be // an infinite loop. int fstate = GetForceState(); if (fstate != 0) { CMN_LOG_CLASS(5) << "WaitMotion, fstate = " << fstate << endl; throw ExcpWaitMotion("WaitMotion: fstate != 0"); } // Start a separate thread to query the controller. The TRUE flag allows Windows to get and dispatch messages. DMCWaitForMotionComplete(hDmc, "ABCD", TRUE); // As a check, call GetStatus // PK: The following lines were commented out at MSKCC on 2/17/05 // status = GetStatus(); // if (status & MOTOR_MOVING_MASK) { // CMN_LOG_CLASS(5) << "WaitMotion: motor still moving!" << endl; // throw ExcpWaitMotion("WaitMotion: motor still moving"); // } } // GetStatus: Return the robot status, which is a 32 bit number (8 general + 6 per axis) // Note that the header file defines enums for the different bit masks. // // General (8): // 7 -- unused (0) // 6 -- 1 if robot is not homed, NOT_HOMED_MASK // 5 -- 1 if force threshold was exceeded, FSTOP_MASK // 4 -- 1 if motor power supply is off (red switch latched), ESTOP_MASK // 3 -- 1 for any other amplifier error (over temp, over voltage, over current), AMP_ERROR_MASK // 2 -- 1 if Galil program is not running, PROG_ERROR_MASK // 1 -- 1 if any axis in motion, MOTOR_MOVING_MASK // 0 -- 1 if any motor is off, MOTOR_OFF_MASK // // Per axis (6), N = 0,1,2,3: // 8+6*N+5 (bits 13,19,25,31) -- 1 if axis in motion, JT_MOTOR_MOVING_MASK // 8+6*N+4 (bits 12,18,24,30) -- 1 if axis error exceeds limit, JT_ERROR_LIMIT_MASK // 8+6*N+3 (bits 11,17,23,29) -- 1 if motor is off, JT_MOTOR_OFF_MASK // 8+6*N+2 (bits 10,16,22,28) -- 1 if at forward limit switch, JT_FWDLIM_MASK // 8+6*N+1 (bits 9,15,21,27) -- 1 if at reverse limit switch, JT_REVLIM_MASK // 8+6*N+0 (bits 8,14,20,26) -- 1 if at home switch, JT_HOME_MASK // // Note that the ANY_ERROR_MASK can be used to quickly check if there are any general problems: // if (status&ANY_ERROR_MASK) // CMN_LOG_CLASS(1) << "There is an error, status = " << status << endl; // This mask looks for any set bits except the "motor moving" bit long mskccRobot::GetStatus() { long status = 0; const char *retbuf; int value, home_cfg; // Note whether robot is homed if (!IsHomed()) status |= NOT_HOMED_MASK; // Check whether the force limit was exceeded retbuf = SendCommand("fstop="); value = 0; if (sscanf(retbuf,"%d", &value) != 1) { CMN_LOG_CLASS(1) << "GetStatus: error sscanf of fstop: \"" << retbuf << "\"" << endl; throw ExcpSystemError("GetStatus: error sscanf for fstop"); } if (value) status |= FSTOP_MASK; // Get the current home switch configuration retbuf = SendCommand("MG _CN1"); home_cfg = 1; if (sscanf(retbuf,"%d", &home_cfg) != 1) { CMN_LOG_CLASS(1) << "GetStatus: error sscanf of CN1: \"" << retbuf << "\"" << endl; throw ExcpSystemError("GetStatus: error sscanf for CN1"); } // Send the TS command to get the switch status (also info about motors) retbuf = SendCommand("TS ABCD"); int sw[4] = { 0,0,0,0}; if (sscanf(retbuf,"%d,%d,%d,%d", &sw[0], &sw[1], &sw[2], &sw[3]) != 4) { CMN_LOG_CLASS(1) << "GetStatus: error sscanf of TS: \"" << retbuf << "\"" << endl; throw ExcpSystemError("GetStatus: error sscanf for TS"); } for (int i=0; i < MAX_AXES; i++) { int jt_shift = 8+6*i; if (sw[i]&0x0080) { status |= MOTOR_MOVING_MASK; status |= (JT_MOTOR_MOVING_MASK<