controlAllegroHand.cpp
Go to the documentation of this file.
00001 /*
00002  * control_AllegroHand.cpp
00003  *
00004  *  Created on: Nov 15, 2012
00005  *      Author: Seungsu KIM
00006  */
00007 
00008 #include "controlAllegroHand.h"
00009 #include <iostream>
00010 #include <math.h>
00011 #include <stdio.h>
00012 using namespace std;
00013 
00014 void PRINT_INFO(const char *msg)
00015 {
00016         cout << msg << endl;
00017 }
00018 
00019 controlAllegroHand::controlAllegroHand()
00020 {
00021         mEmergencyStop = false;
00022 
00023         mPWM_MAX[eJOINTNAME_INDEX_0] = PWM_LIMIT_ROLL;
00024         mPWM_MAX[eJOINTNAME_INDEX_1] = PWM_LIMIT_NEAR;
00025         mPWM_MAX[eJOINTNAME_INDEX_2] = PWM_LIMIT_MIDDLE;
00026         mPWM_MAX[eJOINTNAME_INDEX_3] = PWM_LIMIT_FAR;
00027 
00028         mPWM_MAX[eJOINTNAME_MIDDLE_0] = PWM_LIMIT_ROLL;
00029         mPWM_MAX[eJOINTNAME_MIDDLE_1] = PWM_LIMIT_NEAR;
00030         mPWM_MAX[eJOINTNAME_MIDDLE_2] = PWM_LIMIT_MIDDLE;
00031         mPWM_MAX[eJOINTNAME_MIDDLE_3] = PWM_LIMIT_FAR;
00032 
00033         mPWM_MAX[eJOINTNAME_PINKY_0] = PWM_LIMIT_ROLL;
00034         mPWM_MAX[eJOINTNAME_PINKY_1] = PWM_LIMIT_NEAR;
00035         mPWM_MAX[eJOINTNAME_PINKY_2] = PWM_LIMIT_MIDDLE;
00036         mPWM_MAX[eJOINTNAME_PINKY_3] = PWM_LIMIT_FAR;
00037 
00038         mPWM_MAX[eJOINTNAME_THUMB_0] = PWM_LIMIT_THUMB_ROLL;
00039         mPWM_MAX[eJOINTNAME_THUMB_1] = PWM_LIMIT_THUMB_NEAR;
00040         mPWM_MAX[eJOINTNAME_THUMB_2] = PWM_LIMIT_THUMB_MIDDLE;
00041         mPWM_MAX[eJOINTNAME_THUMB_3] = PWM_LIMIT_THUMB_FAR;
00042 
00043 //SAH01020003 Left
00044 mEncoderOffset[eJOINTNAME_INDEX_0] = -199;
00045 mEncoderOffset[eJOINTNAME_INDEX_1] = -66044;
00046 mEncoderOffset[eJOINTNAME_INDEX_2] = -632;
00047 mEncoderOffset[eJOINTNAME_INDEX_3] = 1517;
00048 mEncoderOffset[eJOINTNAME_MIDDLE_0] = 497;
00049 mEncoderOffset[eJOINTNAME_MIDDLE_1] = -66232;
00050 mEncoderOffset[eJOINTNAME_MIDDLE_2] = -190;
00051 mEncoderOffset[eJOINTNAME_MIDDLE_3] = 1031;
00052 mEncoderOffset[eJOINTNAME_PINKY_0] = -777;
00053 mEncoderOffset[eJOINTNAME_PINKY_1] = -66787;
00054 mEncoderOffset[eJOINTNAME_PINKY_2] = 548;
00055 mEncoderOffset[eJOINTNAME_PINKY_3] = 1358;
00056 mEncoderOffset[eJOINTNAME_THUMB_0] = -64803;
00057 mEncoderOffset[eJOINTNAME_THUMB_1] = -66346;
00058 mEncoderOffset[eJOINTNAME_THUMB_2] = 682;
00059 mEncoderOffset[eJOINTNAME_THUMB_3] = 574;
00060 
00061 mEncoderDirection[eJOINTNAME_INDEX_0] = 1;
00062 mEncoderDirection[eJOINTNAME_INDEX_1] = -1;
00063 mEncoderDirection[eJOINTNAME_INDEX_2] = 1;
00064 mEncoderDirection[eJOINTNAME_INDEX_3] = 1;
00065 mEncoderDirection[eJOINTNAME_MIDDLE_0] = 1;
00066 mEncoderDirection[eJOINTNAME_MIDDLE_1] = -1;
00067 mEncoderDirection[eJOINTNAME_MIDDLE_2] = 1;
00068 mEncoderDirection[eJOINTNAME_MIDDLE_3] = 1;
00069 mEncoderDirection[eJOINTNAME_PINKY_0] = 1;
00070 mEncoderDirection[eJOINTNAME_PINKY_1] = -1;
00071 mEncoderDirection[eJOINTNAME_PINKY_2] = 1;
00072 mEncoderDirection[eJOINTNAME_PINKY_3] = 1;
00073 mEncoderDirection[eJOINTNAME_THUMB_0] = -1;
00074 mEncoderDirection[eJOINTNAME_THUMB_1] = -1;
00075 mEncoderDirection[eJOINTNAME_THUMB_2] = 1;
00076 mEncoderDirection[eJOINTNAME_THUMB_3] = 1;
00077 
00078 mMotorDirection[eJOINTNAME_INDEX_0] = 1;
00079 mMotorDirection[eJOINTNAME_INDEX_1] = -1;
00080 mMotorDirection[eJOINTNAME_INDEX_2] = 1;
00081 mMotorDirection[eJOINTNAME_INDEX_3] = 1;
00082 mMotorDirection[eJOINTNAME_MIDDLE_0] = 1;
00083 mMotorDirection[eJOINTNAME_MIDDLE_1] = -1;
00084 mMotorDirection[eJOINTNAME_MIDDLE_2] = 1;
00085 mMotorDirection[eJOINTNAME_MIDDLE_3] = 1;
00086 mMotorDirection[eJOINTNAME_PINKY_0] = 1;
00087 mMotorDirection[eJOINTNAME_PINKY_1] = -1;
00088 mMotorDirection[eJOINTNAME_PINKY_2] = 1;
00089 mMotorDirection[eJOINTNAME_PINKY_3] = 1;
00090 mMotorDirection[eJOINTNAME_THUMB_0] = -1;
00091 mMotorDirection[eJOINTNAME_THUMB_1] = -1;
00092 mMotorDirection[eJOINTNAME_THUMB_2] = 1;
00093 mMotorDirection[eJOINTNAME_THUMB_3] = 1;
00094 // End SAH01020003 Left
00095 
00096 
00097 
00098 /* // SAH01010002 Right (Seungsu's Hand_ Orig)
00099         mEncoderOffset[eJOINTNAME_INDEX_0]  =  -1105;
00100         mEncoderOffset[eJOINTNAME_INDEX_1]  = -65301;
00101         mEncoderOffset[eJOINTNAME_INDEX_2]  =   -412;
00102         mEncoderOffset[eJOINTNAME_INDEX_3]  =    832;
00103         mEncoderOffset[eJOINTNAME_MIDDLE_0] =    327;
00104         mEncoderOffset[eJOINTNAME_MIDDLE_1] = -66828;
00105         mEncoderOffset[eJOINTNAME_MIDDLE_2] =     44;
00106         mEncoderOffset[eJOINTNAME_MIDDLE_3] =   -436;
00107         mEncoderOffset[eJOINTNAME_PINKY_0]  =    758;
00108         mEncoderOffset[eJOINTNAME_PINKY_1]  = -65920;
00109         mEncoderOffset[eJOINTNAME_PINKY_2]  =    -42;
00110         mEncoderOffset[eJOINTNAME_PINKY_3]  =   -180;
00111         mEncoderOffset[eJOINTNAME_THUMB_0]  =    412;
00112         mEncoderOffset[eJOINTNAME_THUMB_1]  =   -266;
00113         mEncoderOffset[eJOINTNAME_THUMB_2]  = -64487;
00114         mEncoderOffset[eJOINTNAME_THUMB_3]  = -65882;
00115 
00116         mEncoderDirection[eJOINTNAME_INDEX_0]  =  1;
00117         mEncoderDirection[eJOINTNAME_INDEX_1]  = -1;
00118         mEncoderDirection[eJOINTNAME_INDEX_2]  =  1;
00119         mEncoderDirection[eJOINTNAME_INDEX_3]  =  1;
00120         mEncoderDirection[eJOINTNAME_MIDDLE_0] =  1;
00121         mEncoderDirection[eJOINTNAME_MIDDLE_1] = -1;
00122         mEncoderDirection[eJOINTNAME_MIDDLE_2] =  1;
00123         mEncoderDirection[eJOINTNAME_MIDDLE_3] =  1;
00124         mEncoderDirection[eJOINTNAME_PINKY_0]  =  1;
00125         mEncoderDirection[eJOINTNAME_PINKY_1]  = -1;
00126         mEncoderDirection[eJOINTNAME_PINKY_2]  =  1;
00127         mEncoderDirection[eJOINTNAME_PINKY_3]  =  1;
00128         mEncoderDirection[eJOINTNAME_THUMB_0]  =  1;
00129         mEncoderDirection[eJOINTNAME_THUMB_1]  =  1;
00130         mEncoderDirection[eJOINTNAME_THUMB_2]  = -1;
00131         mEncoderDirection[eJOINTNAME_THUMB_3]  = -1;
00132 
00133         mMotorDirection[eJOINTNAME_INDEX_0]  = -1;
00134         mMotorDirection[eJOINTNAME_INDEX_1]  =  1;
00135         mMotorDirection[eJOINTNAME_INDEX_2]  = -1;
00136         mMotorDirection[eJOINTNAME_INDEX_3]  = -1;
00137         mMotorDirection[eJOINTNAME_MIDDLE_0] = -1;
00138         mMotorDirection[eJOINTNAME_MIDDLE_1] =  1;
00139         mMotorDirection[eJOINTNAME_MIDDLE_2] = -1;
00140         mMotorDirection[eJOINTNAME_MIDDLE_3] = -1;
00141         mMotorDirection[eJOINTNAME_PINKY_0]  = -1;
00142         mMotorDirection[eJOINTNAME_PINKY_1]  =  1;
00143         mMotorDirection[eJOINTNAME_PINKY_2]  = -1;
00144         mMotorDirection[eJOINTNAME_PINKY_3]  = -1;
00145         mMotorDirection[eJOINTNAME_THUMB_0]  = -1;
00146         mMotorDirection[eJOINTNAME_THUMB_1]  = -1;
00147         mMotorDirection[eJOINTNAME_THUMB_2]  =  1;
00148         mMotorDirection[eJOINTNAME_THUMB_3]  =  1; 
00149 */
00150 // end SAH01010002 Right        
00151         
00152 
00153 /* // SAH01020006 Right
00154         mEncoderOffset[eJOINTNAME_INDEX_0]  =  1465;
00155         mEncoderOffset[eJOINTNAME_INDEX_1]  = -65230;
00156         mEncoderOffset[eJOINTNAME_INDEX_2]  =   13;
00157         mEncoderOffset[eJOINTNAME_INDEX_3]  =    697;
00158         
00159         mEncoderOffset[eJOINTNAME_MIDDLE_0] =    1251;
00160         mEncoderOffset[eJOINTNAME_MIDDLE_1] = -66019;
00161         mEncoderOffset[eJOINTNAME_MIDDLE_2] =     420;
00162         mEncoderOffset[eJOINTNAME_MIDDLE_3] =   -413;
00163         
00164         mEncoderOffset[eJOINTNAME_PINKY_0]  =    377;
00165         mEncoderOffset[eJOINTNAME_PINKY_1]  = -63587;
00166         mEncoderOffset[eJOINTNAME_PINKY_2]  =    1017;
00167         mEncoderOffset[eJOINTNAME_PINKY_3]  =   -50;
00168         
00169         mEncoderOffset[eJOINTNAME_THUMB_0]  =    13;
00170         mEncoderOffset[eJOINTNAME_THUMB_1]  =   1294;
00171         mEncoderOffset[eJOINTNAME_THUMB_2]  = -65652;
00172         mEncoderOffset[eJOINTNAME_THUMB_3]  = -66046;
00173 //
00174         mEncoderDirection[eJOINTNAME_INDEX_0]  =  1;
00175         mEncoderDirection[eJOINTNAME_INDEX_1]  = -1;
00176         mEncoderDirection[eJOINTNAME_INDEX_2]  =  1;
00177         mEncoderDirection[eJOINTNAME_INDEX_3]  =  1;
00178         
00179         mEncoderDirection[eJOINTNAME_MIDDLE_0] =  1;
00180         mEncoderDirection[eJOINTNAME_MIDDLE_1] = -1;
00181         mEncoderDirection[eJOINTNAME_MIDDLE_2] =  1;
00182         mEncoderDirection[eJOINTNAME_MIDDLE_3] =  1;
00183         
00184         mEncoderDirection[eJOINTNAME_PINKY_0]  =  1;
00185         mEncoderDirection[eJOINTNAME_PINKY_1]  = -1;
00186         mEncoderDirection[eJOINTNAME_PINKY_2]  =  1;
00187         mEncoderDirection[eJOINTNAME_PINKY_3]  =  1;
00188         
00189         mEncoderDirection[eJOINTNAME_THUMB_0]  =  1;
00190         mEncoderDirection[eJOINTNAME_THUMB_1]  =  1;
00191         mEncoderDirection[eJOINTNAME_THUMB_2]  = -1;
00192         mEncoderDirection[eJOINTNAME_THUMB_3]  = -1;
00193 //
00194         mMotorDirection[eJOINTNAME_INDEX_0]  = 1;
00195         mMotorDirection[eJOINTNAME_INDEX_1]  =  -1;
00196         mMotorDirection[eJOINTNAME_INDEX_2]  = 1;
00197         mMotorDirection[eJOINTNAME_INDEX_3]  = 1;
00198         
00199         mMotorDirection[eJOINTNAME_MIDDLE_0] = 1;
00200         mMotorDirection[eJOINTNAME_MIDDLE_1] =  -1;
00201         mMotorDirection[eJOINTNAME_MIDDLE_2] = 1;
00202         mMotorDirection[eJOINTNAME_MIDDLE_3] = 1;
00203         
00204         mMotorDirection[eJOINTNAME_PINKY_0]  = 1;
00205         mMotorDirection[eJOINTNAME_PINKY_1]  =  -1;
00206         mMotorDirection[eJOINTNAME_PINKY_2]  = 1;
00207         mMotorDirection[eJOINTNAME_PINKY_3]  = 1;
00208         
00209         mMotorDirection[eJOINTNAME_THUMB_0]  = 1;
00210         mMotorDirection[eJOINTNAME_THUMB_1]  = 1;
00211         mMotorDirection[eJOINTNAME_THUMB_2]  =  -1;
00212         mMotorDirection[eJOINTNAME_THUMB_3]  =  -1; 
00213 
00214 // end SAH01020006 Right 
00215 */
00216 
00217 }
00218 
00219 controlAllegroHand::~controlAllegroHand()
00220 {
00221         PRINT_INFO("Setting System OFF");
00222         _writeDeviceMsg(ID_CMD_SET_SYSTEM_OFF, ID_DEVICE_MAIN, ID_COMMON);
00223         usleep(10000);
00224 
00225         if(CAN_Close(CanHandle))
00226         {
00227                 PRINT_INFO("Error in CAN_Close()");
00228         }
00229 }
00230 
00231 void controlAllegroHand::init(int mode)
00232 {
00233 
00234         unsigned char data[8];
00235         int ret;
00236 
00237         PRINT_INFO("Opening CAN device");
00238 
00239         CanHandle = LINUX_CAN_Open("/dev/pcan32", O_RDWR);
00240         if (!CanHandle)
00241         {
00242                 PRINT_INFO("Error in CAN_Open()");
00243         }
00244 
00245         char txt[VERSIONSTRING_LEN];
00246         ret = CAN_VersionInfo(CanHandle, txt);
00247         if (!ret)
00248         {
00249                 PRINT_INFO(txt);
00250         }
00251         else {
00252                 PRINT_INFO("Error getting CAN_VersionInfo()");
00253         }
00254 
00255         PRINT_INFO("Initializing CAN device");
00256         // init to an user defined bit rate
00257         ret = CAN_Init(CanHandle, CAN_BAUD_1M, CAN_INIT_TYPE_ST);
00258         if (ret)
00259         {
00260                 PRINT_INFO("Error in CAN_Init()");
00261         }
00262 
00263         // system off
00264         _writeDeviceMsg(ID_CMD_SET_SYSTEM_OFF, ID_DEVICE_MAIN, ID_COMMON);
00265         usleep(10000);
00266 
00267         PRINT_INFO("Setting loop period = 3 ms");
00268         data[0] = (char)(ALLEGRO_CONTROL_TIME_INTERVAL * 1000.);
00269         _writeDeviceMsg(ID_CMD_SET_PERIOD, ID_DEVICE_MAIN, ID_COMMON, 1, data );
00270         usleep(100000);
00271 
00272         PRINT_INFO("Setting task mode");
00273         _writeDeviceMsg(ID_CMD_SET_MODE_TASK, ID_DEVICE_MAIN, ID_COMMON);
00274         usleep(100000);
00275 
00276         PRINT_INFO("Setting joint query command");
00277         _writeDeviceMsg(ID_CMD_QUERY_STATE_DATA, ID_DEVICE_MAIN, ID_COMMON);
00278         usleep(100000);
00279 
00280         PRINT_INFO("Setting System ON");
00281         _writeDeviceMsg(ID_CMD_SET_SYSTEM_ON, ID_DEVICE_MAIN, ID_COMMON);
00282         usleep(100000);
00283 
00284         PRINT_INFO("Clear the CAN buffer");
00285 
00286         while( LINUX_CAN_Read_Timeout(CanHandle, &read_msg, 0) != 0 )
00287         {
00288                 usleep(1000);
00289         }
00290 
00291         // write initial read command
00292         _writeDeviceMsg(ID_CMD_QUERY_STATE_DATA, ID_DEVICE_MAIN, ID_COMMON);
00293 }
00294 
00295 int controlAllegroHand::update(void)
00296 {
00297         _readDevices();
00298         _writeDevices();
00299 
00300         if(mEmergencyStop == true)
00301         {
00302                 return -1;
00303         }
00304         else
00305         {
00306                 return 0;
00307         }
00308 }
00309 
00310 int  controlAllegroHand::command(const short& cmd, const int& arg)
00311 {
00312         return 0;
00313 }
00314 
00315 void controlAllegroHand::setTorque(double *torque)
00316 {
00317         for(int i=0; i<DOF_JOINTS; i++)
00318         {
00319                 desired_torque[i] = torque[i];
00320         }
00321 }
00322 
00323 void controlAllegroHand::getJointInfo(double *position, double *torque)
00324 {
00325         for(int i=0; i<DOF_JOINTS; i++)
00326         {
00327                 position[i] = curr_position[i];
00328                 torque[i] = curr_torque[i];
00329         }
00330 }
00331 
00332 
00333 void controlAllegroHand::_readDevices()
00334 {
00335         double q[4];
00336         char lID;
00337         int ret = 0;
00338         int itr = 0;
00339         static int errorcnt = 0;
00340 
00341         while( true )
00342         {
00343                 //ret=LINUX_CAN_Read_Timeout(CanHandle, &read_msg, 2900); // timeout in microsecond
00344                 ret=LINUX_CAN_Read_Timeout(CanHandle, &read_msg, 0); // 0 : polling
00345 
00346                 if (ret)
00347                 {
00348                         //PRINT_INFO("CAN communication error (reading timeout)");
00349                         break;
00350                 }
00351                 else
00352                 {
00353                         lID  = _parseCANMsg(read_msg.Msg, q);
00354                         if( (lID >= ID_DEVICE_SUB_01) && (lID <= ID_DEVICE_SUB_04) )
00355                         {
00356                                 for(int i=0; i<4; i++)
00357                                 {
00358                                         curr_position[i+4*(lID-ID_DEVICE_SUB_01)] = q[i];
00359                                 }
00360                                 itr++;
00361                         }
00362                         if( lID < 0 )
00363                         {
00364                                 mEmergencyStop = true;
00365                         }
00366                 }
00367         }
00368         if( itr < 4)
00369         {
00370                 errorcnt++;
00371                 if( errorcnt > 5 ){
00372                         cout << "read error" << endl;
00373                         mEmergencyStop = true;
00374                 }
00375         }
00376         else{
00377                 errorcnt = 0;
00378         }
00379 
00380 
00381 }
00382 
00383 void controlAllegroHand::_writeDevices()
00384 {
00385         double pwmDouble[DOF_JOINTS];
00386         short pwm[DOF_JOINTS];
00387         unsigned char data[8];
00388 
00389         // convert to torque to pwm
00390         for(int i=0; i<DOF_JOINTS; i++ ){
00391                 pwmDouble[i] =  desired_torque[i] *1.0 * (double)mMotorDirection[i] *800.0;
00392 
00393                 mPWM_MAX[i] = 800.0;
00394 
00395                 // limitation should be less than 800
00396                 if     ( pwmDouble[i] >  mPWM_MAX[i] )
00397                 {
00398                         pwmDouble[i] =  mPWM_MAX[i];
00399                         cout <<i << " max"<< endl;
00400                 }
00401                 else if( pwmDouble[i] < -mPWM_MAX[i] ) {
00402                         pwmDouble[i] = -mPWM_MAX[i];
00403                         cout <<i<< " min"<< endl;
00404                 }
00405 
00406                 pwm[i] = (short)pwmDouble[i];
00407         }
00408 
00409 
00410         for(int findex=0; findex<4; findex++ ){
00411                 data[0] = (unsigned char)( (pwm[0+findex*4] >> 8) & 0x00ff);
00412                 data[1] = (unsigned char)(  pwm[0+findex*4]       & 0x00ff);
00413                 data[2] = (unsigned char)( (pwm[1+findex*4] >> 8) & 0x00ff);
00414                 data[3] = (unsigned char)(  pwm[1+findex*4]       & 0x00ff);
00415                 data[4] = (unsigned char)( (pwm[2+findex*4] >> 8) & 0x00ff);
00416                 data[5] = (unsigned char)(  pwm[2+findex*4]       & 0x00ff);
00417                 data[6] = (unsigned char)( (pwm[3+findex*4] >> 8) & 0x00ff);
00418                 data[7] = (unsigned char)(  pwm[3+findex*4]       & 0x00ff);
00419 
00420                 _writeDeviceMsg( (unsigned long)(ID_CMD_SET_TORQUE_1 + findex), ID_DEVICE_MAIN, ID_COMMON, 8, data);
00421         }
00422 
00423         // send message to call joint position and torque query
00424         _writeDeviceMsg(ID_CMD_QUERY_STATE_DATA, ID_DEVICE_MAIN, ID_COMMON);
00425 
00426 }
00427 
00428 void controlAllegroHand::_writeDeviceMsg(unsigned long command, unsigned long from, unsigned long to, BYTE len, unsigned char *data)
00429 {
00430         TPCANMsg msg1;
00431         long Txid;
00432 
00433         Txid = ((unsigned long)command<<6) | ((unsigned long)to <<3) | ((unsigned long)from);
00434         msg1.ID  = Txid;
00435         msg1.MSGTYPE  = MSGTYPE_STANDARD;
00436         msg1.LEN  = len;
00437 
00438         for(BYTE i=0; i<len; i++)
00439         {
00440                 msg1.DATA[i] = data[i];
00441         }
00442 
00443         if(CAN_Write(CanHandle, &msg1))
00444         {
00445                 cout << "CAN communication error (write)" << endl;
00446         }
00447 
00448 }
00449 
00450 void controlAllegroHand::_writeDeviceMsg(unsigned long command, unsigned long from, unsigned long to)
00451 {
00452         _writeDeviceMsg(command, from, to, 0, NULL);
00453 }
00454 
00455 
00456 
00457 char controlAllegroHand::_parseCANMsg(TPCANMsg &read_msg,  double *values)
00458 {
00459         char cmd, src, to;
00460         int len;
00461         unsigned char tmpdata[8];
00462         int tmppos[4];
00463         int lIndexBase;
00464 
00465         cmd = (char)( (read_msg.ID >> 6) & 0x1f );
00466         to  = (char)( (read_msg.ID >> 3) & 0x07 );
00467         src = (char)( read_msg.ID & 0x07 );
00468         len = (int)( read_msg.LEN );
00469         for(int nd=0; nd<len; nd++)
00470                 tmpdata[nd] = read_msg.DATA[nd];
00471 
00472         switch (cmd)
00473         {
00474         case ID_CMD_QUERY_STATE_DATA:
00475                 if (src >= ID_DEVICE_SUB_01 && src <= ID_DEVICE_SUB_04)
00476                 {
00477 
00478                         tmppos[0] = (int)(tmpdata[0] | (tmpdata[1] << 8));
00479                         tmppos[1] = (int)(tmpdata[2] | (tmpdata[3] << 8));
00480                         tmppos[2] = (int)(tmpdata[4] | (tmpdata[5] << 8));
00481                         tmppos[3] = (int)(tmpdata[6] | (tmpdata[7] << 8));
00482 
00483                         lIndexBase = 4*(src-ID_DEVICE_SUB_01);
00484 
00485                         //values[0] = (double)mEncoderDirection[lIndexBase+0] * (double)(tmppos[0] - 32768 - mEncoderOffset[lIndexBase+0]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00486                         //values[1] = (double)mEncoderDirection[lIndexBase+1] * (double)(tmppos[1] - 32768 - mEncoderOffset[lIndexBase+1]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00487                         //values[2] = (double)mEncoderDirection[lIndexBase+2] * (double)(tmppos[2] - 32768 - mEncoderOffset[lIndexBase+2]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00488                         //values[3] = (double)mEncoderDirection[lIndexBase+3] * (double)(tmppos[3] - 32768 - mEncoderOffset[lIndexBase+3]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00489                         values[0] = (double)(mEncoderDirection[lIndexBase+0] *tmppos[0] - 32768 - mEncoderOffset[lIndexBase+0]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00490                         values[1] = (double)(mEncoderDirection[lIndexBase+1] *tmppos[1] - 32768 - mEncoderOffset[lIndexBase+1]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00491                         values[2] = (double)(mEncoderDirection[lIndexBase+2] *tmppos[2] - 32768 - mEncoderOffset[lIndexBase+2]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00492                         values[3] = (double)(mEncoderDirection[lIndexBase+3] *tmppos[3] - 32768 - mEncoderOffset[lIndexBase+3]) * ( 333.3 / 65536.0 ) * ( M_PI/180.0);
00493 
00494                         return src;
00495 
00496                 }
00497                 else
00498                 {
00499                         cout << "No subdevice match!" << endl;
00500                         return -1;
00501                 }
00502 
00503                 break;
00504                 //TODO: Implement this
00505         case ID_CMD_QUERY_CONTROL_DATA:
00506                 return 0;
00507                 break;
00508         default:
00509                 printf("unknown command %d, src %d, to %d, len %d \n", cmd, src, to, len);
00510                 /*
00511                 for(int nd=0; nd<len; nd++)
00512                 {
00513                         printf("%d \n ", tmpdata[nd]);
00514                 }
00515                 */
00516                 return -1;
00517                 break;
00518         }
00519 
00520 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


AllegroHand
Author(s): Alex Alspach (SimLab), Seungsu Kim (EPFL)
autogenerated on Fri Mar 22 2013 04:47:25