void BiasedDelay::processChannelBlock(int size, float* buf, float* delayBuf, int delayBufIdx){
unsigned int sampleDelay = getSampleDelay(getParameterValue(PARAMETER_TIME));
float feedback = getParameterValue(PARAMETER_FEEDBACK);
float bias = getBiasExponent(1 - getParameterValue(PARAMETER_BIAS));
float dryWetMix = getParameterValue(PARAMETER_DRYWET);
for (int i=0; i<size; i++)
{
float delaySample = delayBuf[delayBufIdx];
float v = buf[i] + delaySample * feedback;
v = applyBias(v, bias);
delayBuf[delayBufIdx] = softLimit(v); // Guard: range limit.
buf[i] = sigmoidXFade(buf[i], delaySample, dryWetMix);
delayBufIdx = (++delayBufIdx) % sampleDelay;
}
}
/*
Control loop to manage the biclops joint limits in speed control.
This control loop is running in a seperate thread in order to detect each 5
ms joint limits during the speed control. If a joint limit is detected the
axis should be halted.
\warning Velocity control mode is not exported from the top-level Biclops API
class provided by Traclabs. That means that there is no protection in this
mode to prevent an axis from striking its hard limit. In position mode,
Traclabs put soft limits in that keep any command from driving to a position
too close to the hard limits. In velocity mode this protection does not exist
in the current API.
\warning With the understanding that hitting the hard limits at full
speed/power can damage the unit, damage due to velocity mode commanding is
under user responsibility.
*/
void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
{
vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
int iter = 0;
// PMDAxisControl *panAxis = controller->getPanAxis();
// PMDAxisControl *tiltAxis = controller->getTiltAxis();
vpRobotBiclopsController::shmType shm;
vpDEBUG_TRACE(10, "Start control loop");
vpColVector mes_q;
vpColVector mes_q_dot;
vpColVector softLimit(vpBiclops::ndof);
vpColVector q_dot(vpBiclops::ndof);
bool *new_q_dot = new bool [ vpBiclops::ndof ];
bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
double secure = vpMath::rad(2); // add a security angle before joint limit
// Set the soft limits
softLimit[0] = vpBiclops::panJointLimit - secure;
softLimit[1] = vpBiclops::tiltJointLimit - secure;
vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
vpMath::deg(softLimit[0]),
vpMath::deg(softLimit[1]));
// Initilisation
vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
prev_q_dot [i] = shm.q_dot[i];
new_q_dot [i] = false;
change_dir [i] = false;
force_halt [i] = false;
enable_limit[i] = true;
}
// Initialize actual position and velocity
mes_q = controller->getActualPosition();
mes_q_dot = controller->getActualVelocity();
vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
// Updates the shm
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
shm.actual_q[i] = mes_q[i];
shm.actual_q_dot[i] = mes_q_dot[i];
}
// Update the actuals positions
controller->writeShm(shm);
vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
while (! controller->isStopRequested()) {
// Get actual position and velocity
mes_q = controller->getActualPosition();
mes_q_dot = controller->getActualVelocity();
vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
// Updates the shm
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
shm.actual_q[i] = mes_q[i];
shm.actual_q_dot[i] = mes_q_dot[i];
}
vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
vpMath::deg(mes_q[0]),
vpMath::deg(mes_q[1]));
vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
vpMath::deg(mes_q_dot[0]),
vpMath::deg(mes_q_dot[1]));
vpDEBUG_TRACE(12, "desired q_dot : %f %f",
vpMath::deg(shm.q_dot[0]),
vpMath::deg(shm.q_dot[1]));
vpDEBUG_TRACE(13, "previous q_dot : %f %f",
vpMath::deg(prev_q_dot[0]),
vpMath::deg(prev_q_dot[1]));
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
// test if joint limits are reached
if (mes_q[i] < -softLimit[i]) {
vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
shm.status[i] = vpRobotBiclopsController::STOP;
shm.jointLimit[i] = true;
}
else if (mes_q[i] > softLimit[i]) {
vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
shm.status[i] = vpRobotBiclopsController::STOP;
shm.jointLimit[i] = true;
}
else {
shm.status[i] = vpRobotBiclopsController::SPEED;
shm.jointLimit[i] = false;
}
// Test if new a speed is demanded
//if (shm.q_dot[i] != prev_q_dot[i])
if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
new_q_dot[i] = true;
else
new_q_dot[i] = false;
// Test if desired speed change of sign
if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
change_dir[i] = true;
else
change_dir[i] = false;
}
vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
bool updateVelocity = false;
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
// Test if a new desired speed is to apply
if (new_q_dot[i]) {
// A new desired speed is to apply
if (shm.status[i] == vpRobotBiclopsController::STOP) {
// Axis in joint limit
if (change_dir[i] == false) {
// New desired speed without change of direction
// We go in the joint limit
if (enable_limit[i] == true) { // limit detection active
// We have to stop this axis
// Test if this axis was stopped before
if (force_halt[i] == false) {
q_dot[i] = 0.;
force_halt[i] = true; // indicate that it will be stopped
updateVelocity = true; // We have to send this new speed
}
}
else {
// We have to apply the desired speed to go away the joint
// Update the desired speed
q_dot[i] = shm.q_dot[i];
shm.status[i] = vpRobotBiclopsController::SPEED;
force_halt[i] = false;
updateVelocity = true; // We have to send this new speed
}
}
else {
// New desired speed and change of direction.
if (enable_limit[i] == true) { // limit detection active
// Update the desired speed to go away the joint limit
q_dot[i] = shm.q_dot[i];
shm.status[i] = vpRobotBiclopsController::SPEED;
force_halt[i] = false;
enable_limit[i] = false; // Disable joint limit detection
updateVelocity = true; // We have to send this new speed
}
else {
// We have to stop this axis
// Test if this axis was stopped before
if (force_halt[i] == false) {
q_dot[i] = 0.;
force_halt[i] = true; // indicate that it will be stopped
enable_limit[i] = true; // Joint limit detection must be active
updateVelocity = true; // We have to send this new speed
}
}
}
}
else {
// Axis not in joint limit
// Update the desired speed
q_dot[i] = shm.q_dot[i];
shm.status[i] = vpRobotBiclopsController::SPEED;
enable_limit[i] = true; // Joint limit detection must be active
updateVelocity = true; // We have to send this new speed
}
}
else {
// No change of the desired speed. We have to stop the robot in case of
// joint limit
if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
if (enable_limit[i] == true) { // limit detection active
// Test if this axis was stopped before
if (force_halt[i] == false) {
// We have to stop this axis
q_dot[i] = 0.;
force_halt[i] = true; // indicate that it will be stopped
updateVelocity = true; // We have to send this new speed
}
}
}
else {
// No need to stop the robot
enable_limit[i] = true; // Normal situation, activate limit detection
}
}
}
// Update the actuals positions
controller->writeShm(shm);
vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
if (updateVelocity) {
vpDEBUG_TRACE(12, "apply q_dot : %f %f",
vpMath::deg(q_dot[0]),
vpMath::deg(q_dot[1]));
// Apply the velocity
controller -> setVelocity( q_dot );
}
// Update the previous speed for next iteration
for (unsigned int i=0; i < vpBiclops::ndof; i ++)
prev_q_dot[i] = shm.q_dot[i];
vpDEBUG_TRACE(12, "iter: %d", iter);
//wait 5 ms
vpTime::wait(5.0);
// if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
// vpDEBUG_TRACE (12, "Calling thread will end");
// vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
// std::cout << "Calling thread will end" << std::endl;
// std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
//
// pthread_mutex_unlock(&vpEndThread_mutex);
// break;
// }
iter ++;
}
controller->stopRequest(false);
// Stop the robot
vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
q_dot = 0;
controller -> setVelocity( q_dot );
delete [] new_q_dot;
delete [] change_dir;
delete [] force_halt;
delete [] enable_limit;
vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
pthread_mutex_unlock(&vpEndThread_mutex);
vpDEBUG_TRACE (10, "Exit control thread ");
// pthread_exit(0);
return NULL;
}
