template<> complex __power(complex a, complex b) {
complex r;
double vabs, len, at, phase;
if(b.real == 0 and b.imag == 0) {
r.real = 1;
r.imag = 0;
}
else if(a.real == 0 and a.imag == 0) {
r.real = 0;
r.imag = 0;
}
else {
vabs = __abs(a);
len = std::pow(vabs,b.real);
at = std::atan2(a.imag, a.real);
phase = at*b.real;
if (b.imag != 0.0) {
len /= std::exp(at*b.imag);
phase += b.imag*std::log(vabs);
}
r.real = len*std::cos(phase);
r.imag = len*std::sin(phase);
}
return r;
}
Fraction::Fraction(double f, int digits)
{
double log_f = log10(__abs(f));
int counter = 0;
l = 1;
while(log_f < digits && counter < digits)
{
f *= 10;
log_f += 1;
counter += 1;
l *= 10;
}
u = f;
lg_u = log10(__abs((double)u));
lg_l = counter;
is_sharp = false;
}
Fraction operator + (Fraction const &a, Fraction const &b)
{
Fraction r;
if(a.lg_l + b.lg_u > 18 || a.lg_u + b.lg_l > 18 || a.lg_l + b.lg_l > 19)
{
long double tmp1 = (long double) a.u / a.l;
long double tmp2 = (long double) b.u / b.l;
long double tmp = tmp1 + tmp2;
long double log_tmp = log10(__abs(tmp));
int counter = 0;
r.l = 1;
while(log_tmp < 18 && counter < 18)
{
tmp *= 10;
log_tmp += 1;
counter += 1;
r.l *= 10;
}
r.u = (int64)tmp;
r.lg_u = log10(__abs((double)r.u));
r.lg_l = counter;
r.is_sharp = false;
}
else
{
r.u = a.u * b.l + b.u * a.l;
r.l = a.l * b.l;
r.lg_u = log10(__abs((double)r.u));
r.lg_l = a.lg_l + b.lg_l;
r.is_sharp = a.is_sharp && b.is_sharp;
}
r.smart();
return r;
}
Fraction Fraction::to_decimal()
{
long double tmp = (long double)u / l;
long double log_tmp = log10(__abs(tmp));
int counter = 0;
l = 1;
while(log_tmp < 8 && counter < 8)
{
tmp *= 10;
log_tmp += 1;
counter += 1;
l *= 10;
}
u = (int64)tmp;
lg_u = log10(__abs((double)u));
lg_l = counter;
is_sharp = false;
return *this;
}
Point Point::upload(Fraction const &X, Fraction const &Y)
{
int64 tmp = gcd(X.getL(), Y.getL());
double log_tmp = log10((double)_abs(tmp));
if(X.lg_l + Y.lg_u > 19 + log_tmp || X.lg_u + Y.lg_l > 19 + log_tmp || X.lg_l + Y.lg_l > 19 + log_tmp)
{
double tmp1 = (double) X.u / X.l;
double tmp2 = (double) Y.u / Y.l;
double log_tmp1 = log10(__abs((double)tmp1));
double log_tmp2 = log10(__abs(tmp2));
int counter = 0;
s = 1;
while(log_tmp1 < 18 && log_tmp2 < 18 && counter < 18)
{
tmp1 *= 10;
tmp2 *= 10;
log_tmp1 += 1;
log_tmp2 += 1;
counter += 1;
s *= 10;
}
x = (int64)tmp1;
y = (int64)tmp2;
is_sharp = false;
}
else
{
x = X.getU() * (Y.getL() / tmp);
y = Y.getU() * (X.getL() / tmp);
s = X.getL() * (Y.getL() / tmp);
is_sharp = X.is_sharp && Y.is_sharp;
}
smart();
return *this;
}
float accum = 0.;
float tmp = x * x * x;
accum += (10 * tmp);
tmp *= x;
accum -= (15 * tmp);
tmp *= x;
accum += (6 * tmp);
return accum;
}
Int16 init_trajectory (byte jj, Int32 current, Int32 final, Int16 speed)
{
float currentf = current;
float finalf = final;
float speedf = __abs(speed);
//if (!_ended[jj] || speed <= 0)
if (speed <= 0)
return -1;
_x0[jj] = current;
_xf[jj] = final;
_distance[jj] = _xf[jj] - _x0[jj];
_tf[jj] = 100 *__labs (_distance[jj]) / speedf;
_tf[jj] /= (float)_period;
_stepf[jj] = 1 / _tf[jj];
if (_tf[jj] < 1)
{
bool operator ! () const { return __abs().v < 0.00001; }
/************************************************************
* this function checks if the calibration is terminated
* and if calibration is terminated resets the encoder
************************************************************/
void check_in_position_calib(byte jnt)
{
Int32 temporary_long;
bool temporary_cond1 = true;
bool temporary_cond2 = true;
bool temporary_cond3 = true;
static int time_passed_counter = 0;
// increase the counter for the calibration (wait for the movement to start)
_counter_calib +=1;
// time_passed_counter is used to obtain the position of some time ago and check if the joint moved
time_passed_counter +=1;
if (time_passed_counter > 20)
{
time_passed_counter = 0;
_position_of_some_time_ago[jnt] =_position[jnt] ;
}
// three different type of calibration:
// 1: MODE_CALIB_ABS_POS_SENS
temporary_long = (Int32) extract_h(_filt_abs_pos[jnt]);
temporary_cond1 = temporary_cond1 && (_control_mode[jnt] == MODE_CALIB_ABS_POS_SENS);
temporary_cond1 = temporary_cond1 && (__abs( temporary_long - _abs_pos_calibration[jnt]) < INPOSITION_CALIB_THRESHOLD);
temporary_cond1 = temporary_cond1 && _ended[jnt];
// 2: MODE_CALIB_HARD_STOPS
temporary_cond2 = temporary_cond2 && (_control_mode[jnt] == MODE_CALIB_HARD_STOPS);
temporary_cond2 = temporary_cond2 &&(_position[jnt] == _position_of_some_time_ago[jnt]) && (time_passed_counter == 19);
temporary_cond2 = temporary_cond2 && (_counter_calib > 1200);
// 3: MODE_CALIB_ABS_AND_INCREMENTAL
temporary_cond3 = temporary_cond3 && (_control_mode[jnt] == MODE_CALIB_ABS_AND_INCREMENTAL);
temporary_cond3 = temporary_cond3 && _ended[jnt];
if (temporary_cond1 | temporary_cond2 | temporary_cond3)
{
#if VERSION != 0x0119
_control_mode[jnt] = MODE_POSITION;
set_position_encoder (jnt, 0);
#endif
#if ((VERSION == 0x0128) || (VERSION == 0x0228))
if (jnt!=0)
{
_max_position_enc[jnt] = _max_position_enc_tmp[jnt];
_min_position_enc[1]=-1800; //Thumb proximal right and left
if (_max_position_enc[2]>0)
_min_position_enc[2]=-3000; //Thumb distal
else
_min_position_enc[2]=3000; //Thumb distal
_min_position_enc[3]=-300; //Index proximal right and left
_calibrated[jnt] = true;
return;
}
#elif ((VERSION == 0x0130) || (VERSION == 0x0230))
{
_max_position_enc[jnt] = _max_position_enc_tmp[jnt];
if (_max_position_enc[0]>0)
_min_position_enc[0]=-3000; //Index distal
else
_min_position_enc[0]=3000; //Index distal
_min_position_enc[1]=-300; //Middle proximal right and left
if (_max_position_enc[2]>0)
_min_position_enc[2]=-3000; //Middle distal
else
_min_position_enc[2]=3000; //Middle distal
_min_position_enc[3]=0; //little fingers right and left
_calibrated[jnt] = true;
return;
}
#elif VERSION ==0x0119
_control_mode[jnt] = MODE_POSITION;
if (jnt == 0)
{
set_position_encoder (jnt, 0);
}
if (jnt==1)
{
set_position_encoder (2, _position[2]);
set_position_encoder (1, 0);
_position[2] = _position[1] + _position[2];
}
if (jnt==2)
{
set_position_encoder (2, -_position[1]);
_position[2] = _position[1] + _position[2];
}
#endif
_position[jnt] = 0;
_position_old[jnt] = 0;
_integral[jnt] = 0;
//Keep the system in the current configuration
_set_point[jnt] = _position[jnt];
init_trajectory (jnt, _position[jnt], _position[jnt], 1);
_calibrated[jnt] = true;
}
}
