diff --git a/src/liboncilla-hw/synchronization/SBCPQueue.cpp b/src/liboncilla-hw/synchronization/SBCPQueue.cpp
index 3a507b7..266ff56 100644
--- a/src/liboncilla-hw/synchronization/SBCPQueue.cpp
+++ b/src/liboncilla-hw/synchronization/SBCPQueue.cpp
@@ -1,283 +1,307 @@
/***
* \file SBCPQueue.cpp
*
* \date Jun 27, 2013
* \author Alexandre Tuleu
*/
#include "SBCPQueue.h"
#include <biorob-cpp/log/Logger.h>
#include <libsbcp/utils/Config.h>
#include <liboncilla-hw/config/Config.h>
namespace liboncilla {
namespace hw {
SBCPQueue::SBCPQueue(const Config & config)
: Queue(config.Main().SBCPPriority(), true)
, d_bus() {
sbcp::Config sbcpConfig;
sbcpConfig.LoadAllFiles();
// create links to the 4 motorboards
unsigned int requiredFrameSize = 4; // 4 consecutive packets send at oncefrom roboard to master communication board
unsigned int requiredPacketSize = 32; // Size of the biggest packet you will send, the actual maximum might be higher
d_bus = sbcpConfig.OpenDefaultBusWithFrame(requiredFrameSize,
requiredPacketSize);
if (!d_bus) {
throw std::runtime_error("SBCPQueue::SBCPQueue() Can't load bus due to missing configuration.");
} else {
log(debug, "Bus Workflow: ", d_bus->CurrentWorkflow());
}
const MotorDriverGroup & devices = config.Motors().Devices();
const BrushlessParameterGroup & motorConfig = config.Motors().Params();
int16_t timestep = config.Main().Timestep();
d_motordrivers[rci::oncilla::LEFT_FORE] =
OpenAndConfigureMotorDriver(devices.LeftFore(), motorConfig, timestep);
d_motordrivers[rci::oncilla::RIGHT_FORE] =
OpenAndConfigureMotorDriver(devices.RightFore(), motorConfig, timestep);
d_motordrivers[rci::oncilla::LEFT_HIND] =
OpenAndConfigureMotorDriver(devices.LeftHind(), motorConfig, timestep);
d_motordrivers[rci::oncilla::RIGHT_HIND] =
OpenAndConfigureMotorDriver(devices.RightHind(), motorConfig, timestep);
}
SBCPQueue::~SBCPQueue() {
}
void SBCPQueue::DownstreamData() {
for (MotorAndEncoderByL1L2::const_iterator motAndEnc = d_motAndEncByL1L2.begin();
motAndEnc != d_motAndEncByL1L2.end();
++motAndEnc) {
motAndEnc->second.motor->GoalPosition().Set(motAndEnc->first->nodeToQueueJointPosition());
}
}
void SBCPQueue::UpstreamData() {
for (MotorAndEncoderByL1L2::const_iterator motAndEnc = d_motAndEncByL1L2.begin();
motAndEnc != d_motAndEncByL1L2.end();
++motAndEnc) {
uint16_t pos(motAndEnc->second.encoder->PositionAndStatus().Get() & 0x3fff);
uint16_t status((motAndEnc->second.encoder->PositionAndStatus().Get() & 0xc000) >> 14);
int16_t motPos(motAndEnc->second.motor->PresentPosition().Get());
motAndEnc->first->queueToNodeJointPosition(pos, status, motPos);
}
for (MotorDriverByL0::const_iterator mdv = d_mdvByL0.begin();
mdv != d_mdvByL0.end();
++mdv) {
if (mdv->second == NULL) {
// TODO: Throw? std::cout << "SBCPQueue::InitializeIO: Ignoring motor driver "<<md->second<<std::endl;
} else {
mdv->first->queueToNodeForces(mdv->second->Force(0).Get(),
mdv->second->Force(1).Get(),
mdv->second->Force(2).Get());
}
}
for (MagneticEncoderByL3::const_iterator enc = d_encByL3.begin();
enc != d_encByL3.end();
++enc) {
if (enc->second == NULL) {
// TODO: Throw? std::cout << "SBCPQueue::InitializeIO: Ignoring motor driver "<<md->second<<std::endl;
} else {
enc->first->queueToNodeJointPosition(enc->second->PositionAndStatus().Get() & 0x3fff,
(enc->second->PositionAndStatus().Get() & 0xc000) >> 14);
}
}
}
void SBCPQueue::PerformIO() {
sbcp::ScheduledWorkflow & w = this->d_bus->Scheduled();
// Start transfer
w.StartTransfers();
// Wait for transfer to complete
w.WaitForTransfersCompletion();
}
void SBCPQueue::InitializeIO() {
// Enable scheduled woirkflow
sbcp::ScheduledWorkflow & w = this->d_bus->Scheduled();
// Append devices to transfer
for (MotordriverByLeg::iterator md = d_motordrivers.begin();
md != d_motordrivers.end();
++md) {
if (md->second == NULL) {
log(debug, "SBCPQueue::InitializeIO: Ignoring motor driver ",
LegPrefix(md->first) );
} else {
log( debug, "Appending scheduled device " , LegPrefix(md->first) );
w.AppendScheduledDevice(std::tr1::static_pointer_cast<sbcp::Device>(md->second));
}
}
}
void SBCPQueue::DeinitializeIO() {
// Disable scheduled woirkflow
this->d_bus->Lazy();
}
void SBCPQueue::RegisterL0(rci::oncilla::Leg l, const L0::Ptr & node) {
// check if this map contains this leg
MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
if (fi == d_motordrivers.end()) {
throw(std::runtime_error("Failed to open motordriver, upon initialization of this Queue, for motordriver "
+ LegPrefix(l)));
}
d_mdvByL0[node] = fi->second;
}
void SBCPQueue::RegisterL1(rci::oncilla::Leg l, const L1L2::Ptr & node) {
bool isLeftLeg =
((l == rci::oncilla::LEFT_FORE) || (l == rci::oncilla::LEFT_HIND)) ?
true : false;
bool isHip = true;
//check if this map contains this leg
MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
if (fi == d_motordrivers.end() || !fi->second) {
throw(std::runtime_error("Failed to open motordriver earlier, upon initialization of this Queue, for motordriver "
+ LegPrefix(l)));
}
node->initialize(isLeftLeg, isHip,
fi->second->Motor1().PositionLimit().Get() & 0x7fff,
fi->second->Motor1().PresentPosition().Get());
/// \todo hardcoded mapping, could be configurable
MotorAndEncoder m1;
m1.motor = &(fi->second->Motor1());
m1.encoder = &(fi->second->Q1());
d_motAndEncByL1L2[node] = m1;
}
void SBCPQueue::RegisterL2(rci::oncilla::Leg l, const L1L2::Ptr & node) {
bool isRightLeg = ((l == rci::oncilla::RIGHT_FORE) || (l == rci::oncilla::RIGHT_HIND)) ? true
: false;
bool isHip = false;
//check if this map contains this leg
MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
if (fi == d_motordrivers.end() || !fi->second) {
throw(std::runtime_error("Failed to open motordriver earlier, upon initialization of this Queue, for motordriver "
+ LegPrefix(l)));
}
node->initialize(isRightLeg,
isHip,
fi->second->Motor2().PositionLimit().Get() & 0x7fff,
fi->second->Motor2().PresentPosition().Get());
/// \todo hardcoded mapping, could be configurable
MotorAndEncoder m2;
m2.motor = &(fi->second->Motor2());
m2.encoder = &(fi->second->Q2());
d_motAndEncByL1L2[node] = m2;
}
void SBCPQueue::RegisterL3(rci::oncilla::Leg l, const L3::Ptr & node) {
//check if this map contains this leg
MotordriverByLeg::const_iterator fi = d_motordrivers.find(l);
if (fi == d_motordrivers.end() || !fi->second) {
throw(std::runtime_error("Failed to open motordriver, upon initialization of this Queue, for motordriver "
+ LegPrefix(l)));
}
d_encByL3[node] = &(fi->second->Q3());
}
+bool AllMotorsCalibrated(const sbcp::amarsi::MotorDriver::Ptr & mdv) {
+ return mdv->Motor1().RuntimeCalibrated() && mdv->Motor2().RuntimeCalibrated();
+ }
+
+void SBCPQueue::CalibrateMotorDrivers() {
+ typedef std::set< sbcp::amarsi::MotorDriver::Ptr> SetOfMDV;
+
+ SetOfMDV uncalibrated;
+ for(MotordriverByLeg::const_iterator mdv = d_motordrivers.begin();
+ mdv != d_motordrivers.end();
+ ++mdv) {
+ if (!mdv->second) {
+ continue;
+ }
+ if (AllMotorsCalibrated(mdv->second)) {
+ log(debug, LegPrefix(mdv->first), " motors are already runtime calibrated");
+ } else {
+ log(debug, "Calibrating ", LegPrefix(mdv->first), " motors");
+ mdv->second->CalibrateMotors();
+ uncalibrated.insert(mdv->second);
+ }
+ }
+
+
+ while(!uncalibrated.empty()) {
+ usleep(1e5);
+ SetOfMDV toTest = uncalibrated;
+ for(SetOfMDV::const_iterator mdv = toTest.begin();
+ mdv != toTest.end();
+ ++mdv) {
+ if(AllMotorsCalibrated(*mdv)) {
+ log(debug,"All motor of board ", (int)(*mdv)->ID(), " calibrated");
+ uncalibrated.erase(*mdv);
+ }
+ }
+
+ }
+
+ log(debug, "All motors calibrated");
+
+}
+
sbcp::amarsi::MotorDriver::Ptr
SBCPQueue::OpenAndConfigureMotorDriver(const MotorDriverSection & def,
const BrushlessParameterGroup & params,
int16_t expectedTsInMs) {
log(debug, "SBCPQueue::OpenAndConfigureMotorDriver()");
if (!d_bus) {
throw std::logic_error("SBCPQueue::d_bus private member should be initialized in"
"constructor. Please report this internal bug");
}
this->d_bus->Lazy();
sbcp::amarsi::MotorDriver::Ptr res = d_bus->OpenDevice< sbcp::amarsi::MotorDriver > (def.BoardID());
if (!res) {
// board is just not here, buit we will throw later if it is required.
log(debug, "Could not find an AMARSi motordriver board with ID ", (int)def.BoardID(), "on the bus");
return res;
}
- bool m1Calibrated(GetCalibrationStatus(res->Motor1(),1));
- bool m2Calibrated(GetCalibrationStatus(res->Motor2(),2));
- if(! (m1Calibrated || m2Calibrated)){
- res->CalibrateMotors();
- } else {
- log(debug, "Motor already calibrated");
- }
-
- while(!(m1Calibrated && m2Calibrated)){
- usleep(1e5);
- m1Calibrated = GetCalibrationStatus(res->Motor1(), 1);
- m2Calibrated = GetCalibrationStatus(res->Motor2(), 2);
- }
- log(debug, "Motor calibrated");
SetMotorParameters(params, def.M1Params(), expectedTsInMs, res->Motor1());
SetMotorParameters(params, def.M2Params(), expectedTsInMs, res->Motor2());
return res;
}
void SBCPQueue::SetMotorParameters(const BrushlessParameterGroup & paramGroup,
const std::string & paramName, int16_t expectedTsInMs,
sbcp::amarsi::MotorDriver::Motor & motor) {
log(debug, "SBCPQueue::SetMotorParameters()" );
//due to a bad design of biorob-cpp dynamic section we should do this
std::tr1::shared_ptr<BrushlessParameterSection> params =
const_cast<BrushlessParameterGroup &>(paramGroup).SubSection(paramName);
if (!params) {
throw std::runtime_error("Unknown motor parameter group '" + paramName + "'");
}
motor.MotorControlMode().Set(sbcp::amarsi::MotorDriver::Motor::SMOOTH_POSITION);
motor.PGain().Set(params->PGain());
motor.IGain().Set(params->IGain());
motor.DGain().Set(params->DGain());
motor.Stiffness().Set(params->Stiffness());
motor.Damping().Set(params->Damping());
motor.Preload().Set(params->PreCompression());
motor.MaxTorque().Set(params->MaxTorque());
motor.MaxSpeed().Set(params->MaxSpeed());
motor.MaxAcceleration().Set(params->MaxAcceleration());
motor.SmoothPositionUpdate().Set(expectedTsInMs);
}
-bool SBCPQueue::GetCalibrationStatus(sbcp::amarsi::MotorDriver::Motor & m, int number) {
- uint16_t val(m.MotorControlMode().Get());
- return val & (1 << 9);
-}
} /* namespace hw */
} /* namespace liboncilla */
diff --git a/src/liboncilla-hw/synchronization/SBCPQueue.h b/src/liboncilla-hw/synchronization/SBCPQueue.h
index 628f9da..c50fc8f 100644
--- a/src/liboncilla-hw/synchronization/SBCPQueue.h
+++ b/src/liboncilla-hw/synchronization/SBCPQueue.h
@@ -1,82 +1,80 @@
/*
* \file SBCPQueue.h
*
* \date Jun 27, 2013
* \author Alexandre Tuleu
*/
#pragma once
#include "Queue.h"
#include <liboncilla-hw/nodes/Nodes.h>
#include <libsbcp/bus/Bus.h>
#include <libsbcp/bus/ScheduledWorkflow.h>
#include <libsbcp/devices/amarsi/MotorDriver.h>
namespace liboncilla {
namespace hw {
class Config;
class BrushlessParameterGroup;
class MotorDriverSection;
class SBCPQueue : public Queue {
public:
SBCPQueue(const liboncilla::hw::Config & config);
virtual ~SBCPQueue();
virtual void DownstreamData();
virtual void UpstreamData();
virtual void PerformIO();
virtual void InitializeIO();
virtual void DeinitializeIO();
void RegisterL0(rci::oncilla::Leg l, const L0::Ptr & node);
void RegisterL1(rci::oncilla::Leg l, const L1L2::Ptr & node);
void RegisterL2(rci::oncilla::Leg l, const L1L2::Ptr & node);
void RegisterL3(rci::oncilla::Leg l, const L3::Ptr & node);
void CalibrateMotorDrivers();
private:
//std::tr1::shared_ptr<sbcp::amarsi::MotorDriver> d_motordrivers[4];
typedef std::map<liboncilla::hw::L0::Ptr, sbcp::amarsi::MotorDriver::Ptr> MotorDriverByL0;
struct MotorAndEncoder {
sbcp::amarsi::MotorDriver::Motor * motor;
sbcp::amarsi::MotorDriver::MagneticEncoder * encoder;
};
typedef std::map<liboncilla::hw::L1L2::Ptr, MotorAndEncoder> MotorAndEncoderByL1L2;
typedef std::map<liboncilla::hw::L3::Ptr,
sbcp::amarsi::MotorDriver::MagneticEncoder *> MagneticEncoderByL3;
typedef std::map<rci::oncilla::Leg, sbcp::amarsi::MotorDriver::Ptr> MotordriverByLeg;
void SetMotorParameters(const BrushlessParameterGroup & paramGroup,
const std::string & paramName,
int16_t expectedTsInMs,
sbcp::amarsi::MotorDriver::Motor & motor);
sbcp::amarsi::MotorDriver::Ptr
OpenAndConfigureMotorDriver(const MotorDriverSection & def,
const BrushlessParameterGroup & config,
int16_t expectedTsInMs);
- inline bool GetCalibrationStatus(sbcp::amarsi::MotorDriver::Motor &, int);
-
MotorDriverByL0 d_mdvByL0;
MotorAndEncoderByL1L2 d_motAndEncByL1L2;
MagneticEncoderByL3 d_encByL3;
MotordriverByLeg d_motordrivers;
std::tr1::shared_ptr<sbcp::Bus> d_bus;
};
} /* namespace hw */
} /* namespace liboncilla */
diff --git a/src/liboncilla-hw/synchronization/Synchronizer.cpp b/src/liboncilla-hw/synchronization/Synchronizer.cpp
index cd081c0..f3a4176 100644
--- a/src/liboncilla-hw/synchronization/Synchronizer.cpp
+++ b/src/liboncilla-hw/synchronization/Synchronizer.cpp
@@ -1,217 +1,217 @@
#include "Synchronizer.h"
#include <sys/mman.h>
#include <signal.h>
#include <errno.h>
#include <liboncilla-hw/xenomai-utils/Utils.h>
#include <liboncilla-hw/config/Config.h>
namespace liboncilla {
namespace hw {
Synchronizer::Synchronizer(const Config & config)
: rci::oncilla::Synchronizer("Oncilla HW Synchronizer")
, d_timestep(config.Main().Timestep() * 1.0e-3)
, d_priority(config.Main().MainPriority())
, d_sbcpQueue(config)
, d_rbioQueue(config.Main().RBIOPriority()) {
CheckMainConfig(config.Main());
Init();
//TODO bad bad pratice new queue should be added automatically to this list.
d_queues.push_back(&d_sbcpQueue);
d_queues.push_back(&d_rbioQueue);
}
void Synchronizer::Init() {
InitRT();
InitModules();
}
void Synchronizer::InitRT() {
xeno_call(rt_task_shadow, NULL, NULL, d_priority, T_FPU);
}
void Synchronizer::CheckPriority(unsigned int p, const std::string & name) {
if (p > 99) {
std::ostringstream os;
os << "Unsupported " << name << " of " << p
<< " only value between 0 and 99 are supported";
throw std::runtime_error(os.str());
}
}
void Synchronizer::CheckMainConfig(const MainSection& config) {
std::ostringstream os;
if (config.Timestep() < 2) {
os << "Unsupported timestep of " << config.Timestep()
<< " ms. It should be >= 2ms";
throw std::runtime_error(os.str());
}
CheckPriority(config.MainPriority(), "main priority");
CheckPriority(config.SBCPPriority(), "sbcp priority");
CheckPriority(config.RBIOPriority(), "rbio priority");
}
void Synchronizer::InitModules() {
}
Synchronizer::~Synchronizer() {
}
void Synchronizer::calibrateIfNeeded() {
-
+ d_sbcpQueue.CalibrateMotorDrivers();
}
bool Synchronizer::start() {
d_firstStepped = false;
RT_EVENT * e = new RT_EVENT();
xeno_call(rt_event_create, e, Queue::EventName, 0, EV_PRIO);
d_event = NativeHolder<RT_EVENT>(e);
for (ListOfQueue::const_iterator q = d_queues.begin();
q != d_queues.end();
++q) {
(*q)->StartTask();
}
d_idleQueueMask = Queue::AllQueueMask();
bool res = rci::oncilla::Synchronizer::start();
xeno_call(rt_task_set_periodic, NULL, TM_NOW, d_timestep * 1e9);
return res;
}
bool Synchronizer::stop() {
for (ListOfQueue::const_iterator q = d_queues.begin();
q != d_queues.end();
++q) {
(*q)->StopTask();
}
d_event = NativeHolder<RT_EVENT>();
return rci::Synchronizer::stop();
}
double Synchronizer::lastProcessTimeStep() const {
if (!d_firstStepped) {
return 0.0;
}
return d_timestep * (d_overruns + 1);
}
void Synchronizer::ProcessAsyncPrimpl() {
for (ListOfQueue::const_iterator q = d_queues.begin();
q != d_queues.end();
++q) {
if (IsFinished(**q)) {
(*q)->DownstreamData();
}
}
WakeIdleQueues();
d_firstStepped = true;
}
void Synchronizer::WaitForProcessAsyncPrimpl() {
int res;
unsigned int maxTrials(20);
while (true) {
res = rt_task_wait_period(&d_overruns);
if (res != 0 && res != -ETIMEDOUT) {
xeno_throw_error_from(rt_task_wait_period, res);
}
FetchIdleQueues();
//we ensure that all sbcp communication are done !
if (IsFinished(d_sbcpQueue)) {
break;
}
--maxTrials;
if (maxTrials == 0) {
std::ostringstream os;
os << "SBCP communication did not finished after " << maxTrials
<< " timesteps. We abort everything";
throw std::runtime_error(os.str());
}
}
for (ListOfQueue::const_iterator q = d_queues.begin();
q != d_queues.end();
++q) {
if (IsFinished(**q)) {
(*q)->UpstreamData();
}
}
}
void Synchronizer::RegisterTrunk(const Trunk::Ptr& node) {
/// \todo implement
}
void Synchronizer::RegisterL0(rci::oncilla::Leg l, const L0::Ptr& node) {
d_rbioQueue.RegisterL0(l, node);
d_sbcpQueue.RegisterL0(l, node);
}
void Synchronizer::RegisterL1(rci::oncilla::Leg l, const L1L2::Ptr& node) {
if (!node) {
std::ostringstream os;
os << "Ignoring L1 node of leg " << l << ". Is it connected?";
throw std::runtime_error(os.str());
}
d_sbcpQueue.RegisterL1(l, node);
}
void Synchronizer::RegisterL2(rci::oncilla::Leg l, const L1L2::Ptr& node) {
if (!node) {
std::ostringstream os;
os << "Ignoring L2 node of leg " << l << ". Is it connected?";
throw std::runtime_error(os.str());
}
d_sbcpQueue.RegisterL2(l, node);
}
void Synchronizer::RegisterL3(rci::oncilla::Leg l, const L3::Ptr& node) {
if (!node) {
std::ostringstream os;
os << "Ignoring L3 node of leg " << l << ". Is it connected?";
throw std::runtime_error(os.str());
}
d_sbcpQueue.RegisterL3(l, node);
}
void Synchronizer::WakeIdleQueues() {
xeno_call(rt_event_signal, d_event.get(), d_idleQueueMask);
}
void Synchronizer::FetchIdleQueues() {
xeno_call(rt_event_wait,
d_event.get(),
0,
&d_idleQueueMask,
EV_ANY,
TM_NONBLOCK);
d_idleQueueMask ^= Queue::AllQueueMask();
}
bool Synchronizer::IsFinished(const Queue& q) {
return q.Mask() & d_idleQueueMask;
}
} //namespace hw
} //namespace liboncilla