pg70_rs232_control_lib.cpp
Go to the documentation of this file.
00001 /*********************************************************************************************/
00031 /* Author: Frantisek Durovsky */
00032 
00033 #ifndef PG70_RS232_CONTROL_LIB_CPP
00034 #define PG70_RS232_CONTROL_LIB_CPP
00035 
00036 #include <pg70_rs232_control.h>
00037 
00038 namespace schunk_pg70
00039 {
00040 
00041 PG70_serial::PG70_serial(ros::NodeHandle *nh) :
00042     act_position_(-1),
00043     pg70_error_(0xff)
00044 {
00045   //Read launch file params
00046   nh->getParam("schunk_pg70/portname", port_name_);
00047   nh->getParam("schunk_pg70/baudrate", baudrate_);
00048   nh->getParam("schunk_pg70/gripper_id", gripper_id_);
00049   
00050   //Initialize and open serial port
00051   com_port_ = new serial::Serial (port_name_, (uint32_t)baudrate_, serial::Timeout::simpleTimeout(100));
00052   if (com_port_->isOpen())
00053   {
00054     ROS_INFO_STREAM("PG70: Serial port " << port_name_ << " openned");
00055       
00056     //Get initial state and discard input buffer
00057     while(pg70_error_ == 0xff)
00058     {
00059       pg70_error_ = getError(com_port_);
00060       ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00061     }
00062     
00063     //Start periodic gripper state reading 
00064     getPeriodicPositionUpdate(com_port_, TF_UPDATE_PERIOD);
00065   }
00066   else
00067     ROS_ERROR_STREAM("PG70: Serial port " << port_name_ << " not opened");
00068 }
00069 
00070 PG70_serial::~PG70_serial()
00071 {
00072   com_port_->close();      //Close port
00073   delete com_port_;        //delete object
00074 }
00075 
00076 void
00077 PG70_serial::reference(serial::Serial *port)
00078 {
00079   std::vector<uint8_t> output;
00080   output.push_back(0x05);           //message from master to module
00081   output.push_back(gripper_id_);    //module id
00082   output.push_back(0x01);           //Data Length
00083   output.push_back(0x92);           //Command reference
00084 
00085   //Checksum calculation
00086   uint16_t crc = 0;
00087 
00088   for(size_t i = 0; i < output.size(); i++)
00089     crc = CRC16(crc,output[i]);
00090 
00091   //Add checksum to the output buffer
00092   output.push_back(crc & 0x00ff);
00093   output.push_back((crc & 0xff00) >> 8);
00094 
00095   //Send message to the module
00096   port->write(output);
00097 }
00098 
00099 uint8_t
00100 PG70_serial::getError(serial::Serial *port)
00101 {
00102   ROS_INFO("Reading current module state...");
00103   std::vector<uint8_t> output;
00104   output.push_back(0x05);           //message from master to module
00105   output.push_back(gripper_id_);    //module id
00106   output.push_back(0x06);           //Data Length
00107   output.push_back(0x95);           //Command get state
00108   output.push_back(0x00);
00109   output.push_back(0x00);
00110   output.push_back(0x00);
00111   output.push_back(0x00);
00112   output.push_back(0x01);           //get state only once
00113  
00114   //Checksum calculation
00115   uint16_t crc = 0;
00116 
00117   for(size_t i = 0; i < output.size(); i++)
00118     crc = CRC16(crc,output[i]);
00119 
00120   //Add checksum to the output buffer
00121   output.push_back(crc & 0x00ff);
00122   output.push_back((crc & 0xff00) >> 8);
00123 
00124   //Send message to the module and wait for response
00125   port->write(output);
00126   ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00127 
00128   //Read response
00129   std::vector<uint8_t> input;
00130   port->read(input, INPUT_BUFFER_SIZE);
00131 
00132   //Process current state message
00133   int get_state_index = 0;
00134   if(input.size() > 0)
00135   {
00136     if(input.size() > 7)
00137     {
00138       for(size_t i = 0; i < input.size(); i++)
00139         if ((input[i] == 0x07) && (input[i+1] == 0x95)) get_state_index = i;
00140 
00141       int pg70_error_ = input[get_state_index + 7];
00142 
00143       switch (pg70_error_)
00144       {
00145         case 0x00: ROS_INFO("PG70: No error detected"); break;
00146         case 0xC8: ROS_ERROR("PG70: Error: 0xC8 detected: Wrong ramp type"); break;
00147         case 0xD2: ROS_ERROR("PG70: Error: 0xD2 detected: Config memory"); break;
00148         case 0xD3: ROS_ERROR("PG70: Error: 0xD3 detected: Program memory"); break;
00149         case 0xD4: ROS_ERROR("PG70: Error: 0xD4 detected: Invalid phrase"); break;
00150         case 0xD5: ROS_ERROR("PG70: Error: 0xD5 detected: Soft low"); break;
00151         case 0xD6: ROS_ERROR("PG70: Error: 0xD6 detected: Soft high"); break;
00152         case 0xD7: ROS_ERROR("PG70: Error: 0xD7 detected: Pressure"); break;
00153         case 0xD8: ROS_ERROR("PG70: Error: 0xD8 detected: Service required"); break;
00154         case 0xD9: ROS_ERROR("PG70: Error: 0xD9 detected: Emergency stop"); break;
00155         case 0xDA: ROS_ERROR("PG70: Error: 0xDA detected: Tow"); break;
00156         case 0xE4: ROS_ERROR("PG70: Error: 0xE4 detected: Too fast"); break;
00157         case 0xEC: ROS_ERROR("PG70: Error: 0xEC detected: Math error"); break;
00158         case 0xDB: ROS_ERROR("PG70: Error: 0xDB detected: VPC3"); break;
00159         case 0xDC: ROS_ERROR("PG70: Error: 0xDC detected: Fragmentation"); break;
00160         case 0xDE: ROS_ERROR("PG70: Error: 0xDE detected: Current"); break;
00161         case 0xDF: ROS_ERROR("PG70: Error: 0xDF detected: I2T"); break;
00162         case 0xE0: ROS_ERROR("PG70: Error: 0xE0 detected: Initialize"); break;
00163         case 0xE1: ROS_ERROR("PG70: Error: 0xE1 detected: Internal"); break;
00164         case 0xE2: ROS_ERROR("PG70: Error: 0xE2 detected: Hard low"); break;
00165         case 0xE3: ROS_ERROR("PG70: Error: 0xE3 detected: Hard high"); break;
00166         case 0x70: ROS_ERROR("PG70: Error: 0x70 detected: Temp low"); break;
00167         case 0x71: ROS_ERROR("PG70: Error: 0x71 detected: Temp high"); break;
00168         case 0x72: ROS_ERROR("PG70: Error: 0x72 detected: Logic low"); break;
00169         case 0x73: ROS_ERROR("PG70: Error: 0x73 detected: Logic high"); break;
00170         case 0x74: ROS_ERROR("PG70: Error: 0x74 detected: Motor voltage low"); break;
00171         case 0x75: ROS_ERROR("PG70: Error: 0x75 detected: Motor voltage high"); break;
00172         case 0x76: ROS_ERROR("PG70: Error: 0x76 detected: Cable break"); break;
00173         case 0x78: ROS_ERROR("PG70: Error: 0x78 detected: Motor temp"); break;
00174       }
00175       //Re-start periodic position reading 
00176       getPeriodicPositionUpdate(com_port_, TF_UPDATE_PERIOD);
00177       return((uint8_t)pg70_error_);
00178     }
00179     else
00180     {
00181       //Re-start periodic position reading 
00182       getPeriodicPositionUpdate(com_port_, TF_UPDATE_PERIOD);
00183       return(0xff) ;
00184     }
00185   }  
00186 }
00187 
00188 void
00189 PG70_serial::acknowledgeError(serial::Serial *port)
00190 {
00191   std::vector<uint8_t> output;
00192   output.push_back(0x05);          //message from master to module
00193   output.push_back(gripper_id_);   //module id
00194   output.push_back(0x01);          //Data Length
00195   output.push_back(0x8b);          //Command CMD ACK
00196 
00197   //Checksum calculation
00198   uint16_t crc = 0;
00199 
00200   for(size_t i = 0; i < output.size(); i++)
00201     crc = CRC16(crc,output[i]);
00202 
00203   //Add checksum to the output buffer
00204   output.push_back(crc & 0x00ff);
00205   output.push_back((crc & 0xff00) >> 8);
00206 
00207   //Send message to the module
00208   port->write(output);
00209 }
00210 
00211 void
00212 PG70_serial::setPosition(serial::Serial *port, int goal_position, int velocity, int acceleration)
00213 {
00214   ROS_INFO_STREAM("PG70: Moving from: " << act_position_ << " [mm] to " << goal_position << " [mm]");
00215   
00216   std::vector<uint8_t> output;
00217   output.push_back(0x05);                //message from master to module
00218   output.push_back(gripper_id_);          //module id
00219   output.push_back(0x0D);                //D-Len
00220   output.push_back(0xb0);                //Command mov pos
00221 
00222   //Position <0-69>mm
00223   unsigned int IEEE754_bytes[4];
00224   float_to_IEEE_754(goal_position,IEEE754_bytes);
00225 
00226   output.push_back(IEEE754_bytes[0]);    //Position first byte
00227   output.push_back(IEEE754_bytes[1]);    //Position second byte
00228   output.push_back(IEEE754_bytes[2]);    //Position third byte
00229   output.push_back(IEEE754_bytes[3]);    //Position fourth byte
00230 
00231   //Velocity<0-82>mm/s
00232   float_to_IEEE_754(velocity, IEEE754_bytes);
00233   output.push_back(IEEE754_bytes[0]);    //Velocity first byte
00234   output.push_back(IEEE754_bytes[1]);    //Velocity second byte
00235   output.push_back(IEEE754_bytes[2]);    //Velocity third byte
00236   output.push_back(IEEE754_bytes[3]);    //Velocity fourth byte
00237 
00238   //Acceleration<0-320>mm/s2
00239   float_to_IEEE_754(acceleration, IEEE754_bytes);
00240   output.push_back(IEEE754_bytes[0]);    //Acceleration first byte
00241   output.push_back(IEEE754_bytes[1]);    //Acceleration second byte
00242   output.push_back(IEEE754_bytes[2]);    //Acceleration third byte
00243   output.push_back(IEEE754_bytes[3]);    //Acceleration fourth byte
00244 
00245   //Checksum calculation
00246   uint16_t crc = 0;
00247 
00248   for(size_t i = 0; i < output.size(); i++)
00249     crc = CRC16(crc,output[i]);
00250 
00251   //Add checksum to the output buffer
00252   output.push_back(crc & 0x00ff);
00253   output.push_back((crc & 0xff00) >> 8);
00254 
00255   //Send message to the module
00256   port->write(output);
00257 }
00258 
00259 float
00260 PG70_serial::getPosition(serial::Serial *port)
00261 {
00262   std::vector<uint8_t> output;
00263   output.push_back(0x05);                 //message from master to module
00264   output.push_back(gripper_id_);           //module id
00265   output.push_back(0x06);                 //Data Length
00266   output.push_back(0x95);                 //Command get state
00267   output.push_back(0x00);
00268   output.push_back(0x00);
00269   output.push_back(0x00);
00270   output.push_back(0x00);
00271   output.push_back(0x01);                 //get state only once
00272 
00273   //Checksum calculation
00274   uint16_t crc = 0;
00275 
00276   for(size_t i = 0; i < output.size(); i++)
00277     crc = CRC16(crc,output[i]);
00278 
00279   //Add checksum to the output buffer
00280   output.push_back(crc & 0x00ff);
00281   output.push_back((crc & 0xff00) >> 8);
00282 
00283   //Send message to the module
00284   port->write(output);
00285   ros::Duration(0.1).sleep();
00286 
00287   std::vector<uint8_t> input;
00288   port->read(input, INPUT_BUFFER_SIZE);
00289 
00290   //Detect reached position response
00291   float act_position;
00292 
00293   if(input.size() > 0)
00294   {
00295     for(size_t i = 0; i < input.size(); i++)
00296     {
00297       if((input[i] == 0x07) && (input[i+1] == 0x95))
00298       {
00299         uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00300         act_position = IEEE_754_to_float(raw);
00301         ROS_INFO_STREAM("PG70 INFO: Actual position: " << act_position << " [mm]");
00302         return(act_position);
00303       }
00304       else if  ((input[i] == 0x05) && (input[i+1] == 0x94))
00305       {
00306         uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00307         act_position = IEEE_754_to_float(raw);
00308         ROS_INFO_STREAM("PG70 INFO: Actual position: " << act_position << " [mm]");
00309         return(act_position);
00310       }
00311     }
00312   }
00313 }
00314 
00315 void
00316 PG70_serial::getPeriodicPositionUpdate(serial::Serial *port, float update_period)
00317 {
00318   std::vector<uint8_t> output;
00319   output.push_back(0x05);                 //message from master to the module
00320   output.push_back(gripper_id_);          //gripper id
00321   output.push_back(0x06);                 //Data Length
00322   output.push_back(0x95);                 //Command get state
00323     
00324   unsigned int IEEE754_bytes[4];
00325   float_to_IEEE_754(update_period, IEEE754_bytes);
00326   
00327   output.push_back(IEEE754_bytes[0]);    //period first byte
00328   output.push_back(IEEE754_bytes[1]);    //period second byte
00329   output.push_back(IEEE754_bytes[2]);    //period third byte
00330   output.push_back(IEEE754_bytes[3]);    //period fourth byte
00331   output.push_back(0x07);
00332 
00333   //Checksum calculation
00334   uint16_t crc = 0;                      
00335 
00336   for(size_t i = 0; i < output.size(); i++)
00337     crc = CRC16(crc,output[i]);
00338 
00339   //Add checksum to the output buffer
00340   output.push_back(crc & 0x00ff);
00341   output.push_back((crc & 0xff00) >> 8);
00342   
00343   //Send get_state message to the module 
00344   port->write(output);   
00345 }
00346 
00347 void
00348 PG70_serial::stop(serial::Serial *port)
00349 {
00350   std::vector<uint8_t> output;
00351   output.push_back(0x05);         //message from master to module
00352   output.push_back(gripper_id_);  //module id
00353   output.push_back(0x01);         //D-Len
00354   output.push_back(0x91);         //Command stop
00355 
00356   //Checksum calculation
00357   uint16_t crc = 0;
00358 
00359   for(size_t i = 0; i < output.size(); i++)
00360     crc = CRC16(crc,output[i]);
00361 
00362   //Add checksum to the output buffer
00363   output.push_back(crc & 0x00ff);
00364   output.push_back((crc & 0xff00) >> 8);
00365 
00366   //Send message to the module
00367   port->write(output);
00368 }
00369 
00371 //CALLBACKS
00373 
00374 bool
00375 PG70_serial::referenceCallback(schunk_pg70::reference::Request &req,
00376                                schunk_pg70::reference::Response &res)
00377 {
00378   ROS_INFO("PG70: Reference Cmd recieved ");
00379   reference(com_port_);     
00380 }
00381 
00382 bool
00383 PG70_serial::setPositionCallback(schunk_pg70::set_position::Request &req,
00384                                  schunk_pg70::set_position::Response &res)
00385 {
00386   
00387   //Check if goal request respects position limits <0-69> mm
00388   if ((req.goal_position >= MIN_GRIPPER_POS_LIMIT) && (req.goal_position < MAX_GRIPPER_POS_LIMIT))
00389   {
00390     //Check if goal request respects velocity limits <0-83> mm/s
00391     if((req.goal_velocity > MIN_GRIPPER_VEL_LIMIT) && (req.goal_velocity < MAX_GRIPPER_VEL_LIMIT))
00392     {
00393       //Check if goal request respects acceleration limits <0-320> mm/s2  
00394       if((req.goal_acceleration > MIN_GRIPPER_ACC_LIMIT) && (req.goal_acceleration <= MAX_GRIPPER_ACC_LIMIT))
00395       {
00396         setPosition(com_port_,req.goal_position, req.goal_velocity, req.goal_acceleration);
00397         res.goal_accepted = true;
00398       }
00399       else
00400       {
00401         ROS_WARN("PG70: Goal acceleration rejected!");
00402         res.goal_accepted = false;
00403       }
00404     }
00405     else
00406     {
00407       ROS_WARN("PG70: Goal velocity rejected!");
00408       res.goal_accepted = false;
00409     }
00410   }
00411   else
00412   {
00413     ROS_WARN("PG70: Goal position rejected!");
00414     res.goal_accepted = false;
00415   }
00416 }
00417 
00418 bool
00419 PG70_serial::getPositionCallback(schunk_pg70::get_position::Request &req,
00420                                  schunk_pg70::get_position::Response &res)
00421 {
00422   ROS_INFO("PG70: Get position request recieved");
00423   res.actual_position = getPosition(com_port_);   
00424 }
00425 
00426 bool
00427 PG70_serial::getErrorCallback(schunk_pg70::get_error::Request &req,
00428                               schunk_pg70::get_error::Response &res)
00429 {
00430   ROS_INFO("PG70: Get state request recieved");
00431   res.error_code = getError(com_port_);
00432 }
00433 
00434 bool
00435 PG70_serial::acknowledgeErrorCallback(schunk_pg70::acknowledge_error::Request &req,
00436                                       schunk_pg70::acknowledge_error::Response &res)
00437 {
00438   ROS_INFO("PG70: Cmd Acknowledge error recieved");
00439   acknowledgeError(com_port_);
00440   ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00441   if(getError(com_port_) == 0x00)
00442     res.acknowledge_response = true;
00443   else
00444     res.acknowledge_response = false;
00445 }
00446 
00447 bool
00448 PG70_serial::stopCallback(schunk_pg70::stop::Request &req,
00449                           schunk_pg70::stop::Response &res)
00450 {
00451   ROS_INFO("PG70: Cmd Stop recieved");
00452   stop(com_port_);
00453   ros::Duration(WAIT_FOR_RESPONSE_INTERVAL).sleep();
00454   res.stop_result = getPosition(com_port_);
00455 }
00456 
00457 void
00458 PG70_serial::timerCallback(const ros::TimerEvent &event)
00459 {
00460   std::vector<uint8_t> input;
00461   com_port_->read(input,INPUT_BUFFER_SIZE);
00462  
00463   //Detect position reached response
00464   for(size_t i = 0; i < input.size(); i++)
00465   {
00466     if((input[i] == 0x05) && (input[i+1] == 0x94))
00467     {
00468       uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00469       act_position_ = IEEE_754_to_float(raw);
00470     }
00471     
00472     if((input[i] == 0x0F) && (input[i+1] == 0x95))
00473     {
00474       uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00475       act_position_ = IEEE_754_to_float(raw);
00476     }
00477     
00478     if((input[i] == 0x07) && (input[i+1] == 0x95))
00479     {
00480       uint8_t raw[4] = {input[i+5], input[i+4], input[i+3], input[i+2]};
00481       act_position_ = IEEE_754_to_float(raw);
00482     }
00483   }
00484   
00485   ROS_DEBUG_STREAM("Actual position: " << act_position_);
00486   
00487   //Publish TF
00488   pg70_joint_state_.header.stamp = ros::Time::now();
00489   pg70_joint_state_.name.clear();
00490   pg70_joint_state_.position.clear();
00491     
00492   pg70_joint_state_.name.push_back("pg70_finger1_joint");
00493   pg70_joint_state_.position.push_back(act_position_/URDF_SCALE_FACTOR);
00494   
00495   pg70_joint_state_.name.push_back("pg70_finger2_joint");
00496   pg70_joint_state_.position.push_back(act_position_/URDF_SCALE_FACTOR);
00497    
00498   joint_pub.publish(pg70_joint_state_);         
00499   
00500 }
00501 
00503 //ADDITIONAL FUNCTIONS
00505 
00506 float
00507 PG70_serial::IEEE_754_to_float(uint8_t *raw)
00508 {
00509   int sign = (raw[0] >> 7) ? -1 : 1;
00510   int8_t exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;
00511 
00512   uint32_t fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];
00513 
00514   float fraction = 0.5f;
00515   for (uint8_t ii = 0; ii < 24; ++ii)
00516     fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));
00517 
00518   float significand = sign * fraction;
00519 
00520   return ldexpf(significand, exponent);
00521 }
00522 
00523 void
00524 PG70_serial::float_to_IEEE_754(float position, unsigned int *output_array)
00525 {
00526   unsigned char *p_byte = (unsigned char*)(&position);
00527 
00528   for(size_t i = 0; i < sizeof(float); i++)
00529     output_array[i] = (static_cast<unsigned int>(p_byte[i]));
00530 }
00531 
00532 uint16_t
00533 PG70_serial::CRC16(uint16_t crc, uint16_t data)
00534 {
00535   const uint16_t tbl[256] = {
00536   0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
00537   0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
00538   0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
00539   0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
00540   0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
00541   0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
00542   0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
00543   0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
00544   0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
00545   0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
00546   0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
00547   0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
00548   0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
00549   0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
00550   0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
00551   0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
00552   0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
00553   0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
00554   0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
00555   0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
00556   0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
00557   0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
00558   0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
00559   0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
00560   0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
00561   0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
00562   0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
00563   0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
00564   0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
00565   0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
00566   0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
00567   0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
00568   };
00569 
00570   return(((crc & 0xFF00) >> 8)  ^ tbl[(crc & 0x00FF) ^ (data & 0x00FF)]);
00571 }
00572 }  //schunk_pg70
00573 
00574 #endif //PG70_RS232_CONTROL_LIB_CPP


schunk_pg70
Author(s): Frantisek Durovsky
autogenerated on Sat Jun 8 2019 20:52:12