diff --git a/src/libsbcp/devices/amarsi/MotorDriver.cpp b/src/libsbcp/devices/amarsi/MotorDriver.cpp index a661a7c..62a6baf 100644 --- a/src/libsbcp/devices/amarsi/MotorDriver.cpp +++ b/src/libsbcp/devices/amarsi/MotorDriver.cpp @@ -1,276 +1,279 @@ /** * \file MotorDriver.cpp * * \date Apr 10, 2012 * \author Alexandre Tuleu */ #include <iostream> #include <sstream> #include "MotorDriver.h" using namespace sbcp; using namespace sbcp::amarsi; MotorDriver::MotorDriver(Bus & bus, DevID id, FirmwareVersion version,SBCPVersion comVersion) : Device(bus,Klass,id,version,comVersion) , d_m1 (*this,MOTOR_1) , d_m2 (*this,MOTOR_2){ d_q[0] = MagneticEncoder::Ptr(new MagneticEncoder(*this,ME_Q1)); d_q[1] = MagneticEncoder::Ptr(new MagneticEncoder(*this,ME_Q2)); d_q[2] = MagneticEncoder::Ptr(new MagneticEncoder(*this,ME_Q3)); d_force[0] = RegisterReader<Data>::Ptr(new RegisterReader<Data>(*this, - FORCE_AXIS_1)); + FORCE_AXIS_1, + sbcp::Register::LL_READABLE)); d_force[1] = RegisterReader<Data>::Ptr(new RegisterReader<Data>(*this, - FORCE_AXIS_2)); + FORCE_AXIS_2, + sbcp::Register::LL_READABLE)); d_force[2] = RegisterReader<Data>::Ptr(new RegisterReader<Data>(*this, - FORCE_AXIS_3)); + FORCE_AXIS_3, + sbcp::Register::LL_READABLE)); ListOfWriters inputs; ListOfReaders outputs; inputs.push_back(&d_m1.GoalPosition()); inputs.push_back(&d_m2.GoalPosition()); outputs.push_back(&d_m1.PresentPosition()); outputs.push_back(&d_m2.PresentPosition()); outputs.push_back(&d_q[0]->PositionAndStatus()); outputs.push_back(&d_q[1]->PositionAndStatus()); outputs.push_back(&d_q[2]->PositionAndStatus()); outputs.push_back(d_force[0].get()); outputs.push_back(d_force[1].get()); outputs.push_back(d_force[2].get()); EnablesLowLatencyPacket(inputs,outputs); } MotorDriver::~MotorDriver(){ } void MotorDriver::CalibrateMotors(){ if(GetBus().CurrentWorkflow() == Bus::SCHEDULED){ throw std::runtime_error("could not calibrate motor in scheduled workflow"); } LazyWorkflow & w = GetBus().Lazy(); w.Prepare(Device::Klass(),ID(),CALIBRATE,0); const Byte * res; PayloadSize size; w.Submit(&res,size); w.Release(); d_m1.GoalPosition().Get(); d_m2.GoalPosition().Get(); } void MotorDriver::UncalibrateMotors(){ if(GetBus().CurrentWorkflow() == Bus::SCHEDULED){ throw std::runtime_error("Should be in Lazy mode"); } LazyWorkflow & w = GetBus().Lazy(); w.Prepare(Device::Klass(),ID(),UNCALIBRATE,0); const Byte * res; PayloadSize size; w.Submit(&res,size); w.Release(); d_m1.GoalPosition().Get(); d_m2.GoalPosition().Get(); } MotorDriver::MagneticEncoder & MotorDriver::Q1(){ return *(d_q[0]); } MotorDriver::MagneticEncoder & MotorDriver::Q2(){ return *(d_q[1]); } MotorDriver::MagneticEncoder & MotorDriver::Q3(){ return *(d_q[2]); } MotorDriver::Motor::Motor(Device & dev, sbcp::Register::Address start) - : d_mode(dev,start + STATUS_AND_CONTROL_MODE) + : d_mode(dev,start + STATUS_AND_CONTROL_MODE, sbcp::Register::LL_READABLE) , d_goalPosition(dev,start + GOAL_POSITION) , d_goalVelocity(dev,start + GOAL_SPEED) - , d_presentPosition(dev,start + PRESENT_POSITION) - , d_presentVelocity(dev,start + PRESENT_SPEED) - , d_targetedTorque(dev,start + TARGETED_TORQUE) + , d_presentPosition(dev,start + PRESENT_POSITION, sbcp::Register::LL_READABLE) + , d_presentVelocity(dev,start + PRESENT_SPEED, sbcp::Register::LL_READABLE) + , d_targetedTorque(dev,start + TARGETED_TORQUE, sbcp::Register::LL_READABLE) , d_pgain(dev,start + P_GAIN) , d_igain(dev,start + I_GAIN) , d_dgain(dev,start + D_GAIN) , d_stiffness(dev,start + COMPLIANCE_STIFFNESS) , d_damping(dev,start + COMPLIANCE_DAMPING) , d_preload(dev,start + COMPLIANCE_PRELOAD) , d_MaxTorque(dev,start + MAX_TORQUE) , d_MaxAcceleration(dev,start + MAX_ACCELERATION) , d_MaxSpeed(dev,start + MAX_SPEED) , d_gearMult(dev, start + GEAR_MULT, sbcp::Register::PERSISTENT) , d_gearDiv(dev,start + GEAR_DIV, sbcp::Register::PERSISTENT) , d_positionLimit(dev,start + LIMIT_POS_CW, sbcp::Register::PERSISTENT) , d_regionMin(dev,start + CALIBRATION_REGION_MIN,sbcp::Register::PERSISTENT) , d_regionMax(dev,start + CALIBRATION_REGION_MAX,sbcp::Register::PERSISTENT) , d_calibrationOffset(dev,start + CALIBRATION_OFFSET,sbcp::Register::PERSISTENT) , d_SmoothUpdate(dev,start + SMOOTH_POSITION_UPDATE) { //dev.AddRegister(start + TARGETED_POSITION,DeviceRegister::READABLE); //dev.AddRegister(start + TARGETED_SPEED,DeviceRegister::READABLE); } MotorDriver::Motor::~Motor(){} RegisterAccessor<double,int16_t> & MotorDriver::Motor::GoalPosition(){ return d_goalPosition; } RegisterAccessor<double,int16_t> & MotorDriver::Motor::GoalVelocity(){ return d_goalVelocity; } RegisterReader<double,int16_t> & MotorDriver::Motor::PresentPosition(){ return d_presentPosition; } RegisterReader<double,int16_t> & MotorDriver::Motor::PresentVelocity(){ return d_presentVelocity; } RegisterReader<double> & MotorDriver::Motor::TargetedTorque(){ return d_targetedTorque; } RegisterAccessor<Data> & MotorDriver::Motor::PGain(){ return d_pgain; } RegisterAccessor<Data> & MotorDriver::Motor::IGain(){ return d_igain; } RegisterAccessor<Data> & MotorDriver::Motor::DGain(){ return d_dgain; } RegisterAccessor<Data> & MotorDriver::Motor::Stiffness(){ return d_stiffness; } RegisterAccessor<Data> & MotorDriver::Motor::Damping(){ return d_damping; } RegisterAccessor<int16_t> & MotorDriver::Motor::Preload(){ return d_preload; } RegisterAccessor<MotorDriver::Motor::ControlMode> & MotorDriver::Motor::MotorControlMode(){ return d_mode; } RegisterAccessor<Data> & MotorDriver::Motor::MaxTorque(){ return d_MaxTorque; } RegisterAccessor<Data>& MotorDriver::Motor::MaxAcceleration(){ return d_MaxAcceleration; } RegisterReader<Data>& MotorDriver::Force(unsigned int dimension){ if(dimension >= 3 ){ throw std::range_error("only have 3d force sensor !!!!"); } return *(d_force[dimension]); } MotorDriver::MagneticEncoder::MagneticEncoder(Device & dev, sbcp::Register::Address start) - : d_positionAndStatus(dev,start + POSITION_AND_STATUS) + : d_positionAndStatus(dev,start + POSITION_AND_STATUS, sbcp::Register::LL_READABLE) , d_gearMult(dev,start + GEAR_MULT, sbcp::Register::PERSISTENT) , d_gearDiv(dev,start + GEAR_DIV, sbcp::Register::PERSISTENT){ } RegisterReader<Data> & MotorDriver::MagneticEncoder::PositionAndStatus(){ return d_positionAndStatus; } RegisterAccessor<int16_t> & MotorDriver::MagneticEncoder::GearMultiple(){ return d_gearMult; } RegisterAccessor<int16_t> & MotorDriver::MagneticEncoder::GearDivisor(){ return d_gearDiv; } RegisterAccessor<Data>& MotorDriver::Motor::MaxSpeed(){ return d_MaxSpeed; } RegisterAccessor<Data>& MotorDriver::Motor::SmoothPositionUpdate(){ return d_SmoothUpdate; } RegisterAccessor<Data> & MotorDriver::Motor::GearMult(){ return d_gearMult; } RegisterAccessor<Data> & MotorDriver::Motor::GearDiv(){ return d_gearDiv; } RegisterAccessor<Data> & MotorDriver::Motor::PositionLimit(){ return d_positionLimit; } RegisterAccessor<Data> & MotorDriver::Motor::RegionMin(){ return d_regionMin; } RegisterAccessor<Data> & MotorDriver::Motor::RegionMax(){ return d_regionMax; } RegisterAccessor<int16_t> & MotorDriver::Motor::CalibrationOffset(){ return d_calibrationOffset; } MotorDriver::MagneticEncoder & MotorDriver::Q(unsigned int i){ if(i > 2){ std::ostringstream os; os << "Invalid index (" << i << ")for magnetic encoder."; throw std::range_error(os.str()); } return *(d_q[i]); } bool MotorDriver::Motor::RuntimeCalibrated(){ return d_mode.Get() & (RUNTIME_CALIBRATED << 8); }