bool simulatedArm::Init(PowerCubeCtrlParams * params)
{
m_DOF = params->GetNumberOfDOF();;
double vmax = 1.0;
double amax = 1.5;
setMaxVelocity(vmax);
setMaxAcceleration(amax);
m_maxVel.resize(m_DOF);
m_maxAcc.resize(m_DOF);
std::vector<double> ul(m_DOF);
std::vector<double> ll(m_DOF);
ul = params->GetUpperLimits();
ll = params->GetLowerLimits();
m_maxVel = params->GetMaxVel();
m_maxAcc = params->GetMaxAcc();
for (int i=0; i < m_DOF; i++)
{
m_motors.push_back( simulatedMotor(ll[i], ul[i], amax, vmax) );
}
std::cerr << "===========Initializing Simulated Powercubes\n";
m_Initialized = true;
return true;
}
void
ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
{
/* Check for max acceleration */
setMaxAcceleration();
/* check if zero input stick */
const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
/* default is acceleration */
ManualIntentionZ intention = ManualIntentionZ::acceleration;
/* check zero input stick */
if (is_current_zero) {
intention = ManualIntentionZ::brake;
}
/*
* update intention
*/
if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
/* we start with lowest acceleration */
_acc_state_dependent = _acc_max_down.get();
/* reset slew-rate: this ensures that there
* is no delay present when user demands to brake
*/
_vel_sp_prev = _vel;
}
switch (intention) {
case ManualIntentionZ::brake: {
/* limit jerk when braking to zero */
float jerk = (_acc_max_up.get() - _acc_state_dependent) / dt;
if (jerk > _jerk_max.get()) {
_acc_state_dependent = _jerk_max.get() * dt + _acc_state_dependent;
} else {
_acc_state_dependent = _acc_max_up.get();
}
break;
}
case ManualIntentionZ::acceleration: {
_acc_state_dependent = (getMaxAcceleration() - _acc_max_down.get())
* fabsf(_stick) + _acc_max_down.get();
break;
}
}
_intention = intention;
}
/*!
* \brief Initializing
*
* Setting paramters initialized by PowerCubeCtrlParams.h
*/
bool PowerCubeCtrl::Init(PowerCubeCtrlParams * params)
{
int ret = 0;
int DOF = m_params->GetDOF();
std::string CanModule = m_params->GetCanModule();
std::string CanDevice = m_params->GetCanDevice();
std::vector<int> ModulIDs = m_params->GetModuleIDs();
int CanBaudrate = m_params->GetBaudrate();
std::vector<double> MaxVel = m_params->GetMaxVel();
std::vector<double> MaxAcc = m_params->GetMaxAcc();
std::vector<double> Offsets = m_params->GetOffsets();
std::vector<double> LowerLimits = m_params->GetLowerLimits();
std::vector<double> UpperLimits = m_params->GetUpperLimits();
/// Output of current settings in the terminal
std::cout << " D O F :" << DOF << std::endl;
m_status.resize(DOF);
m_dios.resize(DOF);
m_positions.resize(DOF);
std::cout << "=========================================================================== " << std::endl;
std::cout << "PowerCubeCtrl:Init: Trying to initialize with the following parameters: " << std::endl;
std::cout << "DOF: " << DOF << std::endl;
std::cout << "CanModule: " << CanModule << std::endl;
std::cout << "CanDevice: " << CanDevice << std::endl;
std::cout << "CanBaudrate: " << CanBaudrate << std::endl;
std::cout << "ModulIDs: ";
for (int i = 0; i < DOF; i++)
{
std::cout << ModulIDs[i] << " ";
}
std::cout << std::endl << "maxVel: ";
for (int i = 0; i < DOF; i++)
{
std::cout << MaxVel[i] << " ";
}
std::cout << std::endl << "maxAcc: ";
for (int i = 0; i < DOF; i++)
{
std::cout << MaxAcc[i] << " ";
}
std::cout << std::endl << "upperLimits: ";
for (int i = 0; i < DOF; i++)
{
std::cout << UpperLimits[i] << " ";
}
std::cout << std::endl << "lowerLimits: ";
for (int i = 0; i < DOF; i++)
{
std::cout << LowerLimits[i] << " ";
}
std::cout << std::endl << "offsets: ";
for (int i = 0; i < DOF; i++)
{
std::cout << Offsets[i] << " ";
}
std::cout << std::endl << "=========================================================================== " << std::endl;
std::ostringstream InitStr;
InitStr << CanModule << ":" << CanDevice << "," << CanBaudrate;
std::cout << "initstring = " << InitStr.str().c_str() << std::endl;
/// open device
pthread_mutex_lock(&m_mutex);
ret = PCube_openDevice(&m_DeviceHandle, InitStr.str().c_str());
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
std::ostringstream errorMsg;
errorMsg << "Could not open device " << CanDevice << ", m5api error code: " << ret;
m_ErrorMessage = errorMsg.str();
return false;
}
m_CANDeviceOpened = true;
/// reset all modules
pthread_mutex_lock(&m_mutex);
ret = PCube_resetAll(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
std::ostringstream errorMsg;
errorMsg << "Could not reset all modules, m5api error code: " << ret;
m_ErrorMessage = errorMsg.str();
return false;
}
/// make sure m_IdModules is clear of Elements:
ModulIDs.clear();
/// check number of modules connected to the bus
pthread_mutex_lock(&m_mutex);
int number_of_modules = PCube_getModuleCount(m_DeviceHandle);
pthread_mutex_unlock(&m_mutex);
std::cout << "found " << number_of_modules << " modules." << std::endl;
/// Check if the modules are connected
for (int i = 0; i < DOF; i++)
{
unsigned long serNo;
pthread_mutex_lock(&m_mutex);
ret = PCube_getModuleSerialNo(m_DeviceHandle, ModulIDs[i], &serNo);
pthread_mutex_unlock(&m_mutex);
if (ret != 0)
{
std::ostringstream errorMsg;
errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
m_ErrorMessage = errorMsg.str();
return false;
}
/// otherwise success
std::cout << "Found module " << ModulIDs[i] << std::endl;
}
// modules should be initialized now
m_pc_status = PC_CTRL_OK;
// check if modules are in normal state
std::vector<std::string> errorMessages;
PC_CTRL_STATUS status;
getStatus(status, errorMessages);
if ((status != PC_CTRL_OK) && (status != PC_CTRL_NOT_HOMED))
{
m_ErrorMessage.assign("");
for (int i = 0; i < DOF; i++)
{
m_ErrorMessage.append(errorMessages[i]);
m_ErrorMessage.append("\n");
}
return false;
}
else if (status == PC_CTRL_NOT_HOMED)
{
std::cout << "PowerCubeCtrl:Init: Homing is executed ...\n";
bool successful = false;
successful = doHoming();
if (!successful)
{
std::cout << "PowerCubeCtrl:Init: homing not successful, aborting ...\n";
return false;
}
}
/// Set angle offsets to hardware
for (int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
//std::cout << "------------------------------> PCube_setHomeOffset()" << std::endl;
PCube_setHomeOffset(m_DeviceHandle, ModulIDs[i], Offsets[i]);
pthread_mutex_unlock(&m_mutex);
}
/// Set limits to hardware
for (int i = 0; i < DOF; i++)
{
pthread_mutex_lock(&m_mutex);
PCube_setMinPos(m_DeviceHandle, ModulIDs[i], LowerLimits[i]);
pthread_mutex_unlock(&m_mutex);
pthread_mutex_lock(&m_mutex);
PCube_setMaxPos(m_DeviceHandle, ModulIDs[i], UpperLimits[i]);
pthread_mutex_unlock(&m_mutex);
}
/// Set max velocity to hardware
setMaxVelocity(MaxVel);
/// Set max acceleration to hardware
setMaxAcceleration(MaxAcc);
/// set synchronous or asynchronous movements
setSyncMotion();
//setASyncMotion();
// All modules initialized successfully
m_pc_status = PC_CTRL_OK;
m_Initialized = true;
return true;
}
SCurveProfile::SCurveProfile(double maxV, double maxA, double timeToMaxA) {
setMaxVelocity(maxV);
setMaxAcceleration(maxA);
setTimeToMaxA(timeToMaxA);
}
