25#include <yarp/os/Time.h>
26#include <yarp/dev/PolyDriver.h>
27#include <ace/config.h>
28#include <ace/Log_Msg.h>
29#include <yarp/os/LogStream.h>
30#include <yarp/os/Log.h>
45#include <yarp/os/Log.h>
46#include <yarp/os/LogStream.h>
47#include <yarp/dev/ControlBoardInterfacesImpl.h>
53 #pragma warning(once:4355)
61using namespace yarp::os;
68 yDebug(
" S:%d R:%d Ch:%d M:%d\n",
getSender(m),
getRcp(m), m.getData()[0]&0x80, m.getData()[0]&0x7F);
74 yError(
"%s not yet implemented for CanBusMotionControl\n", txt);
81 yWarning(
"%s not yet implemented for CanBusMotionControl\n", txt);
88 yError(
"%s has been deprecated for CanBusMotionControl\n", txt);
96bool validate(Bottle &input, Bottle &
out,
const std::string &key1,
const std::string &txt,
int size)
98 Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
101 yError(
"%s not found\n", key1.c_str());
106 yError(
"%s incorrect number of entries\n", key1.c_str());
115 Bottle &tmp=input.findGroup(key1.c_str(), txt.c_str());
122 yError(
"%s incorrect number of entries\n", key1.c_str());
165 inline void getStats(
int &it,
double &dT,
double &min,
double &max)
289 case icubCanProto_controlmode_idle:
290 yInfo (
"[%d] board %d MODE_IDLE \r\n", net, addr);
292 case icubCanProto_controlmode_position:
293 yInfo (
"[%d] board %d MODE_POSITION \r\n", net, addr);
295 case icubCanProto_controlmode_velocity:
296 yInfo (
"[%d] board %d MODE_VELOCITY \r\n", net, addr);
298 case icubCanProto_controlmode_torque:
299 yInfo (
"[%d] board %d MODE_TORQUE \r\n", net, addr);
301 case icubCanProto_controlmode_impedance_pos:
302 yInfo (
"[%d] board %d MODE_IMPEDANCE_POS \r\n", net, addr);
304 case icubCanProto_controlmode_impedance_vel:
305 yInfo (
"[%d] board %d MODE_IMPEDANCE_VEL \r\n", net, addr);
308 yError (
"[%d] board %d MODE_UNKNOWN \r\n", net, addr);
358 PolyDriver polyDriver;
369 bool initialize (yarp::os::Searchable &config);
376 bool addMessage (
int id,
int joint,
int msg_id);
447 std::recursive_mutex *backdoor_mutex;
456 canEchoEnabled=
false;
457 backdoor_mutex=
nullptr;
466 canEchoEnabled = echo;
469 virtual void onRead(Bottle &b);
478 double dval[6] = {0,0,0,0,0,0};
480 std::lock_guard<std::recursive_mutex> lck(*backdoor_mutex);
483 int commandId = b.get(0).asInt32();
486 static double timePrev=Time::now();
487 static int count_timeout=0;
488 static int count_saturation_i[6]={0,0,0,0,0,0};
489 static int count_saturation=0;
490 double timeNow=Time::now();
491 double diff = timeNow - timePrev;
496 yWarning(
"PORT: %s **** TIMEOUT : %f COUNT: %d \n", this->getName().c_str(), diff, count_timeout);
498 timePrev=Time::now();
503 dval[0] = b.get(1).asFloat64();
504 dval[1] = b.get(2).asFloat64();
505 dval[2] = b.get(3).asFloat64();
506 dval[3] = b.get(4).asFloat64();
507 dval[4] = b.get(5).asFloat64();
511 dval[0] = b.get(1).asFloat64();
512 dval[1] = b.get(2).asFloat64();
513 dval[2] = b.get(3).asFloat64();
514 dval[3] = b.get(4).asFloat64();
515 dval[4] = b.get(5).asFloat64();
516 dval[5] = b.get(6).asFloat64();
519 dval[0] = b.get(6).asFloat64();
520 dval[1] = b.get(7).asFloat64();
527 dval[0] = b.get(1).asFloat64();
528 dval[1] = b.get(2).asFloat64();
529 dval[2] = b.get(3).asFloat64();
535 yWarning(
"Got unexpected message on backdoor: %s\n", this->getName().c_str());
540 if (bus && ownerSensor)
543 int boardId = ownerSensor->
getId();
544 short int val[6] = {0,0,0,0,0,0};
545 static double curr_time = Time::now();
549 if (dval[i] > fullScale)
551 if (Time::now() - curr_time > 2)
553 yWarning(
"PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", this->getName().c_str(), i, dval[i], count_saturation);
554 curr_time = Time::now();
559 else if (dval[i] < -fullScale)
561 if (Time::now() - curr_time > 2)
563 yWarning(
"PORT: %s **** SATURATED CH:%d : %+4.4f COUNT: %d \n", this->getName().c_str(), i, dval[i], count_saturation);
564 curr_time = Time::now();
566 dval[i] = -fullScale;
569 val[i] = (
short int)(dval[i] / fullScale * 0x7fff)+0x8000;
573 fakeId = 0x300 + (boardId<<4)+ 0x0A;
597 yError(
"Echobuffer full \n");
602 fakeId = 0x300 + (boardId<<4)+ 0x0B;
626 yError(
"Echobuffer full \n");
637 if (estim_parameters!=0)
656 helper =
new ControlBoardHelper(njoints, aMap, angToEncs, 0, 0);
657 maxHwStep =
new double [jointsNum];
658 maxUserStep =
new double [jointsNum];
659 for (
int i=0; i<jointsNum; i++)
663 helper->posA2E(_stepLimit[i], i, hw_ang, hw_i);
664 maxHwStep[hw_i] = fabs(hw_ang);
665 maxUserStep[i] = _stepLimit[i];
674 double hw_val = (std::min)((std::max)(hw_ref_value, hw_minval), hw_maxval);
679CanBusMotionControl::torqueControlHelper::torqueControlHelper(
int njoints,
double* p_angleToEncoders,
double* p_newtonsTosens )
682 newtonsToSensor =
new double [jointsNum];
683 angleToEncoders =
new double [jointsNum];
685 if (p_angleToEncoders!=0)
686 memcpy(angleToEncoders, p_angleToEncoders,
sizeof(
double)*jointsNum);
688 for (
int i=0; i<jointsNum; i++) {angleToEncoders[i]=1.0;}
690 if (p_newtonsTosens!=0)
691 memcpy(newtonsToSensor, p_newtonsTosens,
sizeof(
double)*jointsNum);
693 for (
int i=0; i<jointsNum; i++) {newtonsToSensor[i]=1.0;}
699 torqueSensorId =
new int [jointsNum];
700 torqueSensorChan =
new int [jointsNum];
701 maximumTorque =
new double [jointsNum];
702 newtonsToSensor =
new double [jointsNum];
704 memcpy(torqueSensorId,
id,
sizeof(
int)*jointsNum);
706 memset(torqueSensorId, 0,
sizeof(
int)*jointsNum);
708 memcpy(torqueSensorChan, chan,
sizeof(
int)*jointsNum);
710 memset(torqueSensorChan, 0,
sizeof(
int)*jointsNum);
712 memcpy(maximumTorque, maxTrq,
sizeof(
double)*jointsNum);
714 memset(maximumTorque, 0,
sizeof(
double)*jointsNum);
716 memcpy(newtonsToSensor, newtons2sens,
sizeof(
double)*jointsNum);
718 memset(newtonsToSensor, 0,
sizeof(
double)*jointsNum);
725 isVirtualSensor=
false;
727 status=IAnalogSensor::AS_OK;
758 scaleFactor=
new double[channels];
760 for (i=0; i<channels; i++) scaleFactor[i]=1;
763 useCalibration=useCalib;
764 isVirtualSensor=isVirtualS;
787 std::lock_guard<std::mutex> lck(mtx);
794 if (status!=IAnalogSensor::AS_OK)
798 case IAnalogSensor::AS_OVF:
803 case IAnalogSensor::AS_ERROR:
808 case IAnalogSensor::AS_TIMEOUT:
822 for(
int k=0;k<data->
size();k++)
840bool TBR_AnalogSensor::decode16(
const unsigned char *msg,
int id,
double *
data)
843 const char groupId=(
id&0x00f);
852 data[k]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
853 if (useCalibration==1)
855 data[k]=
data[k]*scaleFactor[k]/float(0x8000);
864 data[k+3]=(((
unsigned short)(msg[2*k+1]))<<8)+msg[2*k]-0x8000;
865 if (useCalibration==1)
867 data[k+3]=
data[k+3]*scaleFactor[k+3]/float(0x8000);
879 yError(
"Got unexpected class 0x3 msg(s)\n");
890bool TBR_AnalogSensor::decode8(
const unsigned char *msg,
int id,
double *
data)
892 const char groupId=(
id&0x00f);
899 for(
int k=0;k<=6;k++)
905 for(
int k=0;k<=7;k++)
916 yWarning(
"Got unexpected class 0x3 msg(s): groupId 0x%x\n", groupId);
934 std::lock_guard<std::mutex> lck(mtx);
936 double timeNow=Time::now();
937 for (
unsigned int buff_num=0; buff_num<2; buff_num++)
939 unsigned int size = 0;
940 CanBuffer* buffer_pointer=0;
952 for (i = 0; i < size; i++)
955 unsigned int msgid=0;
956 unsigned char *buff=0;
957 CanMessage& m = (*buffer_pointer)[i];
962 status=IAnalogSensor::AS_OK;
963 const char type=((msgid&0x700)>>8);
964 const char id=((msgid&0x0f0)>>4);
970 timeStamp=Time::now();
974 ret=decode8(buff, msgid,
data->getBuffer());
975 status=IAnalogSensor::AS_OK;
980 ret=decode16(buff, msgid,
data->getBuffer());
981 status=IAnalogSensor::AS_OK;
985 if (len==7 && buff[6] == 1)
987 status=IAnalogSensor::AS_OVF;
991 status=IAnalogSensor::AS_ERROR;
993 ret=decode16(buff, msgid,
data->getBuffer());
1005 if (timeStamp+0.1<timeNow)
1007 status=IAnalogSensor::AS_TIMEOUT;
1018 if (list.get(1).asInt32()==1)
1030 if ((list.get(k+1).asInt32())==1)
1048 sprintf(tmp,
"Pid%d", j);
1050 Bottle &xtmp = pidsGroup.findGroup(tmp);
1051 myPid[j].kp = xtmp.get(1).asFloat64();
1052 myPid[j].kd = xtmp.get(2).asFloat64();
1053 myPid[j].ki = xtmp.get(3).asFloat64();
1055 myPid[j].max_int = xtmp.get(4).asFloat64();
1056 myPid[j].max_output = xtmp.get(5).asFloat64();
1058 myPid[j].scale = xtmp.get(6).asFloat64();
1059 myPid[j].offset = xtmp.get(7).asFloat64();
1061 if (xtmp.size()==10)
1063 myPid[j].stiction_up_val = xtmp.get(8).asFloat64();
1064 myPid[j].stiction_down_val = xtmp.get(9).asFloat64();
1076 sprintf(tmp,
"TPid%d", j);
1078 Bottle &xtmp = pidsGroup.findGroup(tmp);
1079 myPid[j].kp = xtmp.get(1).asFloat64();
1080 myPid[j].kd = xtmp.get(2).asFloat64();
1081 myPid[j].ki = xtmp.get(3).asFloat64();
1083 myPid[j].max_int = xtmp.get(4).asFloat64();
1084 myPid[j].max_output = xtmp.get(5).asFloat64();
1086 myPid[j].scale = xtmp.get(6).asFloat64();
1087 myPid[j].offset = xtmp.get(7).asFloat64();
1089 if (xtmp.size()==10)
1091 myPid[j].stiction_up_val = xtmp.get(8).asFloat64();
1092 myPid[j].stiction_down_val = xtmp.get(9).asFloat64();
1102 if (!
validate(pidsGroup, xtmp,
"kp",
"Pid kp parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].kp = xtmp.get(j+1).asFloat64();
1103 if (!
validate(pidsGroup, xtmp,
"kd",
"Pid kd parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].kd = xtmp.get(j+1).asFloat64();
1104 if (!
validate(pidsGroup, xtmp,
"ki",
"Pid kp parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].ki = xtmp.get(j+1).asFloat64();
1105 if (!
validate(pidsGroup, xtmp,
"maxInt",
"Pid maxInt parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].max_int = xtmp.get(j+1).asFloat64();
1106 if (!
validate(pidsGroup, xtmp,
"maxOutput",
"Pid maxOutput parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].max_output = xtmp.get(j+1).asFloat64();
1107 if (!
validate(pidsGroup, xtmp,
"shift",
"Pid shift parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].scale = xtmp.get(j+1).asFloat64();
1108 if (!
validate(pidsGroup, xtmp,
"ko",
"Pid ko parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].
offset = xtmp.get(j+1).asFloat64();
1109 if (!
validate(pidsGroup, xtmp,
"stictionUp",
"Pid stictionUp",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].stiction_up_val = xtmp.get(j+1).asFloat64();
1110 if (!
validate(pidsGroup, xtmp,
"stictionDwn",
"Pid stictionDwn",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) myPid[j].stiction_down_val = xtmp.get(j+1).asFloat64();
1113 xtmp = pidsGroup.findGroup(
"kff");
1116 for (j=0; j<
_njoints; j++) myPid[j].kff = xtmp.get(j+1).asFloat64();
1120 for (j=0; j<
_njoints; j++) myPid[j].kff = 0;
1130 if (!
validate(pidsGroup, xtmp,
"stiffness",
"Pid stiffness parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].stiffness = xtmp.get(j+1).asFloat64();
1131 if (!
validate(pidsGroup, xtmp,
"damping",
"Pid damping parameter",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].damping = xtmp.get(j+1).asFloat64();
1140 if (!
validate(pidsGroup, xtmp,
"debug0",
"debug0",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[0] = xtmp.get(j+1).asFloat64();
1141 if (!
validate(pidsGroup, xtmp,
"debug1",
"debug1",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[1] = xtmp.get(j+1).asFloat64();
1142 if (!
validate(pidsGroup, xtmp,
"debug2",
"debug2",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[2] = xtmp.get(j+1).asFloat64();
1143 if (!
validate(pidsGroup, xtmp,
"debug3",
"debug3",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[3] = xtmp.get(j+1).asFloat64();
1144 if (!
validate(pidsGroup, xtmp,
"debug4",
"debug4",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[4] = xtmp.get(j+1).asFloat64();
1145 if (!
validate(pidsGroup, xtmp,
"debug5",
"debug5",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[5] = xtmp.get(j+1).asFloat64();
1146 if (!
validate(pidsGroup, xtmp,
"debug6",
"debug6",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[6] = xtmp.get(j+1).asFloat64();
1147 if (!
validate(pidsGroup, xtmp,
"debug7",
"debug7",
_njoints+1))
return false;
for (j=0; j<
_njoints; j++) vals[j].
data[7] = xtmp.get(j+1).asFloat64();
1153 if (!
p.check(
"GENERAL",
"section for general motor control parameters")) {
1154 yError(
"Cannot understand configuration parameters\n");
1158 std::string dbg_string =
p.toString().c_str();
1160 int nj =
p.findGroup(
"GENERAL").check(
"Joints",Value(1),
1161 "Number of degrees of freedom").asInt32();
1165 bool useRawJointEncoderData =
false;
1166 bool useRawMotorEncoderData =
false;
1167 Value use_jnt_raw =
p.findGroup(
"GENERAL").find(
"useRawEncoderData");
1168 Value use_mot_raw =
p.findGroup(
"GENERAL").find(
"useRawMotorEncoderData");
1170 if(use_jnt_raw.isNull())
1172 useRawJointEncoderData =
false;
1176 if(!use_jnt_raw.isBool())
1178 yError(
"useRawEncoderData bool param is different from accepted values (true / false). Assuming false\n");
1179 useRawJointEncoderData =
false;
1183 useRawJointEncoderData = use_jnt_raw.asBool();
1184 if(useRawJointEncoderData)
1186 yWarning(
"canBusMotionControl using raw data from encoders! Be careful See 'useRawEncoderData' param in config file \n");
1187 yWarning(
"DO NOT USE OR CALIBRATE THE ROBOT IN THIS CONFIGURATION! \n");
1188 yWarning(
"CHECK IF THE FAULT BUTTON IS PRESSED and press ENTER to continue \n");
1194 if(use_mot_raw.isNull())
1196 useRawMotorEncoderData =
false;
1200 if(!use_mot_raw.isBool())
1202 yError(
"useRawEncoderData bool param is different from accepted values (true / false). Assuming false\n");
1203 useRawMotorEncoderData =
false;
1207 useRawMotorEncoderData = use_mot_raw.asBool();
1208 if(useRawMotorEncoderData)
1210 yWarning(
"canBusMotionControl using raw data from motor encoders");
1215 Value use_limitedPWM =
p.findGroup(
"GENERAL").find(
"useLimitedPWM");
1216 if(use_limitedPWM.isNull())
1222 if(!use_limitedPWM.isBool())
1233 Bottle& canGroup =
p.findGroup(
"CAN");
1237 if (canGroup.check(
"CanForcedDeviceNum"))
1239 _networkN=canGroup.find(
"CanForcedDeviceNum").asInt32();
1242 _networkN=canGroup.check(
"CanDeviceNum",Value(-1),
"numeric identifier of device").asInt32();
1247 yError(
"can network id not valid, skipping device\n");
1251 _my_address=canGroup.check(
"CanMyAddress",Value(0),
1252 "numeric identifier of my address").asInt32();
1255 "polling period").asInt32();
1257 _timeout=canGroup.check(
"CanTimeout",Value(20),
"timeout period").asInt32();
1259 _txTimeout=canGroup.check(
"CanTxTimeout", Value(20),
"tx timeout").asInt32();
1260 _rxTimeout=canGroup.check(
"CanRxTimeout", Value(20),
"rx timeout").asInt32();
1265 if (canGroup.check(
"CanTxQueueSize"))
1266 _txQueueSize=canGroup.find(
"CanTxQueueSize").asInt32();
1270 if (canGroup.check(
"CanRxQueueSize"))
1271 _rxQueueSize=canGroup.find(
"CanRxQueueSize").asInt32();
1275 Bottle &canAddresses = canGroup.findGroup(
"CanAddresses",
1276 "a list of numeric identifiers");
1277 if (canAddresses.isNull())
1279 yError(
"CanAddresses group not found in config file\n");
1282 for (i = 1; i < canAddresses.size(); i++) {
1283 _destinations[i-1] = (
unsigned char)(canAddresses.get(i).asInt32());
1287 Bottle& general =
p.findGroup(
"GENERAL");
1290 if (!
validate(general, xtmp,
"AxisMap",
"a list of reordered indices for the axes", nj+1))
1293 for (i = 1; i < xtmp.size(); i++)
1294 _axisMap[i-1] = xtmp.get(i).asInt32();
1296 if (!
validate(general, xtmp,
"AxisName",
"a list of strings representing the axes names", nj + 1))
1301 for (i = 1; i < xtmp.size(); i++)
1304 if (!
validate(general, xtmp,
"AxisType",
"a list of strings representing the axes types (revolute/prismatic)", nj + 1))
1309 for (i = 1; i < xtmp.size(); i++)
1312 if (!
validate(general, xtmp,
"Encoder",
"a list of scales for the joint encoders", nj+1))
1315 int test = xtmp.size();
1316 for (i = 1; i < xtmp.size(); i++)
1319 if (!
validate_optional(general, xtmp,
"Rotor",
"a list of scales for the rotor encoders", nj+1))
1321 yWarning(
"Rotor: Using default value = 1\n");
1322 for(i=1;i<nj+1; i++)
1327 int test = xtmp.size();
1328 for (i = 1; i < xtmp.size(); i++)
1332 if (!
validate(general, xtmp,
"fullscalePWM",
" list of scales for the fullscalePWM conversion factor", nj + 1))
1334 yError(
"fullscalePWM param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
1337 for (i = 1; i < xtmp.size(); i++)
1340 if (!
validate(general, xtmp,
"ampsToSensor",
"a list of scales for the ampsToSensor conversion factor", nj + 1))
1342 yError(
"ampsToSensor param not found in config file. Please update robot configuration files or contact https://github.com/robotology/icub-support");
1345 for (i = 1; i < xtmp.size(); i++)
1348 if (!
validate(general, xtmp,
"Zeros",
"a list of offsets for the zero point", nj+1))
1351 for (i = 1; i < xtmp.size(); i++)
1352 _zeros[i-1] = xtmp.get(i).asFloat64();
1354 if (useRawJointEncoderData)
1356 for(i=1;i<nj+1; i++)
1367 if (useRawMotorEncoderData)
1369 for(i=1;i<nj+1; i++)
1378 if (!
validate_optional(general, xtmp,
"TorqueId",
"a list of associated joint torque sensor ids", nj+1))
1380 yWarning(
"TorqueId: Using default value = 0 (disabled)\n");
1381 for(i=1;i<nj+1; i++)
1386 for (i = 1; i < xtmp.size(); i++)
_torqueSensorId[i-1] = xtmp.get(i).asInt32();
1389 if (!
validate_optional(general, xtmp,
"TorqueChan",
"a list of associated joint torque sensor channels", nj+1))
1391 yWarning(
"TorqueChan: Using default value = 0 (disabled)\n");
1392 for(i=1;i<nj+1; i++)
1397 for (i = 1; i < xtmp.size(); i++)
_torqueSensorChan[i-1] = xtmp.get(i).asInt32();
1400 if (!
validate(general, xtmp,
"TorqueMax",
"full scale value for a joint torque sensor", nj+1))
1406 for (i = 1; i < xtmp.size(); i++)
1416 Bottle posPidsGroup;
1417 posPidsGroup=
p.findGroup(
"POS_PIDS",
"Position Pid parameters new format");
1418 if (posPidsGroup.isNull() ==
false)
1420 yInfo(
"Position Pids section found, new format\n");
1423 yError() <<
"Position Pids section: error detected in parameters syntax";
1428 yInfo(
"Position Pids successfully loaded\n");
1433 Bottle posPidsGroup2=
p.findGroup(
"PIDS",
"Position Pid parameters old format");
1434 if (posPidsGroup2.isNull()==
false)
1436 yWarning(
"Position Pids section found, old format\n");
1441 yError () <<
"no PIDS group found in config file, returning";
1449 Bottle trqPidsGroup;
1450 Bottle trqPidsOldGroup;
1451 Bottle trqControlGroup;
1452 trqPidsGroup =
p.findGroup(
"TRQ_PIDS",
"Torque Pid parameters new format");
1453 trqPidsOldGroup =
p.findGroup(
"TORQUE_PIDS",
"Torque Pid parameters old format");
1454 trqControlGroup =
p.findGroup(
"TORQUE_CONTROL",
"Torque Control parameters");
1456 if (trqPidsGroup.isNull()==
false)
1458 yInfo(
"TRQ_PIDS: Torque Pids section found, new format\n");
1461 yError () <<
"TRQ_PIDS: error detected in parameters syntax";
1466 xtmp = trqPidsGroup.findGroup(
"kbemf");
1467 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = 0; }
1469 xtmp = trqPidsGroup.findGroup(
"ktau");
1470 {
for (j=0;j<nj;j++) this->
_ktau[j] = 1; }
1472 xtmp = trqPidsGroup.findGroup(
"filterType");
1475 xtmp = trqPidsGroup.findGroup(
"controlUnits");
1481 else if (trqPidsOldGroup.isNull()==
false)
1483 yWarning (
"TORQUE_PIDS: Torque Pids section found, old format\n");
1486 yError () <<
"TORQUE_PIDS: error detected in parameters syntax";
1491 xtmp = trqPidsGroup.findGroup(
"kbemf");
1492 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = 0; }
1494 xtmp = trqPidsGroup.findGroup(
"ktau");
1495 {
for (j=0;j<nj;j++) this->
_ktau[j] = 1; }
1497 xtmp = trqPidsGroup.findGroup(
"filterType");
1500 xtmp = trqPidsGroup.findGroup(
"controlUnits");
1506 else if (trqControlGroup.isNull()==
false)
1508 yInfo(
"TORQUE_CONTROL section found\n");
1511 yError () <<
"TORQUE_CONTROL: error detected in parameters syntax";
1516 xtmp = trqControlGroup.findGroup(
"kbemf");
1518 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = xtmp.get(j+1).asFloat64();}
1520 {
for (j=0;j<nj;j++) this->
_bemfGain[j] = 0; yWarning (
"TORQUE_PIDS: 'kbemf' param missing");}
1522 xtmp = trqControlGroup.findGroup(
"ktau");
1524 {
for (j=0;j<nj;j++) this->
_ktau[j] = xtmp.get(j+1).asFloat64();}
1526 {
for (j=0;j<nj;j++) this->
_ktau[j] = 1.0; yWarning (
"TORQUE_PIDS: 'ktau' param missing");}
1528 xtmp = trqControlGroup.findGroup(
"filterType");
1530 {
for (j=0;j<nj;j++) this->
_filterType[j] = xtmp.get(j+1).asInt32();}
1532 {
for (j=0;j<nj;j++) this->
_filterType[j] = 3; yWarning (
"TORQUE_PIDS: 'filterType' param missing");}
1534 xtmp = trqControlGroup.findGroup(
"controlUnits");
1539 else {yError() <<
"invalid controlUnits value";
return false;}
1543 yError (
"TORQUE_PIDS: 'controlUnits' param missing. Cannot continue");
1553 yWarning(
"Torque control parameters not found for part %s, skipping...\n",
_networkName.c_str());
1559 Bottle &debugGroup=
p.findGroup(
"DEBUG_PARAMETERS",
"DEBUG parameters");
1560 if (debugGroup.isNull()==
false)
1562 yDebug(
"DEBUG_PARAMETERS section found\n");
1565 yError(
"DEBUG_PARAMETERS section: error detected in parameters syntax\n");
1570 yInfo(
"DEBUG_PARAMETERS section: parameters successfully loaded\n");
1575 yDebug(
"DEBUG parameters section NOT found, skipping...\n");
1581 Bottle &impedanceGroup=
p.findGroup(
"IMPEDANCE",
"IMPEDANCE parameters");
1582 if (impedanceGroup.isNull()==
false)
1584 yDebug(
"IMPEDANCE parameters section found\n");
1587 yError(
"IMPEDANCE section: error detected in parameters syntax\n");
1592 yInfo(
"IMPEDANCE section: parameters successfully loaded\n");
1597 yDebug(
"IMPEDANCE parameters section NOT found, skipping...\n");
1614 Bottle &limits=
p.findGroup(
"LIMITS");
1615 if (limits.isNull())
1617 yError(
"Group LIMITS not found in configuration file\n");
1622 if (!
validate(limits, xtmp,
"motorOverloadCurrents",
"a list of current limits", nj+1))
1624 for(i=1;i<xtmp.size(); i++)
_currentLimits[i-1]=xtmp.get(i).asFloat64();
1627 if (!
validate_optional(limits, xtmp,
"motorPwmLimit",
"a list of motor PWM limits", nj + 1))
1629 yWarning(
"motorPwmLimit: Using default motorPwmLimit=1333\n");
1630 for (i = 1; i<nj + 1; i++)
1635 for (i = 1; i < xtmp.size(); i++)
1640 yError() <<
"motorPwmLimit should be a positive value";
1647 if (!
validate_optional(limits, xtmp,
"jntVelMax",
"the maximum velocity command of a joint", nj + 1))
1649 yWarning(
"jntVelMax: Using default jntVelMax=100 deg/s\n");
1650 for (i = 1; i<nj + 1; i++)
1655 for (i = 1; i<xtmp.size(); i++)
1660 yError() <<
"Invalid jntVelMax parameter <0\n";
1666 if (!
validate_optional(limits, xtmp,
"maxPosStep",
"the maximum amplitude of a position direct step", nj+1))
1668 yWarning(
"maxPosStep: Using default MaxPosStep=10 degs\n");
1669 for(i=1;i<nj+1; i++)
1674 for(i=1;i<xtmp.size(); i++)
1676 _maxStep[i-1]=xtmp.get(i).asFloat64();
1679 yError () <<
"Invalid MaxPosStep parameter <0\n";
1686 if (!
validate(limits, xtmp,
"jntPosMax",
"a list of maximum angles (in degrees)", nj+1))
1688 for(i=1;i<xtmp.size(); i++)
_limitsMax[i-1]=xtmp.get(i).asFloat64();
1691 if (!
validate(limits, xtmp,
"jntPosMin",
"a list of minimum angles (in degrees)", nj+1))
1693 for(i=1;i<xtmp.size(); i++)
_limitsMin[i-1]=xtmp.get(i).asFloat64();
1694 for (i = 0; i<nj; i++)
1698 yError(
"invalid limit on joint %d : Max value (%f) < Min value(%f)\n", i,
_limitsMax[i],
_limitsMin[i]);
1704 Bottle &velocityGroup=
p.findGroup(
"VELOCITY");
1705 if (!velocityGroup.isNull())
1708 if (!
validate(velocityGroup, xtmp,
"Shifts",
"a list of shifts to be used in the vmo control", nj+1))
1710 yWarning(
"Shifts: Using default Shifts=4\n");
1711 for(i=1;i<nj+1; i++)
1716 for(i=1;i<xtmp.size(); i++)
1722 if (!
validate(velocityGroup, xtmp,
"Timeout",
"a list of timeout to be used in the vmo control", nj+1))
1724 yWarning(
"Timeout: Using default Timeout=1000, i.e 1s\n");
1725 for(i=1;i<nj+1; i++)
1730 for(i=1;i<xtmp.size(); i++)
1736 if (!
validate_optional(velocityGroup, xtmp,
"JNT_speed_estimation",
"a list of shift factors used by the firmware joint speed estimator", nj+1))
1738 yWarning(
"JNT_speed_estimation: Using default value=5\n");
1739 for(i=1;i<nj+1; i++)
1744 for(i=1;i<xtmp.size(); i++)
1745 _estim_params[i-1].jnt_Vel_estimator_shift = xtmp.get(i).asInt32();
1750 if (!
validate_optional(velocityGroup, xtmp,
"MOT_speed_estimation",
"a list of shift factors used by the firmware motor speed estimator", nj+1))
1752 yWarning(
"MOT_speed_estimation: Using default value=5\n");
1753 for(i=1;i<nj+1; i++)
1758 for(i=1;i<xtmp.size(); i++)
1759 _estim_params[i-1].mot_Vel_estimator_shift = xtmp.get(i).asInt32();
1764 if (!
validate_optional(velocityGroup, xtmp,
"JNT_accel_estimation",
"a list of shift factors used by the firmware joint speed estimator", nj+1))
1766 yWarning(
"JNT_accel_estimation: Using default value=5\n");
1767 for(i=1;i<nj+1; i++)
1772 for(i=1;i<xtmp.size(); i++)
1773 _estim_params[i-1].jnt_Acc_estimator_shift = xtmp.get(i).asInt32();
1778 if (!
validate_optional(velocityGroup, xtmp,
"MOT_accel_estimation",
"a list of shift factors used by the firmware motor speed estimator", nj+1))
1780 yWarning(
"MOT_accel_estimation: Using default value=5\n");
1781 for(i=1;i<nj+1; i++)
1786 for(i=1;i<xtmp.size(); i++)
1787 _estim_params[i-1].mot_Acc_estimator_shift = xtmp.get(i).asInt32();
1793 yWarning(
"A suitable value for [VELOCITY] Shifts was not found. Using default Shifts=4\n");
1794 for(i=1;i<nj+1; i++)
1797 yWarning(
"A suitable value for [VELOCITY] Timeout was not found. Using default Timeout=1000, i.e 1s.\n");
1798 for(i=1;i<nj+1; i++)
1801 yWarning(
"A suitable value for [VELOCITY] speed estimation was not found. Using default shift factor=5.\n");
1802 for(i=1;i<nj+1; i++)
1812 if (!canGroup.findGroup(
"broadcast_pos").isNull())
1814 xtmp=canGroup.findGroup(
"broadcast_pos");
1818 if (!canGroup.findGroup(
"broadcast_pid").isNull())
1820 xtmp=canGroup.findGroup(
"broadcast_pid");
1823 if (!canGroup.findGroup(
"broadcast_fault").isNull())
1825 xtmp=canGroup.findGroup(
"broadcast_fault");
1828 if (!canGroup.findGroup(
"broadcast_current").isNull())
1830 xtmp=canGroup.findGroup(
"broadcast_current");
1833 if (!canGroup.findGroup(
"broadcast_overflow").isNull())
1835 xtmp=canGroup.findGroup(
"broadcast_overflow");
1838 if (!canGroup.findGroup(
"broadcast_canprint").isNull())
1840 xtmp=canGroup.findGroup(
"broadcast_canprint");
1843 if (!canGroup.findGroup(
"broadcast_vel_acc").isNull())
1845 xtmp=canGroup.findGroup(
"broadcast_vel_acc");
1849 if (!canGroup.findGroup(
"broadcast_pid_err").isNull())
1851 xtmp=canGroup.findGroup(
"broadcast_pid_err");
1855 if (!canGroup.findGroup(
"broadcast_rotor_pos").isNull())
1857 xtmp=canGroup.findGroup(
"broadcast_rotor_pos");
1861 if (!canGroup.findGroup(
"broadcast_rotor_vel_acc").isNull())
1863 xtmp=canGroup.findGroup(
"broadcast_rotor_vel_acc");
1868 yError(
"Invalid configuration file, check broadcast_* parameters\n");
1928 _zeros = allocAndCheck<double>(nj);
1936 _ktau=allocAndCheck<double>(nj);
1937 _maxStep=allocAndCheck<double>(nj);
1944 _pids=allocAndCheck<Pid>(nj);
1945 _tpids=allocAndCheck<Pid>(nj);
1963 memset(
_maxStep, 0,
sizeof(
double)*nj);
1978 for(
int j=0;j<nj;j++)
1988 checkAndDestroy<double>(
_zeros);
2003 checkAndDestroy<double>(
_ktau);
2008 checkAndDestroy<std::string>(
_axisName);
2009 checkAndDestroy<std::string>(
_axisType);
2011 checkAndDestroy<Pid>(
_pids);
2012 checkAndDestroy<Pid>(
_tpids);
2036 _timeout = CAN_TIMEOUT;
2037 _polling_interval = CAN_POLLING_INTERVAL;
2044 memset (_destinations, 0,
sizeof(
unsigned char) *
CAN_MAX_CARDS);
2053 _bcastRecvBuffer = NULL;
2055 _error_status =
true;
2072 polyDriver.open(config);
2073 if (!polyDriver.isValid())
2075 yError(
"Could not instantiate CAN device\n");
2079 polyDriver.view(iCanBus);
2080 polyDriver.view(iBufferFactory);
2081 polyDriver.view(iCanErrors);
2083 if ((iCanBus==0) || (iBufferFactory==0))
2085 yError(
"CanBusResources::initialize() could not get ICanBus or ICanBufferFactory interface");
2089 ret=initialize(params);
2095 yTrace(
"Calling CanBusResources::initialize\n");
2105 _error_status =
true;
2111 _velShifts[k] = (1 << ((
int) _velShifts[k]));
2120 _jointNames =
new std::string[_njoints];
2121 for (
int i = 0; i < _njoints; i++) _jointNames[i] = parms.
_axisName[i];
2131 printf(
"printing destinations and inverted map\n");
2132 for(
int jj=0;jj<_njoints;jj+=2)
2134 _destInv[_destinations[jj/2]]=jj;
2135 printf(
"%d %d\n",jj,_destinations[jj/2]);
2138 _bcastRecvBuffer = allocAndCheck<BCastBufferElement> (_njoints);
2139 for (
int j=0; j<_njoints ;j++)
2141 _bcastRecvBuffer[j]._update_e=Time::now();
2142 _bcastRecvBuffer[j]._position_joint.resetStats();
2143 _bcastRecvBuffer[j]._position_rotor.resetStats();
2144 _bcastRecvBuffer[j]._speed_rotor.resetStats();
2145 _bcastRecvBuffer[j]._accel_rotor.resetStats();
2149 iCanBus->canSetBaudRate(_speed);
2152#if ICUB_CANMASKS_STRICT_FILTER
2153 yInfo(
"using ICUB_CANMASKS_STRICT_FILTER option\n");
2157 unsigned int sent_addr = 0x000 + ( 0x000<<4) + _destinations[j];
2158 unsigned int recv_addr = 0x000 + (_destinations[j]<<4) + 0x000;
2159 iCanBus->canIdAdd(sent_addr);
2160 iCanBus->canIdAdd(recv_addr);
2162 yDebug(
"class 0 set\n");
2167 unsigned int start_addr = 0x100 + (_destinations[j]<<4) + 0x000;
2168 unsigned int end_addr = 0x100 + (_destinations[j]<<4) + 0x00F;
2169 for (i = start_addr; i < end_addr; i++)
2170 iCanBus->canIdAdd(i);
2172 yDebug(
"class 1 set\n");
2175 yInfo(
"opening all CAN masks\n");
2177 for (i = 0x000; i < 0x0ff; i++)
2178 iCanBus->canIdAdd(i);
2179 yDebug(
"class 0 set\n");
2182 for (i = 0x100; i < 0x1ff; i++)
2183 iCanBus->canIdAdd(i);
2184 yDebug(
"class 1 set\n");
2188 for (i = 0x200; i < 0x2ff; i++)
2189 iCanBus->canIdAdd(i);
2190 yDebug(
"class 2 set\n");
2193 for (i = 0x300; i < 0x3ff; i++)
2194 iCanBus->canIdAdd(i);
2195 yDebug(
"class 3 set\n");
2197 _readBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2198 _writeBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2199 _replyBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2200 _echoBuffer=iBufferFactory->createBuffer(
BUF_SIZE);
2201 yDebug(
"Can read/write buffers created, buffer size: %d\n",
BUF_SIZE);
2203 requestsQueue =
new RequestsQueue(_njoints, ICUBCANPROTO_POL_MC_CMD_MAXNUM);
2207 yInfo(
"CanBusResources::initialized correctly\n");
2220 checkAndDestroy<BCastBufferElement> (_bcastRecvBuffer);
2224 iBufferFactory->destroyBuffer(_readBuffer);
2225 iBufferFactory->destroyBuffer(_writeBuffer);
2226 iBufferFactory->destroyBuffer(_replyBuffer);
2227 iBufferFactory->destroyBuffer(_echoBuffer);
2231 if (requestsQueue!=0)
2233 delete requestsQueue;
2243 if (_jointNames != 0)
2245 delete[] _jointNames;
2260 res=iCanBus->canRead(_readBuffer, messages, &_readMessages);
2272 unsigned int id = _destinations[joint/2] & 0x0f;
2275 if ((joint % 2) == 1)
2278 _writeBuffer[_writeMessages].setId(
id);
2279 _writeBuffer[_writeMessages].setLen(1);
2280 _writeBuffer[_writeMessages].getData()[0]=
data;
2288 unsigned char *
data=_writeBuffer[_writeMessages].getData();
2289 unsigned int destId= _destinations[joint/2] & 0x0f;
2292 if ((joint % 2) == 1)
2295 _writeBuffer[_writeMessages].setId(destId);
2296 _writeBuffer[_writeMessages].setLen(1);
2303 requestsQueue->append(rq);
2310 if (_writeMessages < 1)
2313 unsigned int sent=0;
2316 for(
unsigned int k=0; k<_writeMessages;k++)
2322 res=iCanBus->canWrite(_writeBuffer, _writeMessages, &sent);
2335 const unsigned char *
data=m.getData();
2341 int ret = sprintf (_printBuffer,
"class: %2d s: %2x d: %2x c: %1d msg: %3d (%x) ",
2345 ((
data[0] & 0x80)==0)?0:1,
2351 ret += sprintf (_printBuffer+ret,
"x: ");
2355 for (j = 1; j < len; j++)
2357 ret += sprintf (_printBuffer+ret,
"%x ",
data[j]);
2360 printf(
"%s", _printBuffer);
2370 yDebug(
"CAN: write buffer\n");
2371 for (j = 0; j < _writeMessages; j++)
2372 printMessage (_writeBuffer[j]);
2374 yDebug(
"CAN: reply buffer\n");
2375 for (j = 0; j < _writeMessages; j++)
2376 printMessage (_replyBuffer[j]);
2378 yDebug(
"CAN: echo buffer\n");
2379 for (j = 0; j < _echoMessages; j++)
2380 printMessage (_echoBuffer[j]);
2382 yDebug(
"CAN: read buffer\n");
2383 for (j = 0; j < _readMessages; j++)
2384 printMessage (_readBuffer[j]);
2385 yDebug(
"CAN: -------------\n");
2392PeriodicThread(0.01),
2393ImplementPositionControl(this),
2394ImplementVelocityControl(this),
2395ImplementPidControl(this),
2396ImplementEncodersTimed(this),
2397ImplementControlCalibration(this),
2398ImplementAmplifierControl(this),
2399ImplementControlLimits(this),
2400ImplementTorqueControl(this),
2401ImplementImpedanceControl(this),
2402ImplementControlMode(this),
2403ImplementPositionDirect(this),
2404ImplementInteractionMode(this),
2405ImplementMotorEncoders(this),
2406ImplementMotor(this),
2407ImplementRemoteVariables(this),
2408ImplementAxisInfo(this),
2409ImplementPWMControl(this),
2410ImplementCurrentControl(this)
2427 mServerLogger = NULL;
2449 yTrace(
"Opening CanBusMotionControl Control\n");
2450 std::string dbg_string = config.toString().c_str();
2457 if(!
p.fromConfig(config))
2460 yError() <<
"One or more errors found while parsing device configuration";
2464 std::string str=config.toString().c_str();
2466 prop.fromString(str.c_str());
2467 networkName=config.find(
"NetworkId").asString();
2468 canDevName=config.find(
"canbusdevice").asString();
2469 if (canDevName==
"") canDevName=config.findGroup(
"CAN").find(
"canbusdevice").asString();
2470 if(canDevName==
"") { yError() <<
"cannot find parameter 'canbusdevice'\n";
return false;}
2471 prop.unput(
"device");
2472 prop.unput(
"subdevice");
2473 prop.put(
"device", canDevName.c_str());
2474 std::string canPhysDevName = config.find(
"physDevice").asString();
2475 if (canPhysDevName==
"") canPhysDevName = config.findGroup(
"CAN").find(
"physDevice").asString();
2476 prop.put(
"physDevice",canPhysDevName.c_str());
2477 prop.put(
"canDeviceNum",
p._networkN);
2478 prop.put(
"canTxTimeout",
p._txTimeout);
2479 prop.put(
"canRxTimeout",
p._rxTimeout);
2480 if (
p._txQueueSize!=-1)
2481 prop.put(
"canTxQueueSize",
p._txQueueSize);
2482 if (
p._rxQueueSize!=-1)
2483 prop.put(
"canRxQueueSize",
p._rxQueueSize);
2490 yError() <<
"Unable to uninitialize CAN driver";
2499 double *tmpZeros =
new double [
p._njoints];
2500 double *tmpOnes =
new double [
p._njoints];
2501 for (
int i=0; i<
p._njoints; i++)
2507 ImplementPositionControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2508 ImplementVelocityControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2516 ImplementPidControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder, NULL,
p._newtonsToSensor,
p._ampsToSensor,
p._dutycycleToPwm);
2518 ImplementEncodersTimed::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2520 ImplementMotorEncoders::initialize(
p._njoints,
p._axisMap,
p._rotToEncoder, tmpZeros);
2522 ImplementMotor::initialize(
p._njoints,
p._axisMap);
2524 ImplementControlCalibration::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2526 ImplementAmplifierControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2528 ImplementControlLimits::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2530 ImplementControlMode::initialize(
p._njoints,
p._axisMap);
2531 ImplementTorqueControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros,
p._newtonsToSensor,
p._ampsToSensor,
nullptr,
nullptr,
nullptr);
2536 else {yError() <<
"Invalid _torqueControlUnits value: %d" <<
p._torqueControlUnits;
return false;}
2539 ImplementImpedanceControl::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros,
p._newtonsToSensor);
2540 ImplementPositionDirect::initialize(
p._njoints,
p._axisMap,
p._angleToEncoder,
p._zeros);
2541 ImplementInteractionMode::initialize(
p._njoints,
p._axisMap);
2543 ImplementAxisInfo::initialize(
p._njoints,
p._axisMap);
2544 ImplementRemoteVariables::initialize(
p._njoints,
p._axisMap);
2545 ImplementCurrentControl::initialize(
p._njoints,
p._axisMap,
p._ampsToSensor);
2546 ImplementPWMControl::initialize(
p._njoints,
p._axisMap,
p._dutycycleToPwm);
2548 delete [] tmpZeros; tmpZeros=0;
2549 delete [] tmpOnes; tmpOnes=0;
2557 _ref_accs = allocAndCheck<double>(
p._njoints);
2562 for (
int i = 0; i<
p._njoints; i++)
2572 yarp::os::Time::delay(0.005);
2573 setPids(VOCAB_PIDTYPE_POSITION,
p._pids);
2575 if (
p._torqueControlEnabled==
true)
2578 yarp::os::Time::delay(0.005);
2579 setPids(VOCAB_PIDTYPE_TORQUE,
p._tpids);
2581 for (
int i=0; i<
p._njoints; i++)
2583 MotorTorqueParameters ps;
2584 ps.bemf =
p._bemfGain[i];
2585 this->setMotorTorqueParams(i,ps);
2587 yarp::os::Time::delay(0.002);
2589 yarp::os::Time::delay(0.002);
2595 for (
int j=0; j<
p._njoints; j++)
2597 yarp::os::Time::delay(0.001);
2603 for (
int j = 0; j<
p._njoints; j++)
2605 yarp::os::Time::delay(0.002);
2606 setRefSpeed(j, 10.0);
2610 for (
int j=0; j<
p._njoints; j++)
2612 yarp::os::Time::delay(0.001);
2613 setImpedance(j,
p._impedance_params[j].stiffness,
p._impedance_params[j].damping);
2617 for(i = 0; i <
p._njoints; i++)
2619 yarp::os::Time::delay(0.001);
2624 for(i = 0; i <
p._njoints; i++)
2626 yarp::os::Time::delay(0.001);
2627 setLimits(i,
p._limitsMin[i],
p._limitsMax[i]);
2628 yarp::os::Time::delay(0.001);
2629 setVelLimits(i, 0,
p._maxJntCmdVelocity[i]);
2630 yarp::os::Time::delay(0.001);
2631 setMaxCurrent(i,
p._currentLimits[i]);
2635 for(i = 0; i <
p._njoints; i++)
2637 yarp::os::Time::delay(0.001);
2639 yarp::os::Time::delay(0.001);
2641 yarp::os::Time::delay(0.001);
2642 setPWMLimit(i,
p._motorPwmLimits[i]);
2646 for(i = 0; i <
p._njoints; i++)
2648 yarp::os::Time::delay(0.001);
2650 p._estim_params[i].jnt_Acc_estimator_shift,
2651 p._estim_params[i].mot_Vel_estimator_shift,
2652 p._estim_params[i].mot_Acc_estimator_shift);
2657 for (i = 0; i <
p._njoints; i++)
2659 yarp::os::Time::delay(0.001);
2660 setControlMode(i, VOCAB_CM_IDLE);
2662 const Bottle &analogList=config.findGroup(
"analog").tail();
2664 if (analogList.size()>0)
2666 for(
int k=0;k<analogList.size();k++)
2668 std::string analogId=analogList.get(k).asString().c_str();
2673 analogSensors.push_back(as);
2681 PeriodicThread::setPeriod((
double)
p._polling_interval/1000.0);
2682 PeriodicThread::start();
2689 for (
int j=0; j<
p._njoints; j++)
2691 yarp::os::Time::delay(0.001);
2693 if (b==
false) yError() <<
"Unable to read firmware version";
2698#ifdef ICUB_CANPROTOCOL_STRICT
2702 PeriodicThread::stop();
2704 yError() <<
"checkFirmwareVersions() failed. CanBusMotionControl::open returning false,";
2709 yWarning(
"*********************************************************************************\n");
2710 yWarning(
"**** WARNING: ICUB_CANPROTOCOL_STRICT not defined, skipping firmware check! *****\n");
2711 yWarning(
"*********************************************************************************\n");
2715 yDebug() <<
"CanBusMotionControl::open returned true\n";
2719bool CanBusMotionControl::readFullScaleAnalog(
int analog_address,
int ch,
double* fullScale)
2722 int destId=0x0200|analog_address;
2734 bool full_scale_read=
false;
2741 unsigned int currId=m.getId();
2742 if (currId==(0x0200|(analog_address<<4)))
2743 if (m.getLen()==4 &&
2744 m.getData()[0]==0x18 &&
2747 *fullScale=m.getData()[2]<<8 | m.getData()[3];
2748 full_scale_read=
true;
2752 yarp::os::Time::delay(0.002);
2755 while(timeout<32 && full_scale_read==
false);
2757 if (full_scale_read==
false)
2759 yError() <<
"Trying to get fullscale data from sensor: no answer received or message lost (ch:" << ch <<
")";
2766TBR_AnalogSensor *CanBusMotionControl::instantiateAnalog(yarp::os::Searchable& config, std::string deviceid)
2774 Bottle &analogConfig=config.findGroup(deviceid.c_str());
2775 if (analogConfig.size()>0)
2777 yDebug(
"Initializing analog device %s\n", deviceid.c_str());
2782 bool isVirtualSensor=
false;
2783 char analogId=analogConfig.find(
"CanAddress").asInt32();
2784 char analogFormat=analogConfig.find(
"Format").asInt32();
2785 int analogChannels=analogConfig.find(
"Channels").asInt32();
2786 int analogCalibration=analogConfig.find(
"UseCalibration").asInt32();
2789 if (analogConfig.check(
"PortName"))
2791 isVirtualSensor =
true;
2792 std::string virtualPortName = analogConfig.find(
"PortName").asString();
2793 bool canEchoEnabled = analogConfig.find(
"CanEcho").asInt32();
2796 std::string rn(
"/");
2797 rn += config.find(
"robotName").asString().c_str();
2799 rn+=virtualPortName;
2800 analogSensor->
backDoor->open(rn.c_str());
2804 switch (analogFormat)
2814 int destId=0x0200|analogSensor->
getId();
2816 if (analogConfig.check(
"Period"))
2818 int period=analogConfig.find(
"Period").asInt32();
2832 if (analogChannels==16 && analogFormat==8)
2843 else if (analogChannels==6 && analogFormat==16 && isVirtualSensor==
false)
2846 if (analogCalibration==1)
2849 for (
int ch=0; ch<6; ch++)
2858 if (attempts>0) yWarning(
"Trying to get fullscale data from sensor: channel recovered (ch:%d)\n", ch);
2862 yarp::os::Time::delay(0.020);
2866 yError(
"Trying to get fullscale data from sensor %s: all attempts failed (ch:%d)\n", deviceid.c_str(), ch);
2867 yError(
"Device %s cannot be opened.\n", deviceid.c_str());
2874 yDebug(
"Sensor Fullscale Id %#4X: %f %f %f %f %f %f ",analogId,
2904 else if (analogChannels==6 && analogFormat==16 && isVirtualSensor==
true)
2911 if (cfgId == analogId)
2913 for (
int ch=0; ch<6; ch++)
2927 yDebug(
"Sensor Fullscale Id %#4X: %f %f %f %f %f %f",analogId,
2937 return analogSensor;
2944 if (analogSensor->
isOpen())
2947 int destId=0x0200|analogSensor->
getId();
2958 yDebug(
"---> Len:%d %x %x %x\n",
2983 setControlMode(i, VOCAB_CM_IDLE);
2994 PeriodicThread::stop ();
2996 ImplementPositionControl::uninitialize();
2997 ImplementVelocityControl::uninitialize();
3002 ImplementPidControl::uninitialize();
3003 ImplementEncodersTimed::uninitialize();
3004 ImplementMotorEncoders::uninitialize();
3005 ImplementControlCalibration::uninitialize();
3006 ImplementAmplifierControl::uninitialize();
3007 ImplementControlLimits::uninitialize();
3009 ImplementControlMode::uninitialize();
3010 ImplementTorqueControl::uninitialize();
3011 ImplementImpedanceControl::uninitialize();
3012 ImplementPositionDirect::uninitialize();
3013 ImplementInteractionMode::uninitialize();
3014 ImplementRemoteVariables::uninitialize();
3015 ImplementAxisInfo::uninitialize();
3016 ImplementCurrentControl::uninitialize();
3017 ImplementPWMControl::uninitialize();
3020 std::list<TBR_AnalogSensor *>::iterator it=analogSensors.begin();
3021 while(it!=analogSensors.end())
3055void CanBusMotionControl::handleBroadcasts()
3059 double before=Time::now();
3063 for (
unsigned int buff_num=0; buff_num<2; buff_num++)
3065 unsigned int size = 0;
3066 CanBuffer* buffer_pointer=0;
3078 for (i = 0; i < size; i++)
3082 unsigned char *
data=0;
3083 CanMessage& m = (*buffer_pointer)[i];
3088 if ((
id & 0x700) == 0x300)
3091 const unsigned int addr = ((
id & 0x0f0) >> 4);
3092 const unsigned int type =
id & 0x00f;
3093 if (type == 0x0A || type == 0x0B)
3095 int off = (type-0x0A)*3;
3096 for (
int axis=0; axis<r.
getJoints(); axis++)
3099 if ( attached_board == addr)
3101 for(
int chan=0;chan<3;chan++)
3104 if ( attached_channel == chan+off)
3117 else if ((
id & 0x700) == 0x100)
3123 const int addr = ((
id & 0x0f0) >> 4);
3140 yError(
"%s [%d] Warning, got unexpected broadcast msg(s), last one from address %d, (original) id 0x%x, len %d\n", canDevName.c_str(), _networkN, addr,
id, len);
3141 char tmp1 [255]; tmp1[0]=0;
3142 char tmp2 [255]; tmp2[0]=0;
3148 yError(
"%s [%d] valid addresses are (%s)\n",canDevName.c_str(), _networkN,tmp2);
3160 case ICUBCANPROTO_PER_MC_MSG__OVERFLOW:
3162 yError (
"CAN PACKET LOSS, board %d buffer full\r\n", (((
id & 0x0f0) >> 4)-1));
3166 case ICUBCANPROTO_PER_MC_MSG__PRINT:
3168 if (
data[0] == ICUBCANPROTO_PER_MC_MSG__PRINT ||
3169 data[0] == ICUBCANPROTO_PER_MC_MSG__PRINT + 128)
3171 int addr = (((
id & 0x0f0) >> 4)-1);
3174 if (string_id != -1)
3182 case ICUBCANPROTO_PER_MC_MSG__POSITION:
3200 case ICUBCANPROTO_PER_MC_MSG__MOTOR_POSITION:
3218 case ICUBCANPROTO_PER_MC_MSG__MOTOR_SPEED:
3239 case ICUBCANPROTO_PER_MC_MSG__TORQUE:
3255 case ICUBCANPROTO_PER_MC_MSG__PID_VAL:
3267 case ICUBCANPROTO_PER_MC_MSG__STATUS:
3280 for(
int m=0;m<8;m++)
3281 yDebug(
"%.2x ",
data[m]);
3289 logJointData(canDevName.c_str(),_networkN,j,9,yarp::os::Value((
int)bFlag));
3297 logJointData(canDevName.c_str(),_networkN,j,7,yarp::os::Value((
int)bFlag));
3300 logJointData(canDevName.c_str(),_networkN,j,10,yarp::os::Value((
int)bFlag));
3303 logJointData(canDevName.c_str(),_networkN,j,8,yarp::os::Value((
int)bFlag));
3306 logJointData(canDevName.c_str(),_networkN,j,11,yarp::os::Value((
int)bFlag));
3309 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3312 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3315 logJointData(canDevName.c_str(),_networkN,j,16,yarp::os::Value((
int)bFlag));
3318 logJointData(canDevName.c_str(),_networkN,j,13,yarp::os::Value((
int)bFlag));
3327 logJointData(canDevName.c_str(),_networkN,j,17,yarp::os::Value((
int)bFlag));
3332 logJointData(canDevName.c_str(),_networkN,j,17,yarp::os::Value((
int)bFlag));
3339 logJointData(canDevName.c_str(),_networkN,j,18,yarp::os::Value((
int)bFlag));
3341 if ((bFlag=r.
_bcastRecvBuffer[j].
isOverTempCh1())) yError (
"%s [%d] board %d OVER TEMPERATURE CH 1 \n", canDevName.c_str(), _networkN, addr);
3342 logJointData(canDevName.c_str(),_networkN,j,19,yarp::os::Value((
int)bFlag));
3344 if ((bFlag=r.
_bcastRecvBuffer[j].
isOverTempCh2())) yError (
"%s [%d] board %d OVER TEMPERATURE CH 2 \n", canDevName.c_str(), _networkN, addr);
3345 logJointData(canDevName.c_str(),_networkN,j+1,19,yarp::os::Value((
int)bFlag));
3348 logJointData(canDevName.c_str(),_networkN,j,20,yarp::os::Value((
int)bFlag));
3351 logJointData(canDevName.c_str(),_networkN,j+1,20,yarp::os::Value((
int)bFlag));
3366 logJointData(canDevName.c_str(),_networkN,j,9,yarp::os::Value((
int)bFlag));
3369 logJointData(canDevName.c_str(),_networkN,j,7,yarp::os::Value((
int)bFlag));
3372 logJointData(canDevName.c_str(),_networkN,j,10,yarp::os::Value((
int)bFlag));
3375 logJointData(canDevName.c_str(),_networkN,j,8,yarp::os::Value((
int)bFlag));
3378 logJointData(canDevName.c_str(),_networkN,j,11,yarp::os::Value((
int)bFlag));
3381 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3384 logJointData(canDevName.c_str(),_networkN,j,12,yarp::os::Value((
int)bFlag));
3389 case ICUBCANPROTO_PER_MC_MSG__ADDITIONAL_STATUS:
3400 case ICUBCANPROTO_PER_MC_MSG__CURRENT:
3411 case ICUBCANPROTO_PER_MC_MSG__PID_ERROR:
3424 case ICUBCANPROTO_PER_MC_MSG__VELOCITY:
3459 errorstring.reserve(errorstringsize);
3462 averageThreadTime=0;
3465 lastReportTime=Time::now();
3481 double before = Time::now();
3484 averagePeriod+=(currentRun-previousRun)*1000;
3488 std::list<CanRequest> timedout;
3494 ACE_ASSERT (fifo!=0);
3495 std::list<ThreadId>::iterator it=fifo->begin();
3496 std::list<ThreadId>::iterator end=fifo->end();
3508 timedout.push_back(rq);
3510 yError(
"%s [%d] thread:%d msg:%d joint:%d timed out\n",
3525 std::list<CanRequest>::iterator it=timedout.begin();
3526 std::list<CanRequest>::iterator end=timedout.end();
3529 int tid=(*it).threadId;
3538 double avPeriod=1000.0*getEstimatedPeriod();
3539 double avThTime=1000.0*getEstimatedUsed();
3540 unsigned int it=getIterations();
3542 yDebug(
"%s [%d] thread ran %d times, req.dT:%d[ms], av.dT:%.2lf[ms] av.loopT :%.2lf[ms]\n",
3550 const char *can=canDevName.c_str();
3559 yDebug(
" Can Errors -- Device Rx:%u, Device Tx:%u, RxOvf: %u, TxOvf: %u, BusOff: %d -- Driver Fifo Rx:%u, Driver Fifo Tx:%u\n",
3562 errors.rxCanFifoOvr,
3563 errors.txCanFifoOvr,
3566 errors.txBufferOvr);
3581 yDebug(
" Device has no ICanBusErr interface\n");
3587 char tmp[255] = {0};
3588 errorstring.clear();
3590 snprintf(tmp,
sizeof(tmp),
"%s [%d] printing boards infos:\n", canDevName.c_str(), r.
_networkN);
3591 errorstring.append(tmp);
3617 errorstring.append(tmp);
3631 snprintf(tmp,
sizeof(tmp),
"None");
3632 errorstring.append(tmp);
3635 yDebug(
"%s\n", errorstring.c_str());
3648 yWarning(
"%s [%d] have not heard from board %d (channel %d) since %.2lf seconds\n",
3651 addr, ch, currentRun-lastRecv);
3673 double POS_LATENCY_WARN_THR=averagePeriod*1.5;
3674 if (max>POS_LATENCY_WARN_THR)
3676 yWarning(
"%s [%d] jnt %d, warning encoder latency above threshold (lat: %.2lf [ms], received %d msgs)\n",
3686 yWarning(
"%s [%d] joint %d, warning not enough encoder messages (received %d msgs)\n",
3701 std::list<TBR_AnalogSensor *>::iterator analogIt=analogSensors.begin();
3702 while(analogIt!=analogSensors.end())
3712 const char* devName=canDevName.c_str();
3713 int id=pAnalog->
getId();
3715 #ifdef _USE_INTERFACEGUI
3716 logAnalogData(devName,r.
_networkN,
id,3,yarp::os::Value((
int)
sat));
3717 logAnalogData(devName,r.
_networkN,
id,4,yarp::os::Value((
int)err));
3718 logAnalogData(devName,r.
_networkN,
id,5,yarp::os::Value((
int)tout));
3721 if (
sat+err+tout!=0)
3723 yWarning(
"%s [%d] analog %s saturated:%u errors: %u timeout:%u\n",
3735 lastReportTime=currentRun;
3737 averageThreadTime=0;
3744 if (r.
read () !=
true)
3752 std::list<TBR_AnalogSensor *>::iterator analogIt=analogSensors.begin();
3753 while(analogIt!=analogSensors.end())
3760 yWarning(
"%s [%d] analog sensor received unexpected class 0x03 messages\n", canDevName.c_str(), r.
_networkN);
3764 yWarning(
"Got null pointer this is unusual\n");
3776 DEBUG_FUNC(
"There are %d pending messages, read msgs: %d\n",
3780 unsigned char *msgData;
3783 msgData=m.getData();
3793 yWarning(
"%s [%d] Received message but no threads waiting for it. (id: 0x%x, Class:%d MsgData[0]:%d)\n ", canDevName.c_str(), r.
_networkN, m.getId(),
getClass(m), msgData[0]);
3799 yWarning(
"Asked a bad thread id, this is probably a bug, check threadPool\n");
3805 yError(
"error while pushing a reply, this is probably an error\n");
3818 r.printMessage("CAN: timeout - still %d messages unacknowledged\n", remainingMsgs);
3819 r._error_status = false;
3826 double now = Time::now();
3827 averageThreadTime+=(now-before)*1000;
3839 std::lock_guard<std::recursive_mutex> lck(
_mutex);
3873 switch (interactionvocab)
3875 case VOCAB_IM_STIFF:
3876 return icubCanProto_interactionmode_stiff;
3878 case VOCAB_IM_COMPLIANT:
3879 return icubCanProto_interactionmode_compliant;
3881 case VOCAB_IM_UNKNOWN:
3883 return icubCanProto_interactionmode_unknownError;
3890 switch (interactionint)
3892 case icubCanProto_interactionmode_stiff:
3893 return VOCAB_IM_STIFF;
3895 case icubCanProto_interactionmode_compliant:
3896 return VOCAB_IM_COMPLIANT;
3898 case icubCanProto_interactionmode_unknownError:
3900 return icubCanProto_interactionmode_unknownError;
3911 return icubCanProto_controlmode_idle;
3913 case VOCAB_CM_POSITION:
3914 return icubCanProto_controlmode_position;
3916 case VOCAB_CM_MIXED:
3917 return icubCanProto_controlmode_mixed;
3919 case VOCAB_CM_POSITION_DIRECT:
3920 return icubCanProto_controlmode_direct;
3922 case VOCAB_CM_VELOCITY:
3923 return icubCanProto_controlmode_velocity;
3925 case VOCAB_CM_TORQUE:
3926 return icubCanProto_controlmode_torque;
3928 case VOCAB_CM_IMPEDANCE_POS:
3929 return icubCanProto_controlmode_impedance_pos;
3931 case VOCAB_CM_IMPEDANCE_VEL:
3932 return icubCanProto_controlmode_impedance_vel;
3935 return icubCanProto_controlmode_openloop;
3937 case VOCAB_CM_CURRENT:
3938 return icubCanProto_controlmode_unknownError;
3939 yError(
"'VOCAB_CM_CURRENT' error condition detected");
3942 case VOCAB_CM_FORCE_IDLE:
3943 return icubCanProto_controlmode_forceIdle;
3947 return icubCanProto_controlmode_unknownError;
3948 yError (
"'VOCAB_CM_UNKNOWN' error condition detected");
3957 case icubCanProto_controlmode_idle:
3958 return VOCAB_CM_IDLE;
3960 case icubCanProto_controlmode_position:
3961 return VOCAB_CM_POSITION;
3963 case icubCanProto_controlmode_mixed:
3964 return VOCAB_CM_MIXED;
3966 case icubCanProto_controlmode_direct:
3967 return VOCAB_CM_POSITION_DIRECT;
3969 case icubCanProto_controlmode_velocity:
3970 return VOCAB_CM_VELOCITY;
3972 case icubCanProto_controlmode_torque:
3973 return VOCAB_CM_TORQUE;
3975 case icubCanProto_controlmode_impedance_pos:
3976 return VOCAB_CM_IMPEDANCE_POS;
3978 case icubCanProto_controlmode_impedance_vel:
3979 return VOCAB_CM_IMPEDANCE_VEL;
3981 case icubCanProto_controlmode_openloop:
3982 return VOCAB_CM_PWM;
3986 case icubCanProto_controlmode_hwFault:
3987 return VOCAB_CM_HW_FAULT;
3989 case icubCanProto_controlmode_notConfigured:
3990 return VOCAB_CM_NOT_CONFIGURED;
3992 case icubCanProto_controlmode_configured:
3993 return VOCAB_CM_CONFIGURED;
3995 case icubCanProto_controlmode_unknownError:
3996 yError (
"'icubCanProto_controlmode_unknownError' error condition detected");
3997 return VOCAB_CM_UNKNOWN;
4000 case icubCanProto_controlmode_calibration:
4009 return VOCAB_CM_CALIBRATING;
4013 yError (
"'VOCAB_CM_UNKNOWN' error condition detected");
4014 return VOCAB_CM_UNKNOWN;
4024 *v=VOCAB_CM_UNKNOWN;
4033 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4043 DEBUG_FUNC(
"Calling GET_CONTROL_MODE MULTIPLE JOINTS \n");
4044 if (joints==0)
return false;
4045 if (modes==0)
return false;
4049 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4050 for (i = 0; i < n_joints; i++)
4062 if (mode == VOCAB_CM_TORQUE &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
return false;}
4064 DEBUG_FUNC(
"Calling SET_CONTROL_MODE_RAW SINGLE JOINT\n");
4068 if (v==icubCanProto_controlmode_unknownError)
return false;
4069 _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,j,v);
4071 int current_mode = VOCAB_CM_UNKNOWN;
4077 if (current_mode==mode) {ret =
true;
break;}
4078 if (current_mode==VOCAB_CM_IDLE && mode==VOCAB_CM_FORCE_IDLE) {ret =
true;
break;}
4079 if (current_mode==VOCAB_CM_HW_FAULT)
4081 if (mode!=VOCAB_CM_FORCE_IDLE) {yError (
"Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), j);}
4084 yarp::os::Time::delay(0.010);
4085 if (timeout >0) yWarning (
"setControlModeRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), j, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(mode).c_str());
4088 while (timeout < 10);
4092 yError (
"100ms Timeout occured in setControlModeRaw (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), j, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(mode).c_str());
4100 DEBUG_FUNC(
"Calling SET_CONTROL_MODE_RAW MULTIPLE JOINTS\n");
4101 if (n_joints==0)
return false;
4102 if (joints==0)
return false;
4104 for (
int i=0;i<n_joints; i++)
4106 if (modes[i] == VOCAB_CM_TORQUE &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
4109 if (v==icubCanProto_controlmode_unknownError) ret =
false;
4110 _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,joints[i],v);
4112 int current_mode = VOCAB_CM_UNKNOWN;
4117 if (current_mode==modes[i]) {ret =
true;
break;}
4118 if (current_mode==VOCAB_CM_IDLE)
4120 if (modes[i]!=VOCAB_CM_FORCE_IDLE) {yError (
"Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), joints[i]);}
4123 yarp::os::Time::delay(0.010);
4124 if (timeout >0) yWarning (
"setControlModesRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), joints[i], yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4127 while (timeout < 10);
4131 yError (
"100ms Timeout occured in setControlModesRaw(M) (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), joints[i], yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4140 listOfKeys->clear();
4141 listOfKeys->addString(
"filterType");
4142 listOfKeys->addString(
"PWMLimit");
4150 if (key ==
"filterType")
4152 Bottle& r = val.addList();
for (
int i = 0; i< res.
getJoints(); i++) {
int tmp = 0;
getFilterTypeRaw(i, &tmp); r.addInt32(tmp); }
4155 if (key ==
"PWMLimit")
4157 Bottle& r = val.addList();
for (
int i = 0; i< res.
getJoints(); i++) {
double tmp = 0;
getPWMLimitRaw(i, &tmp); r.addFloat64(tmp); }
4160 yWarning(
"getRemoteVariable(): Unknown variable %s", key.c_str());
4169 std::string s1 = val.toString();
4170 if (val.size() != _njoints)
4172 yWarning(
"setRemoteVariable(): Protocol error %s", s1.c_str());
4176 if (key ==
"filterType")
4180 int filter_type = val.get(i).asInt32();
4185 if (key ==
"PWMLimit")
4189 double limit = val.get(i).asFloat64();
4194 yWarning(
"setRemoteVariable(): Unknown variable %s", key.c_str());
4219 type = VOCAB_JOINTTYPE_REVOLUTE;
4230 DEBUG_FUNC(
"Calling SET_CONTROL_MODE_RAW ALL JOINT\n");
4236 if (modes[i] == VOCAB_CM_TORQUE &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
4239 if (v==icubCanProto_controlmode_unknownError)
return false;
4240 _writeByte8(ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE,i,v);
4242 int current_mode = VOCAB_CM_UNKNOWN;
4247 if (current_mode==modes[i]) {ret =
true;
break;}
4248 if (current_mode==VOCAB_CM_IDLE && modes[i]==VOCAB_CM_FORCE_IDLE) {ret =
true;
break;}
4249 if (current_mode==VOCAB_CM_HW_FAULT)
4251 if (modes[i]!=VOCAB_CM_FORCE_IDLE) {yError (
"Unable to set the control mode of a joint (%s j:%d) in HW_FAULT", networkName.c_str(), i);}
4254 yarp::os::Time::delay(0.010);
4255 if (timeout >0) yWarning (
"setControlModesRaw delay (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), i, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4258 while (timeout < 10);
4262 yError (
"100ms Timeout occured in setControlModesRaw (%s j:%d), current mode: %s, requested: %s", networkName.c_str(), i, yarp::os::Vocab32::decode(current_mode).c_str(), yarp::os::Vocab32::decode(modes[i]).c_str());
4285 _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_ILIM_GAIN, axis,
S_16(pid.max_int));
4286 _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_OFFSET, axis,
S_16(pid.offset));
4288 _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_TLIM, axis,
S_16(pid.max_output));
4289 _writeWord16Ex (ICUBCANPROTO_POL_MC_CMD__SET_POS_STICTION_PARAMS, axis,
S_16(pid.stiction_up_val),
S_16(pid.stiction_down_val),
false);
4301 case VOCAB_PIDTYPE_POSITION:
4304 case VOCAB_PIDTYPE_VELOCITY:
4307 case VOCAB_PIDTYPE_CURRENT:
4310 case VOCAB_PIDTYPE_TORQUE:
4314 yError()<<
"Invalid pidtype:"<<pidtype;
4345 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_PARAMS);
4355 yError(
"getImpedanceRaw: message timed out\n");
4362 CanMessage *m=t->
get(0);
4371 unsigned char *
data;
4372 data=m->getData()+1;
4373 *stiff= *((
short *)(
data));
4375 *damp= *((
short *)(
data));
4390 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4396 int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4424 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_IMPEDANCE_OFFSET);
4434 yError(
"getImpedanceOffset: message timed out\n");
4440 CanMessage *m=t->
get(0);
4448 unsigned char *
data;
4449 data=m->getData()+1;
4450 *off= *((
short *)(
data));
4469 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4471 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_PARAMS, axis);
4495 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4497 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_SOURCE, axis);
4499 *((
char *)(r.
_writeBuffer[0].getData()+2)) = board_chan;
4522 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4524 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_IMPEDANCE_OFFSET, axis);
4541 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_P_GAIN, axis, s);
out->kp = double(s);
4543 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_D_GAIN, axis, s);
out->kd = double(s);
4545 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_I_GAIN, axis, s);
out->ki = double(s);
4547 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_ILIM_GAIN, axis, s);
out->max_int = double(s);
4549 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_OFFSET, axis, s);
out->offset= double(s);
4551 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_SCALE, axis, s);
out->scale = double(s);
4553 _readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_TLIM, axis, s);
out->max_output = double(s);
4554 DEBUG_FUNC(
"Calling CAN_GET_POS_STICTION_PARAMS\n");
4555 _readWord16Ex (ICUBCANPROTO_POL_MC_CMD__GET_POS_STICTION_PARAMS, axis, s, s2 );
out->stiction_up_val = double(s);
out->stiction_down_val = double(s2);
4569 case VOCAB_PIDTYPE_POSITION:
4572 case VOCAB_PIDTYPE_VELOCITY:
4575 case VOCAB_PIDTYPE_CURRENT:
4578 case VOCAB_PIDTYPE_TORQUE:
4582 yError()<<
"Invalid pidtype:"<<pidtype;
4616 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PID, axis);
4624 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_PIDLIMITS, axis);
4632 _writeWord16Ex (ICUBCANPROTO_POL_MC_CMD__SET_TORQUE_STICTION_PARAMS, axis,
S_16(pid.stiction_up_val),
S_16(pid.stiction_down_val),
false);
4635 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_MODEL_PARAMS, axis);
4673 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PID);
4683 yError(
"getTorquePid: message timed out\n");
4689 CanMessage *m=t->
get(0);
4697 unsigned char *
data;
4698 data=m->getData()+1;
4705 out->scale= *((
char *)(
data));
4711 DEBUG_FUNC(
"Calling CAN_GET_TORQUE_PIDLIMITS\n");
4714 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_PIDLIMITS);
4724 yError(
"getTorquePidLimits: message timed out\n");
4738 data=m->getData()+1;
4739 out->offset= *((
short *)(
data));
4741 out->max_output= *((
short *)(
data));
4743 out->max_int= *((
short *)(
data));
4749 DEBUG_FUNC(
"Calling CAN_GET_MODEL_PARAMS\n");
4752 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_MODEL_PARAMS);
4762 yError(
"CAN_GET_MODEL_PARAMS: message timed out\n");
4776 data=m->getData()+1;
4781 DEBUG_FUNC(
"Calling CAN_GET_TORQUE_STICTION_PARAMS\n");
4784 _readWord16Ex (ICUBCANPROTO_POL_MC_CMD__GET_TORQUE_STICTION_PARAMS, axis, s1, s2 );
4785 out->stiction_up_val = double(s1);
4786 out->stiction_down_val = double(s2);
4794 if (pids==0)
return false;
4815 if (refs==0)
return false;
4835 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE, axis,
S_16(ref_trq));
4841 for(
int j=0; j< n_joint; j++)
4856 int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4857 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4871 if (
_writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_TORQUE, i,
S_16(ref_trqs[i])) !=
true)
4883 for (
int j = 0; j < r.
getJoints() && ret; j++)
4897 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4901 int k=castToMapper(yarp::dev::ImplementTorqueControl::helper)->toUser(j);
4903 std::list<TBR_AnalogSensor *>::iterator it=analogSensors.begin();
4904 while(it!=analogSensors.end())
4908 int id = (*it)->getId();
4936 if (!(axis >= 0 && axis <= r.
getJoints()))
4939 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4942 case VOCAB_PIDTYPE_POSITION:
4945 case VOCAB_PIDTYPE_TORQUE:
4948 case VOCAB_PIDTYPE_VELOCITY:
4952 case VOCAB_PIDTYPE_CURRENT:
4963 if (!(axis >= 0 && axis <= r.
getJoints()))
4965 std::lock_guard<std::recursive_mutex> lck(
_mutex);
4974 for (
int j = 0; j < r.
getJoints() && ret; j++)
4984 for (
int j = 0; j < nj && ret; j++)
4994 if (!(axis >= 0 && axis <= r.
getJoints()))
4996 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5005 for (
int j = 0; j < r.
getJoints() && ret; j++)
5015 for (
int j = 0; j < nj && ret; j++)
5025 if (!(axis >= 0 && axis <= r.
getJoints()))
5035 for (
int j = 0; j < r.
getJoints() && ret; j++)
5045 for (
int j = 0; j < nj && ret; j++)
5096 yError(
"getParameterRaw: message timed out\n");
5102 CanMessage *m=t->
get(0);
5110 unsigned char *
data;
5111 data=m->getData()+1;
5112 *value= *((
short *)(
data));
5143 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_DEBUG_PARAM);
5144 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = index;
5155 yError(
"getDebugParameterRaw: message timed out\n");
5161 CanMessage *m=t->
get(0);
5169 unsigned char *
data;
5170 data=m->getData()+1;
5171 *value= *((
short *)(
data));
5184 if (
_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION, axis, val) ==
true)
5185 *value = double (val);
5217 fw_info->
joint=axis;
5222 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION);
5223 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = (
unsigned char)(icub_interface_protocol.
major & 0xFF);
5224 *((
unsigned char *)(r.
_writeBuffer[0].getData()+2)) = (
unsigned char)(icub_interface_protocol.
minor & 0xFF);
5235 yError(
"getFirmwareVersion: message timed out\n");
5243 CanMessage *m=t->
get(0);
5246 yError(
"getFirmwareVersion: message error\n");
5254 unsigned char *
data;
5255 data=m->getData()+1;
5268 fw_info->
ack = *((
char *)(
data));
5284 case VOCAB_PIDTYPE_POSITION:
5287 if (
_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION, axis, value) ==
true)
5288 { *ref = double (value);}
5290 { *ref =0; yError() <<
"Invalid _readDWord (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_POSITION)";
return false; }
5293 case VOCAB_PIDTYPE_VELOCITY:
5299 case VOCAB_PIDTYPE_CURRENT:
5305 case VOCAB_PIDTYPE_TORQUE:
5314 yError()<<
"Invalid pidtype:"<<pidtype;
5325 if (refs==0)
return false;
5389 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5391 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_DEBUG_PARAM, axis);
5392 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = (
unsigned char)(index & 0xFF);
5407 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_COMMAND_POSITION, axis,
S_32(value));
5413 if (!(axis >= 0 && axis <= r.
getJoints()))
5416 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5419 case VOCAB_PIDTYPE_POSITION:
5422 case VOCAB_PIDTYPE_VELOCITY:
5425 case VOCAB_PIDTYPE_CURRENT:
5428 case VOCAB_PIDTYPE_TORQUE:
5432 yError()<<
"Invalid pidtype:"<<pidtype;
5443 getPidOutput(pidtype, i, &outs[i]);
5469 yWarning() <<
"Performance warning: You are using positionMove commands at high rate (<"<<
MAX_POSITION_MOVE_INTERVAL*1000.0 <<
" ms). Probably position control mode is not the right control mode to use.";
5475 if (mode != VOCAB_CM_POSITION &&
5476 mode != VOCAB_CM_MIXED &&
5477 mode != VOCAB_CM_IMPEDANCE_POS &&
5478 mode != VOCAB_CM_IDLE)
5480 yError() <<
"positionMoveRaw: skipping command because " << networkName.c_str() <<
" joint " << axis <<
"is not in VOCAB_CM_POSITION mode";
5484 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5487 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__POSITION_MOVE, axis);
5502 if (refs == 0)
return false;
5530 if (!
_readWord16 (ICUBCANPROTO_POL_MC_CMD__MOTION_DONE, axis, value))
5563 r.
addMessage (
id, i, ICUBCANPROTO_POL_MC_CMD__MOTION_DONE);
5584 for (i = 0, j = 0; i < r.
getJoints(); i++)
5589 if ( (m!=0) && (m->getId() != 0xffff) )
5591 value = *((
short *)(m->getData()+1));
5655 return _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_ACCELER, axis, s);
5671 acc = accs[i]/1000.0;
5713 if (
_readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_ACCELER, i, value) ==
true) {
5714 _ref_accs[i] = accs[i] = double (value);
5733 if (
_readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_ACCELER, axis, value) ==
true)
5736 *accs = double(value) * 1000.0 * 1000.0;
5753 if (
_readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_TORQUE, i, value) ==
true) {
5771 if (
_readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_TORQUE, axis, value))
5774 *ref_trq = double (value);
5788 MotorTorqueParameters pzero;
5806 r.
addMessage (
id, axis, ICUBCANPROTO_POL_MC_CMD__GET_MOTOR_PARAMS);
5817 yError(
"getMotorTorqueParamsRaw: message timed out\n");
5821 CanMessage *m=t->
get(0);
5827 param->bemf = *((
short *)(m->getData()+1));
5828 param->bemf_scale = 0;
5829 param->ktau = *((
short *)(m->getData()+4));
5830 param->ktau_scale = 0;
5846 if (!
_readByte8(ICUBCANPROTO_POL_MC_CMD__GET_TCFILTER_TYPE, axis, val))
5864 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5866 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_TCFILTER_TYPE, axis);
5883 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5885 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_MOTOR_PARAMS, axis);
5887 *((
unsigned char *)(r.
_writeBuffer[0].getData()+3)) = (
unsigned char) (param.bemf_scale);
5889 *((
unsigned char *)(r.
_writeBuffer[0].getData()+6)) = (
unsigned char) (param.ktau_scale);
5890 *((
unsigned char *)(r.
_writeBuffer[0].getData()+7)) = (
unsigned char) (0);
5899 ret &=
_writeNone (ICUBCANPROTO_POL_MC_CMD__STOP_TRAJECTORY, j);
5909 for (
int j=0; j<
n; j++)
5911 ret &=
_writeNone (ICUBCANPROTO_POL_MC_CMD__STOP_TRAJECTORY, j);
5926 if (mode != VOCAB_CM_VELOCITY &&
5927 mode != VOCAB_CM_MIXED &&
5928 mode != VOCAB_CM_IMPEDANCE_VEL &&
5929 mode != VOCAB_CM_IDLE)
5931 yError() <<
"velocityMoveRaw: skipping command because " << networkName.c_str() <<
" joint " << axis <<
"is not in VOCAB_CM_VELOCITY mode";
5935 std::lock_guard<std::recursive_mutex> lck(
_mutex);
5941 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__VELOCITY_MOVE, axis);
5967 if (sp==0)
return false;
5984 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_ENCODER_POSITION, axis,
S_32(val));
5994 if (
_writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_ENCODER_POSITION, i,
S_32(vals[i])) !=
true)
6009 double *tmp =
new double [
n];
6010 ACE_ASSERT (tmp != NULL);
6012 for(
int i=0;i<
n;i++)
6027 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6037 stampEncoders.update(stamp);
6043 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6044 Stamp ret=stampEncoders;
6051 if (!(axis >= 0 && axis <= r.
getJoints()))
return false;
6053 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6062 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6077 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6087 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6103 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6136 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6146 stampEncoders.update(stamp);
6153 if (!(m >= 0 && m <= r.
getJoints()))
return false;
6155 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6165 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6176 stampEncoders.update(stamp);
6183 if (!(m >= 0 && m <= r.
getJoints()))
return false;
6185 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6219 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6230 if (!(m >= 0 && m <= r.
getJoints()))
return false;
6232 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6242 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6254 if (!(m >= 0 && m <= r.
getJoints()))
return false;
6256 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6279 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6291 if (!(axis >= 0 && axis <= r.
getJoints()))
6294 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6304 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT, axis,
S_32(v));
6326 return _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_VEL_SHIFT, axis,
S_16(shift));
6380 bool ret =
_readWord16(ICUBCANPROTO_POL_MC_CMD__GET_PWM_LIMIT, j, s);
6404 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6406 r.
addMessage (ICUBCANPROTO_POL_MC_CMD__SET_SPEED_ESTIM_SHIFT, axis);
6407 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = (
unsigned char)(jnt_speed) & 0xFF;
6408 *((
unsigned char *)(r.
_writeBuffer[0].getData()+2)) = (
unsigned char)(jnt_acc) & 0xFF;
6409 *((
unsigned char *)(r.
_writeBuffer[0].getData()+3)) = (
unsigned char)(mot_speed) & 0xFF;
6410 *((
unsigned char *)(r.
_writeBuffer[0].getData()+4)) = (
unsigned char)(mot_acc) & 0xFF;
6421 return _writeWord16 (ICUBCANPROTO_POL_MC_CMD__SET_VEL_TIMEOUT, axis,
S_16(timeout));
6427 DEBUG_FUNC(
"Calling doneRaw for joint %d\n", axis);
6431 if (!
_readWord16 (ICUBCANPROTO_POL_MC_CMD__GET_CONTROL_MODE, axis, value))
6436 if (!(value & 0xf0))
6446 yError(
"Calling obsolete function setPrintFunction\n");
6455 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6471 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6483 ret = ret &&
_writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_MIN_POSITION, axis,
S_32(min));
6484 ret = ret &&
_writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_MAX_POSITION, axis,
S_32(max));
6497 ret = ret &&
_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_MIN_POSITION, axis, iMin);
6498 ret = ret &&
_readDWord (ICUBCANPROTO_POL_MC_CMD__GET_MAX_POSITION, axis, iMax);
6514 for(
int j=0; j<n_joint; j++)
6524 for(
int j=0; j<n_joint; j++)
6535 bool tot_value =
true;
6536 for(
int j=0; j<n_joint; j++)
6548 for(
int j=0; j<n_joint; j++)
6558 for(
int j=0; j<n_joint; j++)
6568 for(
int j=0; j<n_joint; j++)
6578 for(
int j=0; j<n_joint; j++)
6588 for(
int j=0; j<n_joint; j++)
6590 ret = ret &&
stopRaw(joints[j]);
6604 for(
int j=0; j< n_joint; j++)
6640 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6650 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6666 if (mode != VOCAB_CM_POSITION_DIRECT &&
6667 mode != VOCAB_CM_IDLE)
6669 yError() <<
"setPositionRaw: skipping command because " << networkName.c_str() <<
" joint " << j <<
"is not in VOCAB_CM_POSITION_DIRECT mode";
6672 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_COMMAND_POSITION, j,
S_32(ref));
6676 yWarning(
"skipping setPosition() on %s, joint %d (req: %.1f curr %.1f) \n", networkName.c_str() , j,
6687 if (refs == 0)
return false;
6688 if (joints == 0)
return false;
6691 for (
int j = 0; j < n_joint; j++)
6700 if (refs == 0)
return false;
6723 ret=
_writeNone(ICUBCANPROTO_POL_MC_CMD__READ_FLASH_MEM, j);
6738 ret=
_writeNone(ICUBCANPROTO_POL_MC_CMD__WRITE_FLASH_MEM, j);
6757 return _writeDWord (ICUBCANPROTO_POL_MC_CMD__SET_BCAST_POLICY, axis,
S_32(v));
6779 DEBUG_FUNC(
"Write None msg:%d axis:%d\n", msg, axis);
6780 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6799 DEBUG_FUNC(
"Writing Word16 msg:%d axis:%d\n", msg, axis);
6803 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6822 DEBUG_FUNC(
"Writing byte msg:%d axis:%d\n", msg, axis);
6827 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6832 *((
int*)(r.
_writeBuffer[0].getData()+1)) = (value & 0xFF);
6846 DEBUG_FUNC(
"Writing DWord msg:%d axis:%d\n", msg, axis);
6851 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6873 ACE_ASSERT ((axis % 2) == 0);
6875 DEBUG_FUNC(
"Write Word16Ex msg:%d axis:%d\n", msg, axis);
6880 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6905 std::lock_guard<std::recursive_mutex> lck(
_mutex);
6918 *((
unsigned char *)(r.
_writeBuffer[0].getData()+1)) = value;
6969 yError(
"readDWord: message timed out\n");
6974 CanMessage *m=t->
get(0);
6981 value = *((
int *)(m->getData()+1));
7028 yError(
"readDWordArray: at least one message timed out\n");
7034 for (i = 0, j = 0; i < r.
getJoints(); i++)
7039 if ( (m!=0) && (m->getId() != 0xffff) )
7041 out[i] = *((
int *)(m->getData()+1));
7075 DEBUG_FUNC(
"readWord16: called from thread %d, axis %d msg %d\n",
id, axis, msg);
7081 DEBUG_FUNC(
"readWord16: going to wait for packet %d\n",
id);
7085 DEBUG_FUNC(
"readWord16: ok, wait done %d\n",
id);
7089 yError(
"readWord16: message timed out\n");
7094 CanMessage *m=t->
get(0);
7101 value = *((
short *)(m->getData()+1));
7127 DEBUG_FUNC(
"_readByte8: called from thread %d, axis %d msg %d\n",
id, axis, msg);
7133 DEBUG_FUNC(
"_readByte8: going to wait for packet %d\n",
id);
7137 DEBUG_FUNC(
"_readByte8: ok, wait done %d\n",
id);
7141 yError(
"_readByte8: message timed out\n");
7146 CanMessage *m = t->
get(0);
7153 value = *((
char *)(m->getData() + 1));
7182 DEBUG_FUNC(
"readWord16Ex: called from thread %d, axis %d msg %d\n",
id, axis, msg);
7188 DEBUG_FUNC(
"readWord16Ex: going to wait for packet %d\n",
id);
7192 DEBUG_FUNC(
"readWord16Ex: ok, wait done %d\n",
id);
7196 yError(
"readWord16: message timed out\n");
7202 CanMessage *m=t->
get(0);
7209 value1 = *((
short *)(m->getData()+1));
7210 value2 = *((
short *)(m->getData()+3));
7259 yError(
"readWord16Array: at least one message timed out\n");
7265 for (i = 0, j = 0; i < r.
getJoints(); i++)
7270 if ( (m!=0) && (m->getId() != 0xffff) )
7271 out[i] = *((
short *)(m->getData()+1));
7285 std::string deviceType=config.find(
"device").asString().c_str();
7286 std::string deviceId=config.find(
"deviceid").asString().c_str();
7289 yDebug() <<
"CanBusMotionControl::createDevice() looking for device " << deviceType <<
" with id " << deviceId;
7291 if (deviceType==
"analog")
7293 std::list<TBR_AnalogSensor *>::iterator it=analogSensors.begin();
7294 while(it!=analogSensors.end())
7296 yDebug() <<
"CanBusMotionControl::createDevice() inspecting "<< (*it)->getDeviceId();
7298 if ((*it)->getDeviceId()==deviceId)
7300 yDebug() <<
"CanBusMotionControl::createDevice() found valid device";
7308 yDebug() <<
"CanBusMotionControl::createDevice() only works for analog kind of devices\n";
7319 std::lock_guard<std::recursive_mutex> lck(
_mutex);
7330 stampEncoders.update(stamp);
7337 if (!(axis >= 0 && axis <= r.
getJoints()))
return false;
7339 std::lock_guard<std::recursive_mutex> lck(
_mutex);
7349 DEBUG_FUNC(
"Calling GET_INTERACTION_MODE SINGLE JOINT\n");
7353 std::lock_guard<std::recursive_mutex> lck(
_mutex);
7361 DEBUG_FUNC(
"Calling GET_INTERACTION_MODE MULTIPLE JOINTS \n");
7362 if (joints==0)
return false;
7363 if (modes==0)
return false;
7367 std::lock_guard<std::recursive_mutex> lck(
_mutex);
7368 for (i = 0; i < n_joints; i++)
7377 DEBUG_FUNC(
"Calling GET_INTERACTION_MODE ALL JOINTS \n");
7381 std::lock_guard<std::recursive_mutex> lck(
_mutex);
7395 DEBUG_FUNC(
"Calling SET_INTERACTION_MODE RAW\n");
7397 if (mode == VOCAB_IM_COMPLIANT &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
return false;}
7399 if (v==icubCanProto_interactionmode_unknownError)
return false;
7407 DEBUG_FUNC(
"Calling SET_INTERACTION_MODE_RAW MULTIPLE JOINTS\n");
7408 if (n_joints==0)
return false;
7409 if (joints==0)
return false;
7411 for (
int i=0;i<n_joints; i++)
7420 DEBUG_FUNC(
"Calling SET_CONTROL_MODE_RAW ALL JOINT\n");
7425 if (modes[i] == VOCAB_IM_COMPLIANT &&
_MCtorqueControlEnabled ==
false) {yError()<<
"Torque control is disabled. Check your configuration parameters";
continue;}
7428 if (v==icubCanProto_interactionmode_unknownError)
return false;
7441 return _writeWord16(ICUBCANPROTO_POL_MC_CMD__SET_OPENLOOP_PARAMS, j,
S_16(v));
7463 short int value = 0;
7464 if (
_readWord16(ICUBCANPROTO_POL_MC_CMD__GET_OPENLOOP_PARAMS, axis, value) ==
true)
7475 if (v == 0)
return false;
7490 std::lock_guard<std::recursive_mutex> lck(
_mutex);
7500 std::lock_guard<std::recursive_mutex> lck(
_mutex);
static can_string_generic cstring[CAN_MAX_CARDS]
bool validate_optional(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
bool NOT_YET_IMPLEMENTED_WARNING(const char *txt)
CanBusResources & RES(void *res)
const double BCAST_STATUS_TIMEOUT
const int PRINT_BUFFER_LENGTH
bool NOT_YET_IMPLEMENTED(const char *txt)
const int REPORT_PERIOD
general purpose stuff.
bool DEPRECATED(const char *txt)
void PRINT_CAN_MESSAGE(const char *str, CanMessage &m)
bool validate(Bottle &input, Bottle &out, const std::string &key1, const std::string &txt, int size)
class for interfacing with a generic can device driver.
#define logNetworkData(a, b, c, d)
#define MAX_POSITION_MOVE_INTERVAL
#define logJointData(a, b, c, d, e)
const int CANCONTROL_MAX_THREADS
const int CAN_MAX_CARDS
Max number of addressable cards in this implementation.
int getJoint(const yarp::dev::CanMessage &m, const unsigned char *invM)
Extract the joint number to which a RECEIVED message is referring to.
int getRcp(const yarp::dev::CanMessage &m)
Extract least significative 4 bits, from the id of a can msg.
const int DEBUG_PRINTF_BUFFER_LENGTH
int getSender(const yarp::dev::CanMessage &m)
Extract most significative 4 bits of the least significative byte from the id of a can msg.
int getClass(const yarp::dev::CanMessage &m)
Extract message class, three bit least significative of the first byte of the message ID.
void DEBUG_FUNC(const char *fmt,...)
bool isMainLoopOverflow()
unsigned char _controlmodeStatus
bool isFaultUndervoltage()
unsigned char _interactionmodeStatus
BCastElement _speed_rotor
unsigned int _mainLoopOverflowCounter
void ControlStatus(int net, short controlmode, short addr)
bool isOpticalEncoderError()
BCastElement _accel_rotor
BCastElement _position_rotor
BCastElement _position_joint
unsigned int _echoMessages
size of the write packet.
ICanBusErrors * iCanErrors
bool _error_status
number of joints (ncards * 2).
bool addMessage(int msg_id, int joint)
char buffer[DEBUG_PRINTF_BUFFER_LENGTH]
bool getErrorStatus(void) const
int _velShifts[CAN_MAX_CARDS]
list of connected cards (and their addresses).
CanBuffer _readBuffer
size of the last read buffer.
bool initialize(const CanBusMotionControlParameters &parms)
int _polling_interval
this is my thread timeout.
int _networkN
speed of the bus.
bool printMessage(const CanMessage &m)
void printMessage(const char *fmt,...)
unsigned char * _destInv
list of velocity shift
unsigned int _readMessages
unsigned int _writeMessages
size of the last read buffer.
int _txQueueSize
network number.
BCastBufferElement * _bcastRecvBuffer
echo buffer.
std::string * _jointNames
int _speed
thread polling interval.
RequestsQueue * requestsQueue
might be better with dynamic allocation.
unsigned char _my_address
local storage for bcast messages.
int getJoints(void) const
ICanBufferFactory * iBufferFactory
unsigned char _destinations[CAN_MAX_CARDS]
CanBuffer _replyBuffer
write buffer.
CanBuffer _echoBuffer
reply buffer.
char _printBuffer[16384]
don't print filtered messages.
CanBuffer _writeBuffer
read buffer.
int _filter
error status of the last packet.
ThreadFifo * getFifo(int j, int msg)
virtual int calibrateChannel(int ch, double v)
bool handleAnalog(void *)
std::string getDeviceId()
bool open(int channels, AnalogDataFormat f, short bId, short useCalib, bool isVirtualSensor)
virtual int getState(int ch)
TBR_CanBackDoor * backDoor
virtual int read(yarp::sig::Vector &out)
void getCounters(unsigned int &sat, unsigned int &err, unsigned int &to)
virtual int getChannels()
void setDeviceId(std::string id)
virtual int calibrateSensor()
double * getScaleFactor()
virtual void onRead(Bottle &b)
void setUp(CanBusResources *p, std::recursive_mutex *mut, bool echo, TBR_AnalogSensor *owner)
ThreadTable2 * getThreadTable(int id)
yarp::dev::CanMessage * getByJoint(int j, const unsigned char *destInv)
bool push(const yarp::dev::CanMessage &m)
void setPending(int pend)
yarp::dev::CanMessage * get(int n)
axisImpedanceHelper(int njoints, ImpedanceLimits *imped_limits)
ImpedanceLimits * getImpedanceLimits()
double getMaxHwStep(int j)
double getSaturatedValue(int j, double curr_value, double ref_value)
axisPositionDirectHelper(int njoints, const int *aMap, const double *angToEncs, double *_maxStep)
double posE2A(double ang, int j)
double getMaximumTorque(int jnt)
double getNewtonsToSensor(int jnt)
axisTorqueHelper(int njoints, int *id, int *chan, double *maxTrq, double *newtons2sens)
int getTorqueSensorChan(int jnt)
int getTorqueSensorId(int jnt)
void clear_string(int buffer_num)
Resets the string buffer.
char * print(int buffer_num, const char *canDevName, int netNum)
Prints a string buffer.
int add_string(void *can_packet)
Process a string can packet.
void printFirmwareVersions()
bool checkFirmwareVersions()
speedEstimationHelper(int njoints, SpeedEstimationParameters *estim_parameters)
SpeedEstimationParameters getEstimationParameters(int jnt)
The PlxCan motion controller device driver.
double * _motorPwmLimits
current limits
double * _maxTorque
Channel of associated Joint Torque Sensor.
int _timeout
thread polling interval [ms]
std::string * _axisName
number of cycles before timing out
DebugParameters * _debug_params
parameters for speed/acceleration estimation
int _polling_interval
my address
double * _optical_factor
max velocity command for a joint
bool parseImpedanceGroup_NewFormat(yarp::os::Bottle &pidsGroup, ImpedanceParameters vals[])
int * _torqueSensorId
reduction ratio of the optical encoder on motor axis
double * _ktau
bemf compensation gain
double * _ampsToSensor
Newtons to force sensor units conversion factors.
double * _rotToEncoder
angle to encoder conversion factors
double * _zeros
angle to rotor conversion factors
bool parseTrqPidsGroup_OldFormat(yarp::os::Bottle &pidsGroup, int nj, Pid myPid[])
bool setBroadCastMask(yarp::os::Bottle &list, int MASK)
SpeedEstimationParameters * _estim_params
set to true if pwm is limited
unsigned char _my_address
destination addresses
bool parsePosPidsGroup_OldFormat(yarp::os::Bottle &pidsGroup, int nj, Pid myPid[])
unsigned char * _destinations
number of joints/axes/controlled motors
Pid * _tpids
initial gains
std::string * _axisType
axis name
bool fromConfig(yarp::os::Searchable &config)
double * _currentLimits
joint limits, min
double * _angleToEncoder
axis remapping lookup-table
~CanBusMotionControlParameters()
Destructor, with memory deallocation.
double * _maxJntCmdVelocity
max size of a positionDirect step
bool _pwmIsLimited
initial torque gains
double * _bemfGain
impedancel imits
int * _torqueSensorChan
Id of associated Joint Torque Sensor.
int * _velocityShifts
pwm limits
double * _newtonsToSensor
Max torque of a joint.
int * _filterType
motor torque constant
torqueControlUnitsType _torqueControlUnits
ImpedanceParameters * _impedance_params
debug parameters
std::string _networkName
network number
CanBusMotionControlParameters()
Constructor (please make sure you use the constructor to allocate memory).
bool parsePidsGroup_NewFormat(yarp::os::Bottle &pidsGroup, Pid myPid[])
ImpedanceLimits * _impedance_limits
impedance parameters
double * _limitsMax
joint limits, max
int * _velocityTimeout
velocity shifts
bool _torqueControlEnabled
double * _maxStep
velocity shifts
bool parseDebugGroup_NewFormat(yarp::os::Bottle &pidsGroup, DebugParameters vals[])
virtual bool setInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum mode) override
bool _readDWordArray(int msg, double *out)
reads an array of double words.
virtual bool enablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getTemperatureLimitRaw(int m, double *temp) override
virtual bool setEncoderRaw(int j, double val) override
bool _readByte8(int msg, int axis, int &value)
virtual bool getRefVelocitiesRaw(double *refs) override
virtual bool setRefCurrentsRaw(const double *t) override
bool _writeWord16(int msg, int axis, short s)
to send a Word16.
virtual bool getMotorEncodersTimedRaw(double *v, double *t) override
virtual bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
bool _readWord16Ex(int msg, int axis, short &value1, short &value2)
virtual bool getEncoderAccelerationsRaw(double *accs) override
virtual bool setRefSpeedsRaw(const double *spds) override
virtual bool getMotorEncoderRaw(int m, double *v) override
icubCanProto_controlmode_t from_modevocab_to_modeint(int modevocab)
virtual bool setPrintFunction(int(*f)(const char *fmt,...))
IControlDebug Interface.
virtual bool getRemoteVariableRaw(std::string key, yarp::os::Bottle &val) override
virtual bool getNominalCurrentRaw(int m, double *val) override
int from_interactionint_to_interactionvocab(unsigned char interactionint)
virtual bool setPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double ref) override
cmd is a SingleAxis poitner with 1 double arg
virtual bool getCurrentRaw(int j, double *val) override
virtual bool setRefAccelerationRaw(int j, double acc) override
virtual bool getEncoderRaw(int j, double *v) override
virtual bool setParameterRaw(int j, unsigned int type, double value)
axisPositionDirectHelper * _axisPositionDirectHelper
firmwareVersionHelper * _firmwareVersionHelper
virtual bool getPidRaw(const PidControlTypeEnum &pidtype, int j, Pid *pid) override
virtual bool stopRaw() override
virtual bool getEncodersRaw(double *encs) override
virtual bool getLimitsRaw(int axis, double *min, double *max) override
virtual ~CanBusMotionControl()
Destructor.
virtual bool disableAmpRaw(int j) override
virtual bool setTorqueSource(int axis, char board_id, char board_chan)
virtual bool getRefVelocityRaw(const int joint, double *ref) override
virtual bool setInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool setMotorEncoderCountsPerRevolutionRaw(int m, const double cpr) override
virtual bool setPidOffsetRaw(const PidControlTypeEnum &pidtype, int j, double v) override
virtual bool getTorqueRangesRaw(double *min, double *max) override
virtual bool getDutyCyclesRaw(double *v) override
bool helper_getTrqPidRaw(int j, Pid *pid)
virtual bool getRefTorqueRaw(int j, double *ref_trq) override
TORQUE CONTROL INTERFACE RAW.
virtual bool setRefTorquesRaw(const double *ref_trqs) override
cmd is an array of double (LATER: to be optimized).
bool _writeWord16Ex(int msg, int axis, short s1, short s2, bool check=true)
two shorts in a single Can message (both must belong to the same control card).
virtual bool setPositionsRaw(const int n_joint, const int *joints, const double *refs) override
bool helper_setTrqPidRaw(int j, const Pid &pid)
virtual bool getPidOutputRaw(const PidControlTypeEnum &pidtype, int j, double *out) override
speedEstimationHelper * _speedEstimationHelper
virtual bool resetEncodersRaw() override
yarp::dev::DeviceDriver * createDevice(yarp::os::Searchable &config)
virtual bool saveBootMemory()
bool _writeDWord(int msg, int axis, int value)
write a DWord
CanBusMotionControl()
Default constructor.
virtual bool getImpedanceOffsetRaw(int j, double *offs) override
bool _readWord16(int msg, int axis, short &value)
virtual yarp::os::Stamp getLastInputStamp() override
virtual bool getEncoderSpeedsRaw(double *spds) override
virtual bool setMotorEncodersRaw(const double *vals) override
bool helper_getVelPidRaw(int j, Pid *pid)
virtual bool calibrationDoneRaw(int j) override
axisTorqueHelper * _axisTorqueHelper
virtual bool getMotorEncoderAccelerationRaw(int m, double *spds) override
virtual bool getControlModesRaw(int *v) override
virtual bool setVelLimitsRaw(int axis, double min, double max) override
double * _max_vel_jnt_cmd
virtual bool setMaxCurrentRaw(int j, double val) override
virtual bool checkMotionDoneRaw(bool *flag) override
ret is a pointer to a bool
virtual bool getVelLimitsRaw(int axis, double *min, double *max) override
virtual bool getCurrentsRaw(double *vals) override
bool getFilterTypeRaw(int j, int *type)
virtual bool getCurrentRangeRaw(int j, double *min, double *max) override
virtual bool getInteractionModesRaw(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes) override
virtual bool getRefPositionsRaw(double *refs) override
bool ENABLED(int axis)
helper function to check whether the enabled flag is on or off.
bool helper_setPosPidRaw(int j, const Pid &pid)
virtual bool setControlModesRaw(const int n_joint, const int *joints, int *modes) override
virtual bool getPidReferencesRaw(const PidControlTypeEnum &pidtype, double *refs) override
virtual bool setImpedanceRaw(int j, double stiff, double damp) override
virtual bool setPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double limit) override
virtual bool getEncoderTimedRaw(int j, double *v, double *t) override
virtual bool getCurrentRangesRaw(double *min, double *max) override
virtual bool getMotorEncoderTimedRaw(int m, double *v, double *t) override
virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum &type) override
std::recursive_mutex _mutex
axisImpedanceHelper * _axisImpedanceHelper
virtual bool getMotorEncoderSpeedRaw(int m, double *sp) override
virtual bool setTemperatureLimitRaw(int m, const double temp) override
virtual bool getEncodersTimedRaw(double *v, double *t) override
virtual bool resetMotorEncodersRaw() override
virtual bool getPidErrorsRaw(const PidControlTypeEnum &pidtype, double *errs) override
double * _ref_command_positions
virtual bool getInteractionModeRaw(int axis, yarp::dev::InteractionModeEnum *mode) override
virtual bool setImpedanceOffsetRaw(int j, double offs) override
virtual bool getControlModeRaw(int j, int *v) override
virtual bool getRefCurrentsRaw(double *t) override
bool helper_getPosPidRaw(int j, Pid *pid)
bool _readWord16Array(int msg, double *out)
reads an array.
virtual bool getPowerSupplyVoltageRaw(int j, double *val) override
virtual bool loadBootMemory()
virtual bool getTargetPositionRaw(const int joint, double *ref) override
virtual bool getNumberOfMotorEncodersRaw(int *num) override
bool helper_setCurPidRaw(int j, const Pid &pid)
int from_modeint_to_modevocab(unsigned char modeint)
bool _writeByteWords16(int msg, int axis, unsigned char value, short s1, short s2, short s3)
virtual bool getAxes(int *ax) override
POSITION CONTROL INTERFACE RAW.
virtual bool setMotorEncoderRaw(int m, const double val) override
virtual bool getRefCurrentRaw(int j, double *t) override
virtual bool getRefAccelerationRaw(int j, double *acc) override
cmd is an array of double (LATER: to be optimized).
virtual bool setPWMLimitRaw(int j, const double val) override
bool _writeNone(int msg, int axis)
WRITE functions sends a message without parameters.
bool setBCastMessages(int axis, unsigned int v)
sets the broadcast policy for a given board (don't need to be called twice).
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override
virtual bool getRefSpeedsRaw(double *spds) override
cmd is an array of double (LATER: to be optimized).
virtual bool close(void)
Closes the device driver.
virtual bool setPositionRaw(int j, double ref) override
virtual bool open(yarp::os::Searchable &config)
Open the device driver and start communication with the hardware.
bool _readDWord(int msg, int axis, int &value)
READ functions sends a message and gets a dword back.
virtual bool setDebugReferencePositionRaw(int j, double value)
virtual bool getPWMLimitRaw(int j, double *val) override
unsigned char from_interactionvocab_to_interactionint(int interactionvocab)
virtual bool getAxisNameRaw(int axis, std::string &name) override
IAxisInfo.
virtual bool positionMoveRaw(int j, double ref) override
virtual bool setPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, const double *limits) override
virtual bool setRefCurrentRaw(int j, double t) override
virtual bool getEncoderAccelerationRaw(int j, double *spds) override
virtual bool getTorqueRangeRaw(int j, double *min, double *max) override
virtual bool threadInit()
virtual bool setPidReferencesRaw(const PidControlTypeEnum &pidtype, const double *refs) override
cmd is an array of double (LATER: to be optimized).
virtual bool setPidRaw(const PidControlTypeEnum &pidtype, int j, const Pid &pid) override
double * _last_position_move_time
virtual bool setPeakCurrentRaw(int m, const double val) override
virtual bool setLimitsRaw(int axis, double min, double max) override
virtual bool getFirmwareVersionRaw(int axis, can_protocol_info const &icub_interface_protocol, firmware_info *info)
virtual bool getTemperatureRaw(int m, double *val) override
virtual bool getDutyCycleRaw(int j, double *v) override
virtual bool relativeMoveRaw(int j, double delta) override
virtual bool getNumberOfMotorsRaw(int *m) override
IMotor.
virtual bool getDebugParameterRaw(int j, unsigned int index, double *value)
virtual bool getRefDutyCycleRaw(int j, double *v) override
virtual bool getMaxCurrentRaw(int j, double *val) override
int _filter
filter for recurrent messages.
virtual bool getRefDutyCyclesRaw(double *v) override
virtual bool isPidEnabledRaw(const PidControlTypeEnum &pidtype, int j, bool *enabled)
virtual bool getMotorTorqueParamsRaw(int j, MotorTorqueParameters *params) override
virtual bool getMotorEncodersRaw(double *encs) override
virtual bool setRefDutyCyclesRaw(const double *v) override
virtual bool getMotorEncoderSpeedsRaw(double *spds) override
virtual bool getRefAccelerationsRaw(double *accs) override
cmd is an array of double (LATER: to be optimized).
virtual bool getMotorEncoderCountsPerRevolutionRaw(int m, double *cpr) override
virtual bool velocityMoveRaw(int j, double sp) override
Velocity control interface raw.
bool setVelocityShiftRaw(int j, double val)
bool setFilterTypeRaw(int j, int type)
virtual bool getPidsRaw(const PidControlTypeEnum &pidtype, Pid *pids) override
virtual bool getPeakCurrentRaw(int m, double *val) override
virtual bool getTargetPositionsRaw(double *refs) override
bool setVelocityTimeoutRaw(int j, double val)
virtual bool resetEncoderRaw(int j) override
virtual bool getRemoteVariablesListRaw(yarp::os::Bottle *listOfKeys) override
virtual bool setCalibrationParametersRaw(int j, const CalibrationParameters ¶ms) override
bool helper_setVelPidRaw(int j, const Pid &pid)
bool setSpeedEstimatorShiftRaw(int j, double jnt_speed, double jnt_acc, double mot_speed, double mot_acc)
virtual bool setRefTorqueRaw(int j, double ref_trq) override
cmd is a SingleAxis poitner with 1 double arg
virtual bool setControlModeRaw(const int j, const int mode) override
double * _ref_command_speeds
virtual bool getParameterRaw(int j, unsigned int type, double *value)
virtual bool getPWMRaw(int j, double *val) override
bool _writeByte8(int msg, int axis, int value)
write a byte
virtual bool setDebugParameterRaw(int j, unsigned int index, double value)
virtual bool getAmpStatusRaw(int *st) override
virtual bool getRefTorquesRaw(double *ref_trqs) override
cmd is an array of double (LATER: to be optimized).
virtual bool getPidOutputsRaw(const PidControlTypeEnum &pidtype, double *outs) override
virtual bool getPidErrorRaw(const PidControlTypeEnum &pidtype, int j, double *err) override
virtual bool setRemoteVariableRaw(std::string key, const yarp::os::Bottle &val) override
virtual bool setRefAccelerationsRaw(const double *accs) override
virtual bool getRefPositionRaw(const int joint, double *ref) override
virtual bool getPidReferenceRaw(const PidControlTypeEnum &pidtype, int j, double *ref) override
bool helper_getCurPidRaw(int j, Pid *pid)
virtual bool getImpedanceRaw(int j, double *stiff, double *damp) override
IMPEDANCE CONTROL INTERFACE RAW.
virtual bool getPidErrorLimitsRaw(const PidControlTypeEnum &pidtype, double *limits) override
virtual bool setRefSpeedRaw(int j, double sp) override
virtual bool getTorqueRaw(int j, double *trq) override
cmd is a SingleAxis pointer with 1 double arg
virtual bool resetMotorEncoderRaw(int m) override
virtual bool getDebugReferencePositionRaw(int j, double *value)
virtual bool getTemperaturesRaw(double *vals) override
bool _MCtorqueControlEnabled
virtual bool setEncodersRaw(const double *vals) override
virtual bool getEncoderSpeedRaw(int j, double *sp) override
virtual bool disablePidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool getTorquesRaw(double *trqs) override
cmd is an array of double (LATER: to be optimized).
virtual bool setMotorTorqueParamsRaw(int j, const MotorTorqueParameters params) override
virtual bool resetPidRaw(const PidControlTypeEnum &pidtype, int j) override
virtual bool setRefDutyCycleRaw(int j, double v) override
short S_16(double x) const
virtual bool getPidErrorLimitRaw(const PidControlTypeEnum &pidtype, int j, double *limit) override
virtual bool setPidsRaw(const PidControlTypeEnum &pidtype, const Pid *pids) override
virtual bool enableAmpRaw(int j) override
virtual void threadRelease()
virtual bool getRefSpeedRaw(int j, double *ref) override
virtual bool setNominalCurrentRaw(int m, const double val) override
virtual bool getMotorEncoderAccelerationsRaw(double *accs) override
#define CAN_PROTOCOL_MAJOR
#define CAN_PROTOCOL_MINOR
#define CAN_SET_INTERACTION_MODE
double sat(const double val, const double min, const double max)
Copyright (C) 2008 RobotCub Consortium.
void update(int value, double st)
void getStats(int &it, double &dT, double &min, double &max)
double jnt_Vel_estimator_shift
double mot_Acc_estimator_shift
double mot_Vel_estimator_shift
double jnt_Acc_estimator_shift
can_protocol_info can_protocol