Libbarrett: include/barrett/products/detail/motor

00001 
00026 /*
00027  * @file motor_puck-inl.h
00028  * @date 02/01/2011
00029  * @author Dan Cody
00030  *   
00031  */
00032 
00033 
00034 #include <barrett/os.h>
00035 
00036 
00037 namespace barrett {
00038 
00039 
00040 template<typename ResultType>
00041 int MotorPuck::MotorPositionParser<ResultType>::parse(int id, int propId, result_type* result, const unsigned char* data, size_t len) {
00042         if (len != 3 && len != 6) {
00043                 logMessage("%s: expected message length of 3 or 6, got message length of %d") % __func__ % len;
00044                 return 1;
00045         }
00046 
00047         *result = twentyTwoBit2<ResultType>(data[0], data[1], data[2]);
00048         return 0;
00049 }
00050 template<typename ResultType>
00051 int MotorPuck::SecondaryPositionParser<ResultType>::parse(int id, int propId, result_type* result, const unsigned char* data, size_t len) {
00052         if (len != 3) {
00053                 logMessage("%s: expected message length of 3, got message length of %d") % __func__ % len;
00054                 return 1;
00055         }
00056 
00057         *result = twentyTwoBit2<ResultType>(data[0], data[1], data[2]);
00058         return 0;
00059 }
00060 template<typename ResultType>
00061 int MotorPuck::CombinedPositionParser<ResultType>::parse(int id, int propId, result_type* result, const unsigned char* data, size_t len) {
00062         if (len == 6) {
00063                 boost::get<0>(*result) = twentyTwoBit2<ResultType>(data[0], data[1], data[2]);
00064                 boost::get<1>(*result) = twentyTwoBit2<ResultType>(data[3], data[4], data[5]);
00065         } else if (len == 3) {
00066                 boost::get<0>(*result) = twentyTwoBit2<ResultType>(data[0], data[1], data[2]);
00067                 boost::get<1>(*result) = std::numeric_limits<ResultType>::max();
00068         } else {
00069                 logMessage("%s: expected message length of 3 or 6, got message length of %d") % __func__ % len;
00070                 return 1;
00071         }
00072 
00073         return 0;
00074 }
00075 
00076 
00077 template<typename ResultType>
00078 ResultType MotorPuck::twentyTwoBit2(unsigned char msb, unsigned char middle, unsigned char lsb)
00079 {
00080         int intResult = 0;
00081         intResult |= ((long) msb << 16) & 0x003F0000;
00082         intResult |= ((long) middle << 8) & 0x0000FF00;
00083         intResult |= ((long) lsb) & 0x000000FF;
00084 
00085         if (intResult & 0x00200000) {  // If negative...
00086                 intResult |= ~((int)0x3fffff); // sign-extend
00087         }
00088 
00089         return intResult;
00090 }
00091 
00092 
00093 }