bool teo::TechnosoftIpos::getTorqueRaw(int j, double *t) {
//CD_INFO("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream.
//-- Check index within range
if ( j != 0 ) return false;
//*************************************************************
uint8_t msg_getCurrent[]={0x40,0x7E,0x20,0x00}; // Query current. Ok only 4.
if(! send(0x600, 4, msg_getCurrent) )
{
CD_ERROR("Could not send msg_getCurrent. %s\n", msgToStr(0x600, 4, msg_getCurrent).c_str() );
return false;
}
//CD_SUCCESS("Sent msg_getCurrent. %s\n", msgToStr(0x600, 4, msg_getCurrent).c_str() ); //-- Too verbose in controlboardwrapper2 stream.
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Time::delay(DELAY); // Must delay as it will be from same driver.
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
getTorqueReady.wait();
*t = getTorque;
getTorqueReady.post();
//*************************************************************
return true;
}
bool teo::TechnosoftIpos::getEncoderTimedRaw(int j, double *encs, double *time) {
//CD_INFO("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream.
//-- Check index within range
if ( j != 0 ) return false;
if( ! iEncodersTimedRawExternal ) {
//*************************************************************
uint8_t msg_read[]={0x40,0x64,0x60,0x00,0x00,0x00,0x00,0x00}; // Query position.
if( ! send( 0x600, 8, msg_read) )
{
CD_ERROR("Could not send \"read encoder\". %s\n", msgToStr(0x600, 8, msg_read).c_str() );
return false;
}
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
Time::delay(DELAY); // Must delay as it will be from same driver.
//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
encoderReady.wait();
*encs = encoder;
*time = encoderTimestamp;
encoderReady.post();
//*************************************************************
} else {
iEncodersTimedRawExternal->getEncoderTimedRaw(0,encs,time);
}
return true;
}
bool teo::FakeControlboard::getControlMode(int j, int *mode) {
// CD_DEBUG("\n"); //-- Way too verbose.
if(modePosVel == 0)
*mode = VOCAB_CM_POSITION;
else if (modePosVel == 1)
*mode = VOCAB_CM_VELOCITY;
else
{
CD_ERROR("Currently unsupported mode.\n");
return false;
}
return true;
}
int main(int argc, char *argv[]) {
CD_ERROR("Test CD_ERROR.\n");
CD_WARNING("Test CD_WARNING.\n");
CD_SUCCESS("Test CD_SUCCESS.\n");
CD_INFO("Test CD_INFO.\n");
CD_DEBUG("Test CD_DEBUG.\n\n");
CD_ERROR_NO_HEADER("Test CD_ERROR_NO_HEADER.\n");
CD_WARNING_NO_HEADER("Test CD_WARNING_NO_HEADER.\n");
CD_SUCCESS_NO_HEADER("Test CD_SUCCESS_NO_HEADER.\n");
CD_INFO_NO_HEADER("Test CD_INFO_NO_HEADER.\n");
CD_DEBUG_NO_HEADER("Test CD_DEBUG_NO_HEADER.\n\n");
return 0;
}
bool teo::ControlboardContainer::start() {
vectorOfJointPos.resize( this->vectorOfJointIdx.size() );
Property options;
options.put("device","controlboardwrapper2"); //
options.put("subdevice","FakeControlboard"); // FakeControlboard provides more interfaces than test_motor
options.put("axes", (int)this->vectorOfJointIdx.size() );
options.put("name", this->manipulatorWrapperName );
dd.open(options);
if(!dd.isValid()) {
CD_ERROR("ManipulatorWrapper device \"%s\" not available.\n", options.find("subdevice").asString().c_str());
dd.close();
return false;
}
dd.view(encs);
return true;
}
bool CartesianRateThread::load(const std::string& fileName)
{
std::string fullFileName = rf->findFileByName( fileName );
this->ifs.open( fullFileName.c_str() );
if( ! this->ifs.is_open() )
{
CD_ERROR("Could not open read file: %s.\n",fullFileName.c_str());
return false;
}
CD_SUCCESS("Opened file: %s.\n",fileName.c_str());
lineCount = 1;
currentState = PTMODE;
iVelocityControl->setVelocityMode();
return true;
}
bool teo::GravityRateThread::threadInit() {
iEncoders->getAxes( &numMotors );
CD_INFO("numMotors: %d.\n",numMotors);
solver->getNumLinks( &solverNumLinks );
CD_INFO("solverNumLinks: %d.\n",solverNumLinks);
if( numMotors < solverNumLinks ) {
CD_ERROR("numMotors < solverNumLinks !!! (must be >=)\n");
return false;
}
q.resize( numMotors );
iTorqueControl->setTorqueMode();
return true;
}
bool CartesianRateThread::threadInit()
{
iEncoders->getAxes( &numMotors );
CD_INFO("numMotors: %d.\n",numMotors);
solver->getNumLinks( &solverNumLinks );
CD_INFO("solverNumLinks: %d.\n",solverNumLinks);
if( numMotors < solverNumLinks ) {
CD_ERROR("numMotors < solverNumLinks !!! (must be >=)\n");
return false;
}
qReal.resize( numMotors );
qDotCmd.resize( numMotors );
xDesired.resize( 3 + 3 ); // x y z + r p y
xDotDesired.resize( 6 );
xDotCmd.resize( 6 );
return true;
}
bool teo::TechnosoftIpos::setRefTorqueRaw(int j, double t) {
CD_INFO("(%d,%f)\n",j,t);
//-- Check index within range
if ( j!= 0 ) return false;
//*************************************************************
uint8_t msg_ref_torque[]={0x23,0x1C,0x20,0x00,0x00,0x00,0x00,0x00}; // put 23 because it is a target
int sendRefTorque = t * (65520.0/20.0) / (this->tr * this->k); // Page 109 of 263, supposing 10 Ipeak.
//memcpy(msg_ref_torque+4,&sendRefTorque,4); // was +6 not +4, but +6 seems terrible with 4!
memcpy(msg_ref_torque+6,&sendRefTorque,2);
if(! send(0x600, 8, msg_ref_torque) )
{
CD_ERROR("Could not send refTorque. %s\n", msgToStr(0x600, 8, msg_ref_torque).c_str() );
return false;
}
CD_SUCCESS("Sent refTorque. %s\n", msgToStr(0x600, 8, msg_ref_torque).c_str() );
//*************************************************************
return true;
}
bool teo::TechnosoftIpos::velocityMoveRaw(int j, double sp) {
CD_INFO("(%d)\n",j);
//-- Check index within range
if ( j != 0 ) return false;
//*************************************************************
uint8_t msg_vel[]={0x23,0xFF,0x60,0x00,0x00,0x00,0x00,0x00}; // Velocity target
int16_t sendVel = sp * this->tr / 22.5; // Apply tr & convert units to encoder increments
memcpy(msg_vel+6,&sendVel,2);
//float sendVel = sp * this->tr / 22.5; // Apply tr & convert units to encoder increments
//int16_t sendVelFormated = roundf(sendVel * 65536); // 65536 = 2^16
//memcpy(msg_vel+4,&sendVelFormated,4);
if( ! send(0x600, 8, msg_vel)){
CD_ERROR("Sent \"velocity target\". %s\n", msgToStr(0x600, 8, msg_vel).c_str() );
return false;
}
CD_SUCCESS("Sent \"velocity target\". %s\n", msgToStr(0x600, 8, msg_vel).c_str() );
//*************************************************************
return true;
}
bool TeoXRpcResponder::read(yarp::os::ConnectionReader& connection) {
yarp::os::Bottle in, out;
in.read(connection);
printf("[xRpcResponder] Got %s\n", in.toString().c_str());
out.clear();
yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender==NULL) return false;
if ((in.get(0).asString() == "help")||(in.get(0).asVocab() == VOCAB_HELP)) // help //
{
out.addString("Available commands: [help] [load] [stat]");
return out.write(*returnToSender);
}
else if ((in.get(0).asString() == "load")||(in.get(0).asVocab() == VOCAB_LOAD)) // load //
{
if ( in.size() != 2 )
{
CD_ERROR("in.size() != 2\n");
out.addVocab(VOCAB_FAILED);
return out.write(*returnToSender);
}
if( ! cartesianRateThread->load( in.get(1).asString() ) )
{
CD_ERROR("cartesianRateThread->load failed\n");
out.addVocab(VOCAB_FAILED);
return out.write(*returnToSender);
}
out.addVocab(VOCAB_OK);
return out.write(*returnToSender);
}
else if ((in.get(0).asString() == "stat")||(in.get(0).asVocab() == VOCAB_STAT)) // stat //
{
std::vector<double> stat;
if( ! cartesianRateThread->stat(stat) )
out.addVocab(VOCAB_FAILED);
else
for(int i=0;i<stat.size();i++)
out.addDouble(stat[i]);
return out.write(*returnToSender);
}
else if ((in.get(0).asString() == "inv")||(in.get(0).asVocab() == VOCAB_INV)) // inv //
{
std::vector<double> xd, q;
for(int i=1;i<in.size();i++)
xd.push_back(in.get(i).asDouble());
if( ! cartesianRateThread->inv(xd,q) )
out.addVocab(VOCAB_FAILED);
else
for(int i=0;i<q.size();i++)
out.addDouble(q[i]);
return out.write(*returnToSender);
}
else if ((in.get(0).asString() == "movj")||(in.get(0).asVocab() == VOCAB_MOVJ)) // movj //
{
std::vector<double> xd, q;
for(int i=1;i<in.size();i++)
xd.push_back(in.get(i).asDouble());
if( ! cartesianRateThread->movj(xd) )
out.addVocab(VOCAB_FAILED);
else
{
if(in.check("wait"))
{
CD_SUCCESS("Waiting\n");
bool done = false;
while(!done) {
cartesianRateThread->checkMotionDone(&done);
printf(".");
fflush(stdout);
yarp::os::Time::delay(0.5);
}
printf("\n");
}
out.addVocab(VOCAB_OK);
}
return out.write(*returnToSender);
}
else
{
fprintf(stderr,"[xRpcResponder] fail: Unknown command (use 'help' if needed).\n");
out.addVocab(VOCAB_FAILED);
return out.write(*returnToSender);
}
}
bool PlaybackLocomotion::configure(ResourceFinder &rf) {
int ptModeMs = rf.check("ptModeMs",yarp::os::Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
CD_INFO("Using ptModeMs: %d (default: %d).\n",ptModeMs,int(DEFAULT_PT_MODE_MS));
playbackThread.hold = rf.check("hold");
//-- Open file for reading.
std::string fileName = rf.check("file",Value(DEFAULT_FILE_NAME),"file name").asString();
CD_INFO("Using file: %s (default: " DEFAULT_FILE_NAME ").\n",fileName.c_str());
playbackThread.ifs.open( fileName.c_str() );
if( ! playbackThread.ifs.is_open() ) {
CD_ERROR("Could not open file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened file: %s.\n",fileName.c_str());
//-- left leg --
std::string leftLegIni = rf.findFileByName("../locomotion/leftLeg.ini");
yarp::os::Property leftLegOptions;
if (! leftLegOptions.fromConfigFile(leftLegIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"leftLeg.ini\".\n");
return false;
}
CD_SUCCESS("Configured left leg from %s.\n",leftLegIni.c_str());
leftLegOptions.put("name","/teo/leftLeg");
leftLegOptions.put("device","CanBusControlboard");
leftLegOptions.put("ptModeMs",ptModeMs);
if (rf.check("home")) leftLegOptions.put("home",1);
if (rf.check("reset")) leftLegOptions.put("reset",1);
leftLegDevice.open(leftLegOptions);
if (!leftLegDevice.isValid()) {
CD_ERROR("leftLegDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- right leg --
std::string rightLegIni = rf.findFileByName("../locomotion/rightLeg.ini");
yarp::os::Property rightLegOptions;
if (! rightLegOptions.fromConfigFile(rightLegIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"rightLeg.ini\".\n");
return false;
}
CD_SUCCESS("Configured right leg from %s.\n",rightLegIni.c_str());
rightLegOptions.put("name","/teo/rightLeg");
rightLegOptions.put("device","CanBusControlboard");
rightLegOptions.put("ptModeMs",ptModeMs);
if (rf.check("home")) rightLegOptions.put("home",1);
if (rf.check("reset")) rightLegOptions.put("reset",1);
rightLegDevice.open(rightLegOptions);
if (!rightLegDevice.isValid()) {
CD_ERROR("rightLegDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- Configure the thread.
//-- Obtain manipulator interfaces.
if ( ! leftLegDevice.view( playbackThread.leftLegPos ) ) {
CD_ERROR("Could not obtain leftLegPos.\n");
return false;
}
CD_SUCCESS("Obtained leftLegPos.\n");
if ( ! leftLegDevice.view( playbackThread.leftLegPosDirect ) ) {
CD_ERROR("Could not obtain leftLegPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained leftLegPosDirect.\n");
if ( ! rightLegDevice.view( playbackThread.rightLegPos ) ) {
CD_ERROR("Could not obtain leftLegPos.\n");
return false;
}
CD_SUCCESS("Obtained rightLegPos.\n");
if ( ! rightLegDevice.view( playbackThread.rightLegPosDirect ) ) {
CD_ERROR("Could not obtain rightLegPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained rightLegPosDirect.\n");
//-- Do stuff.
playbackThread.leftLegPos->getAxes( &(playbackThread.leftLegNumMotors) );
playbackThread.rightLegPos->getAxes( &(playbackThread.rightLegNumMotors) );
CD_INFO("setPositionDirectMode...\n");
playbackThread.leftLegPosDirect->setPositionDirectMode();
playbackThread.rightLegPosDirect->setPositionDirectMode();
//-- Start the thread.
CD_INFO("Start thread...\n");
playbackThread.start();
return true;
}
int main(int argc, char *argv[]) {
YARP_REGISTER_PLUGINS(BodyYarp);
yarp::os::Network yarp;
if (! yarp::os::Network::checkNetwork() ) {
printf("Please start a yarp name server first\n");
return 1;
}
PolyDriver canBusDevice;
teo::CanBusHico* iCanBus;
Property canBusOptions;
canBusOptions.put("device","CanBusHico");
canBusOptions.put("canDevice","/dev/can1");
canBusOptions.put("canBitrate",BITRATE_1000k);
canBusDevice.open(canBusOptions);
if( ! canBusDevice.isValid() ){
CD_ERROR("canBusDevice instantiation not worked.\n");
return 1;
}
canBusDevice.view(iCanBus);
yarp::os::Property options;
options.put("device","TechnosoftIpos");
options.put("canId",23);
options.put("tr",120);
options.put("min",-60);
options.put("max",60);
options.put("refAcceleration",0.575437);
options.put("refSpeed",737.2798);
yarp::dev::PolyDriver dd(options);
if(!dd.isValid()) {
printf("TechnosoftIpos device not available.\n");
dd.close();
yarp::os::Network::fini();
return 1;
}
//-- View TechnosoftIpos interfaces.
teo::ICanBusSharer *iCanBusSharer;
bool ok = dd.view(iCanBusSharer);
if (!ok) {
printf("[error] Problems viewing ICanBusSharer.\n");
return 1;
} else printf("[success] Viewing ICanBusSharer.\n");
yarp::dev::IPositionControlRaw *pos;
ok = dd.view(pos);
if (!ok) {
printf("[error] Problems viewing IPositionControlRaw.\n");
return 1;
} else printf("[success] Viewing IPositionControlRaw.\n");
yarp::dev::IEncodersRaw *enc;
ok = dd.view(enc);
if (!ok) {
printf("[error] Problems viewing IEncodersRaw.\n");
return 1;
} else printf("[success] Viewing IEncodersRaw.\n");
yarp::dev::IVelocityControlRaw *vel;
ok = dd.view(vel);
if (!ok) {
printf("[error] Problems viewing IVelocityControlRaw.\n");
return 1;
} else printf("[success] Viewing IVelocityControlRaw.\n");
//-- Pass before sending commands.
iCanBusSharer->setCanBusPtr(iCanBus);
iCanBusSharer->start();
yarp::os::Time::delay(0.1);
iCanBusSharer->readyToSwitchOn();
yarp::os::Time::delay(0.1);
iCanBusSharer->switchOn();
yarp::os::Time::delay(2);
iCanBusSharer->enable();
//-- Commands on TechnosoftIpos.
ok = pos->setPositionModeRaw();
if (!ok) {
printf("[error] Problems in setPositionModeRaw.\n");
return 1;
} else printf("[success] setPositionModeRaw.\n");
printf("test positionMove(0,-25)\n");
ok = pos->positionMoveRaw(0, -25);
if (!ok) {
printf("[error] Problems in positionMove.\n");
return 1;
} else printf("[success] positionMove.\n");
printf("Delaying 5 seconds...\n");
yarp::os::Time::delay(5);
/*vel->setVelocityModeRaw();
printf("test velocityMove(0,10)\n");
vel->velocityMoveRaw(0,10);
printf("Delaying 5 seconds...\n");
yarp::os::Time::delay(5);*/
dd.close();
return 0;
}
bool TwoCanBusThreeWrappers::configure(ResourceFinder &rf) {
if(rf.check("help")) {
printf("TwoCanBusThreeWrappers options:\n");
printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
CD_DEBUG_NO_HEADER("%s\n",rf.toString().c_str());
return false;
}
//-- /dev/can0 --
Bottle devCan0 = rf.findGroup("devCan0");
CD_DEBUG("%s\n",devCan0.toString().c_str());
Property optionsDevCan0;
optionsDevCan0.fromString(devCan0.toString());
deviceDevCan0.open(optionsDevCan0);
if (!deviceDevCan0.isValid()) {
CD_ERROR("deviceDevCan0 instantiation not worked.\n");
return false;
}
//-- /dev/can1 --
Bottle devCan1 = rf.findGroup("devCan1");
CD_DEBUG("%s\n",devCan1.toString().c_str());
Property optionsDevCan1;
optionsDevCan1.fromString(devCan1.toString());
deviceDevCan1.open(optionsDevCan1);
if (!deviceDevCan1.isValid()) {
CD_ERROR("deviceDevCan1 instantiation not worked.\n");
return false;
}
//-- wrapper0 --
Bottle wrapper0 = rf.findGroup("wrapper0");
CD_DEBUG("%s\n",wrapper0.toString().c_str());
Property optionsWrapper0;
optionsWrapper0.fromString(wrapper0.toString());
deviceWrapper0.open(optionsWrapper0);
if (!deviceWrapper0.isValid()) {
CD_ERROR("deviceWrapper0 instantiation not worked.\n");
return false;
}
//-- wrapper1 --
Bottle wrapper1 = rf.findGroup("wrapper1");
CD_DEBUG("%s\n",wrapper1.toString().c_str());
Property optionsWrapper1;
optionsWrapper1.fromString(wrapper1.toString());
deviceWrapper1.open(optionsWrapper1);
if (!deviceWrapper1.isValid()) {
CD_ERROR("deviceWrapper1 instantiation not worked.\n");
return false;
}
//-- wrapper2 --
Bottle wrapper2 = rf.findGroup("wrapper2");
CD_DEBUG("%s\n",wrapper2.toString().c_str());
Property optionsWrapper2;
optionsWrapper2.fromString(wrapper2.toString());
deviceWrapper2.open(optionsWrapper2);
if (!deviceWrapper2.isValid()) {
CD_ERROR("deviceWrapper2 instantiation not worked.\n");
return false;
}
IMultipleWrapper *iWrapper0, *iWrapper1, *iWrapper2;
deviceWrapper0.view(iWrapper0);
deviceWrapper1.view(iWrapper1);
deviceWrapper2.view(iWrapper2);
PolyDriverList list;
list.push(&deviceDevCan0, "devCan0");
list.push(&deviceDevCan1, "devCan1");
iWrapper0->attachAll(list);
iWrapper1->attachAll(list);
iWrapper2->attachAll(list);
return true;
}
bool PlaybackManipulation::configure(ResourceFinder &rf) {
int ptModeMs = rf.check("ptModeMs",yarp::os::Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
CD_INFO("Using ptModeMs: %d (default: %d).\n",ptModeMs,int(DEFAULT_PT_MODE_MS));
playbackThread.hold = rf.check("hold");
//-- Open file for logging.
if ( rf.check("log") ) {
playbackThread.log = true;
std::string fileName = rf.check("log",yarp::os::Value(DEFAULT_LOG_NAME),"file name").asString();
CD_INFO("Using log file: %s (default: " DEFAULT_LOG_NAME ").\n",fileName.c_str());
playbackThread.logFilePtr = ::fopen (fileName.c_str(),"w");
if( ! playbackThread.logFilePtr ) {
CD_PERROR("Could not open log file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened log file: %s.\n",fileName.c_str());
} else {
playbackThread.log = false;
}
//-- Open file for reading.
std::string fileName = rf.check("file",Value(DEFAULT_FILE_NAME),"file name").asString();
CD_INFO("Using read file: %s (default: " DEFAULT_FILE_NAME ").\n",fileName.c_str());
playbackThread.ifs.open( fileName.c_str() );
if( ! playbackThread.ifs.is_open() ) {
CD_ERROR("Could not open read file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened file: %s.\n",fileName.c_str());
std::string allIni = rf.findFileByName("../launchManipulation/launchManipulation.ini");
yarp::os::Property allOptions;
if (! allOptions.fromConfigFile(allIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"launchManipulation.ini\".\n");
return false;
}
//CD_SUCCESS("Configuring from %s.\n",allOptions.toString().c_str());
//-- /dev/can0 --
yarp::os::Bottle devCan0 = allOptions.findGroup("devCan0");
CD_DEBUG("%s\n",devCan0.toString().c_str());
yarp::os::Property optionsDevCan0;
optionsDevCan0.fromString(devCan0.toString());
optionsDevCan0.put("device","CanBusControlboard");
optionsDevCan0.put("ptModeMs",ptModeMs);
if (rf.check("home")) optionsDevCan0.put("home",1);
if (rf.check("reset")) optionsDevCan0.put("reset",1);
leftArmDevice.open(optionsDevCan0);
if (!leftArmDevice.isValid()) {
CD_ERROR("leftArmDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_BodyYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- /dev/can1 --
yarp::os::Bottle devCan1 = allOptions.findGroup("devCan1");
CD_DEBUG("%s\n",devCan1.toString().c_str());
yarp::os::Property optionsDevCan1;
optionsDevCan1.fromString(devCan1.toString());
optionsDevCan1.put("device","CanBusControlboard");
optionsDevCan1.put("ptModeMs",ptModeMs);
if (rf.check("home")) optionsDevCan1.put("home",1);
if (rf.check("reset")) optionsDevCan1.put("reset",1);
rightArmDevice.open(optionsDevCan1);
if (!rightArmDevice.isValid()) {
CD_ERROR("rightArmDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_BodyYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- Configure the thread.
//-- Obtain manipulator interfaces.
if ( ! leftArmDevice.view( playbackThread.leftArmPos ) ) {
CD_ERROR("Could not obtain leftArmPos.\n");
return false;
}
CD_SUCCESS("Obtained leftArmPos.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmPosDirect ) ) {
CD_ERROR("Could not obtain leftArmPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained leftArmPosDirect.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmEncTimed ) ) {
CD_ERROR("Could not obtain leftArmEncTimed.\n");
return false;
}
CD_SUCCESS("Obtained leftArmEncTimed.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmTorque ) ) {
CD_ERROR("Could not obtain leftArmTorque.\n");
return false;
}
CD_SUCCESS("Obtained leftArmTorque.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmPos ) ) {
CD_ERROR("Could not obtain leftArmPos.\n");
return false;
}
CD_SUCCESS("Obtained rightArmPos.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmPosDirect ) ) {
CD_ERROR("Could not obtain rightArmPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained rightArmPosDirect.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmEncTimed ) ) {
CD_ERROR("Could not obtain rightArmEncTimed.\n");
return false;
}
CD_SUCCESS("Obtained rightArmEncTimed.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmTorque ) ) {
CD_ERROR("Could not obtain rightArmTorque.\n");
return false;
}
CD_SUCCESS("Obtained rightArmTorque.\n");
//-- Do stuff.
playbackThread.leftArmPos->getAxes( &(playbackThread.leftArmNumMotors) );
playbackThread.rightArmPos->getAxes( &(playbackThread.rightArmNumMotors) );
CD_INFO("setPositionDirectMode...\n");
playbackThread.leftArmPosDirect->setPositionDirectMode();
playbackThread.rightArmPosDirect->setPositionDirectMode();
//-- Start the thread.
CD_INFO("Start thread...\n");
playbackThread.start();
return true;
}
bool teo::CanBusControlboard::open(Searchable& config) {
std::string mode = config.check("mode",Value(DEFAULT_MODE),"position/velocity mode").asString();
int16_t ptModeMs = config.check("ptModeMs",Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
Bottle ids = config.findGroup("ids").tail(); //-- e.g. 15
Bottle trs = config.findGroup("trs").tail(); //-- e.g. 160
Bottle ks = config.findGroup("ks").tail(); //-- e.g. 0.0706
Bottle maxs = config.findGroup("maxs").tail(); //-- e.g. 360
Bottle mins = config.findGroup("mins").tail(); //-- e.g. -360
Bottle refAccelerations = config.findGroup("refAccelerations").tail(); //-- e.g. 0.575437
Bottle refSpeeds = config.findGroup("refSpeeds").tail(); //-- e.g. 737.2798
Bottle types = config.findGroup("types").tail(); //-- e.g. 15
//-- Initialize the CAN device.
Property canBusOptions;
canBusOptions.fromString(config.toString()); // canDevice, canBitrate
canBusOptions.put("device","CanBusHico");
canBusDevice.open(canBusOptions);
if( ! canBusDevice.isValid() ){
CD_ERROR("canBusDevice instantiation not worked.\n");
return false;
}
canBusDevice.view(iCanBus);
//-- Start the reading thread (required for checkMotionDoneRaw).
this->Thread::start();
//-- Populate the CAN nodes vector.
nodes.resize( ids.size() );
iControlLimitsRaw.resize( nodes.size() );
iControlModeRaw.resize( nodes.size() );
iEncodersTimedRaw.resize( nodes.size() );
iPositionControlRaw.resize( nodes.size() );
iPositionDirectRaw.resize( nodes.size() );
iTorqueControlRaw.resize( nodes.size() );
iVelocityControlRaw.resize( nodes.size() );
iCanBusSharer.resize( nodes.size() );
for(int i=0; i<nodes.size(); i++)
{
if(types.get(i).asString() == "")
CD_WARNING("Argument \"types\" empty at %d.\n",i);
//-- Create CAN node objects with a pointer to the CAN device, its id and tr (these are locally stored parameters).
Property options;
options.put("device",types.get(i).asString()); //-- "TechnosoftIpos", "LacqueyFetch"
options.put("canId",ids.get(i).asInt());
options.put("tr",trs.get(i).asDouble());
options.put("min",mins.get(i).asDouble());
options.put("max",maxs.get(i).asDouble());
options.put("k",ks.get(i).asDouble());
options.put("refAcceleration",refAccelerations.get(i).asDouble());
options.put("refSpeed",refSpeeds.get(i).asDouble());
options.put("ptModeMs",ptModeMs);
PolyDriver* driver = new PolyDriver(options);
//-- Fill a map entry ( drivers.size() if before push_back, otherwise do drivers.size()-1).
//-- Just "i" if resize already performed.
idxFromCanId[ ids.get(i).asInt() ] = i;
//-- Push the motor driver on to the vectors.
nodes[i] = driver;
driver->view( iControlLimitsRaw[i] );
driver->view( iControlModeRaw[i] );
driver->view( iEncodersTimedRaw[i] );
driver->view( iPositionControlRaw[i] );
driver->view( iPositionDirectRaw[i] );
driver->view( iTorqueControlRaw[i] );
driver->view( iVelocityControlRaw[i] );
driver->view( iCanBusSharer[i] );
//-- Associate absolute encoders to motor drivers
if( types.get(i).asString() == "CuiAbsolute" ) {
int driverCanId = ids.get(i).asInt() - 100;
iCanBusSharer[ idxFromCanId[driverCanId] ]->setIEncodersTimedRawExternal( iEncodersTimedRaw[i] );
}
//-- Aditionally sets initial parameters on physical motor drivers.
iCanBusSharer[i]->setCanBusPtr( iCanBus );
}
//-- Set all motor drivers to mode.
if( mode=="position") {
if( ! this->setPositionMode() )
return false;
} else if( mode=="velocity") {
if( ! this->setVelocityMode() )
return false;
} else if( mode=="torque") {
if( ! this->setTorqueMode() )
return false;
} else {
CD_ERROR("Not prepared for initializing in mode %s.\n",mode.c_str());
return false;
}
//-- Check the status of each driver.
std::vector<int> tmp( nodes.size() );
this->getControlModes( tmp.data() );
//-- Initialize the drivers: start (0.1) ready (0.1) on (2) enable. Wait between each step.
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->start() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->readyToSwitchOn() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->switchOn() )
return false;
}
yarp::os::Time::delay(2);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->enable() )
return false;
}
if( config.check("home") ) {
CD_DEBUG("Moving motors to zero.\n");
for(int i=0; i<nodes.size(); i++)
{
if ( ! iPositionControlRaw[i]->positionMoveRaw(0,0) )
return false;
}
for(int i=0; i<nodes.size(); i++)
{
bool motionDone = false;
while( ! motionDone ) {
yarp::os::Time::delay(0.5); //-- [s]
CD_DEBUG("Moving %d to zero...\n",i);
if( ! iPositionControlRaw[i]->checkMotionDoneRaw(0,&motionDone) )
return false;
}
}
CD_DEBUG("Moved motors to zero.\n");
}
if( config.check("reset") ) {
CD_DEBUG("Forcing encoders to zero.\n");
if ( ! this->resetEncoders() )
return false;
}
return true;
}
