Beispiel #1
0
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;
}
Beispiel #2
0
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;
}
Beispiel #3
0
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;
}
Beispiel #4
0
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;
}
Beispiel #5
0
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;
}
Beispiel #6
0
	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)
	{
Beispiel #7
0
			bool operator ! () const { return __abs().v < 0.00001; }
Beispiel #8
0
/************************************************************
 * 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;
    }
}