Esempio n. 1
0
double lgammafn(NMATH_STATE *state, double x)
{
    double ans, y, sinpiy;

/* For IEEE double precision DBL_EPSILON = 2^-52 = 2.220446049250313e-16 :
   xmax  = DBL_MAX / log(DBL_MAX) = 2^1024 / (1024 * log(2)) = 2^1014 / log(2)
   dxrel = sqrt(DBL_EPSILON) = 2^-26 = 5^26 * 1e-26 (is *exact* below !)
 */
#define xmax  2.5327372760800758e+305
#define dxrel 1.490116119384765696e-8

//    R_signgam = 1;

#ifdef IEEE_754
    if(ISNAN(x)) return x;
#endif

    if (x < 0 && fmod(floor(-x), 2.) == 0)
//	R_signgam = -1;

    if (x <= 0 && x == trunc(x)) { /* Negative integer argument */
	printf("lgammafn: range error"); 
	return INFINITY;/* +Inf, since lgamma(x) = log|gamma(x)| */
    }

    y = fabs(x);

    if (y <= 10)
	return log(fabs(gammafn(state,x)));
    /*
      ELSE  y = |x| > 10 ---------------------- */

    if (y > xmax) {
	printf("lgammafn: range error"); 
	return INFINITY;
    }

    if (x > 0) { /* i.e. y = x > 10 */
#ifdef IEEE_754
	if(x > 1e17)
	    return(x*(log(x) - 1.));
	else if(x > 4934720.)
	    return(M_LN_SQRT_2PI + (x - 0.5) * log(x) - x);
	else
#endif
	    return M_LN_SQRT_2PI + (x - 0.5) * log(x) - x + lgammacor(x);
    }
    /* else: x < -10; y = -x */
    sinpiy = fabs(sin(M_PI * y));

    if (sinpiy == 0) { /* Negative integer argument ===
			  Now UNNECESSARY: caught above */
	printf(" ** should NEVER happen! *** [lgamma.c: Neg.int, y=%g]\n",y);
	return NAN;
    }

    ans = M_LN_SQRT_PId2 + (x - 0.5) * log(y) - x - log(sinpiy) - lgammacor(y);

    if(fabs((x - trunc(x - 0.5)) * ans / x) < dxrel) {

	/* The answer is less than half precision because
	 * the argument is too near a negative integer. */

	printf("lgammafn: precision error"); 
    }

    return ans;
}
Esempio n. 2
0
 void Dodge::onLeave()
 {
     State::onLeave();
     unsigned int shieldVal = static_cast<unsigned int>(trunc(_entity->_shieldMaxValue/5));
     _entity->_shieldValue = _entity->_shieldValue < shieldVal ? 0 : _entity->_shieldValue - shieldVal;
 }
Esempio n. 3
0
float pgeMathTrunc(float x)
{
	return trunc(x);
}
Esempio n. 4
0
int64_t float_to_nsec(double t){
  return trunc(t * NANO_SCALE_FLOAT);
}
Esempio n. 5
0
  void output(vsx_module_param_abs* param)
  {
    VSX_UNUSED(param);
    if (text_in->updates)
    {
      if (process_lines())
      text_in->updates = 0;
    }
    if (text_alpha->get() <= 0)
      return;

    if (!ftfont)
    {
      user_message = "module||error loading font "+cur_font;
      return;
    }

    if (text_in->get() == "_")
      return;


    float obj_size = size->get();

    gl_state->matrix_mode (VSX_GL_MODELVIEW_MATRIX );
    gl_state->matrix_push();

    gl_state->matrix_rotate_f( (float)angle->get()*360, rotation_axis->get(0), rotation_axis->get(1), rotation_axis->get(2) );

    if (obj_size < 0)
      obj_size = 0;

    gl_state->matrix_scale_f( obj_size*0.8*0.01, obj_size*0.01, obj_size*0.01 );

    int l_align = align->get();
    float l_leading = leading->get();
    float ypos = 0;

    if (cur_render_type == 0)
      glEnable(GL_TEXTURE_2D);

    glColor4f(red->get(),green->get(),blue->get(),text_alpha->get());

    for (unsigned long i = 0; i < lines.size(); ++i)
    {
      float ll = limit_line->get();
      if (ll != -1.0f)
      {
        if (trunc(ll) != i) continue;
      }
      gl_state->matrix_push();
      if (l_align == 0)
      {
        gl_state->matrix_translate_f( 0, ypos, 0 );
      } else
      if (l_align == 1)
      {
        gl_state->matrix_translate_f( -lines[i].size_x*0.5f,ypos,0 );
      }
      if (l_align == 2)
      {
        gl_state->matrix_translate_f( -lines[i].size_x,ypos,0 );
      }

      if (cur_render_type == 1)
      {
        if (outline_alpha->get() > 0.0f && ftfont2) {
          float pre_linew;
          pre_linew = gl_state->line_width_get();
          gl_state->line_width_set( outline_thickness->get() );
          glColor4f(outline_color->get(0),outline_color->get(1),outline_color->get(2),outline_alpha->get()*outline_color->get(3));
          ftfont2->Render(lines[i].string.c_str());
          gl_state->line_width_set( pre_linew );
        }
        glColor4f(red->get(),green->get(),blue->get(),text_alpha->get());
      }

      ftfont->Render(lines[i].string.c_str());
      gl_state->matrix_pop();
      ypos += l_leading;
    }

    if (cur_render_type == 0)
      glDisable(GL_TEXTURE_2D);


    gl_state->matrix_pop();

    render_result->set(1);
    loading_done = true;
  }
Esempio n. 6
0
static int rpn_eval(const token* t)
{
  double o1, o2;

  switch (t->type) {
  case MATHOP_NUMBER:
    return push_number(t->value);

  case MATHOP_ADD:
    return pop_number(&o2) || pop_number(&o1) || push_number(o1 + o2);

  case MATHOP_SUB:
    return pop_number(&o2) || pop_number(&o1) || push_number(o1 - o2);

  case MATHOP_MUL:
    return pop_number(&o2) || pop_number(&o1) || push_number(o1 * o2);

  case MATHOP_DIV:
    return pop_number(&o2) || pop_number(&o1) || push_number(o1 / o2);

  case MATHOP_NEG:
    return pop_number(&o1) || push_number(-o1);

  case MATHOP_DROP:
    return pop_number(&o1);

  case MATHOP_DUP:
    if(pop_number(&o1)) return -1;
    return push_number(o1) || push_number(o1);

  case MATHOP_SWAP:
    return pop_number(&o2) || pop_number(&o1) || push_number(o2) || push_number(o1);

  case MATHOP_MOD:
    return pop_number(&o2) || pop_number(&o1) || push_number(fmod(o1,o2));

  case MATHOP_POW:
    return pop_number(&o2) || pop_number(&o1) || push_number(pow(o1,o2));

  case MATHOP_EXP:
    return pop_number(&o1) || push_number(exp(o1));

  case MATHOP_LOG10:
    return pop_number(&o1) || push_number(log10(o1));

  case MATHOP_LN:
    return pop_number(&o1) || push_number(log(o1));

  case MATHOP_ABS:
    return pop_number(&o1) || push_number(fabs(o1));

  case MATHOP_SQRT:
    return pop_number(&o1) || push_number(sqrt(o1));

  case MATHOP_CBRT:
    return pop_number(&o1) || push_number(cbrt(o1));

  case MATHOP_FLOOR:
    return pop_number(&o1) || push_number(floor(o1));

  case MATHOP_CEIL:
    return pop_number(&o1) || push_number(ceil(o1));

  case MATHOP_ROUND:
    return pop_number(&o1) || push_number(round(o1));

  case MATHOP_NEARBYINT:
    return pop_number(&o1) || push_number(nearbyint(o1));

  case MATHOP_TRUNC:
    return pop_number(&o1) || push_number(trunc(o1));

  case MATHOP_E:
    return push_number(M_E);

  case MATHOP_PI:
    return push_number(M_PI);

  default:
    LM_WARN("Invalid RPN token type\n");
    return -1;
  }
}
Esempio n. 7
0
static uint8_t _parse_gcode_block(char *buf) 
{
	uint8_t i=0; 	 			// persistent index into Gcode block buffer (buf)
  	char letter;				// parsed letter, eg.g. G or X or Y
	double value;				// value parsed from letter (e.g. 2 for G2)
	uint8_t status = TG_OK;

	// set initial state for new move 
	memset(&gp, 0, sizeof(gp));	// clear all parser values
	memset(&gf, 0, sizeof(gf));	// clear all next-state flags
	memset(&gn, 0, sizeof(gn));	// clear all next-state values
	gn.motion_mode = cm_get_motion_mode();	// get motion mode from previous block

  	// extract commands and parameters
	while((status = _get_next_statement(&letter, &value, buf, &i)) == TG_OK) {

		switch(letter) {
			case 'G':
				switch((uint8_t)value) {
					case 0:  SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE);
					case 1:  SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED);
					case 2:  SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC);
					case 3:  SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC);
					case 4:  SET_NON_MODAL (next_action, NEXT_ACTION_DWELL);
					case 10: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA);
					case 17: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XY);
					case 18: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XZ);
					case 19: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_YZ);
					case 20: SET_MODAL (MODAL_GROUP_G6, units_mode, INCHES);
					case 21: SET_MODAL (MODAL_GROUP_G6, units_mode, MILLIMETERS);
					case 28: {
						switch (_point(value)) {
							case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GO_HOME);
							case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SEARCH_HOME); 
							default: status = TG_UNRECOGNIZED_COMMAND;
						}
						break;
					}
					case 40: break;	// ignore cancel cutter radius compensation
					case 49: break;	// ignore cancel tool length offset comp.
					case 53: SET_NON_MODAL (absolute_override, true);
					case 54: SET_MODAL (MODAL_GROUP_G12, coord_system, G54);
					case 55: SET_MODAL (MODAL_GROUP_G12, coord_system, G55);
					case 56: SET_MODAL (MODAL_GROUP_G12, coord_system, G56);
					case 57: SET_MODAL (MODAL_GROUP_G12, coord_system, G57);
					case 58: SET_MODAL (MODAL_GROUP_G12, coord_system, G58);
					case 59: SET_MODAL (MODAL_GROUP_G12, coord_system, G59);
					case 61: {
						switch (_point(value)) {
							case 0: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_PATH);
							case 1: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_STOP); 
							default: status = TG_UNRECOGNIZED_COMMAND;
						}
						break;
					}
					case 64: SET_MODAL (MODAL_GROUP_G13,path_control, PATH_CONTINUOUS);
					case 80: SET_MODAL (MODAL_GROUP_G1, motion_mode,  MOTION_MODE_CANCEL_MOTION_MODE);
					case 90: SET_MODAL (MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE);
					case 91: SET_MODAL (MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE);
					case 92: {
						switch (_point(value)) {
							case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_ORIGIN_OFFSETS);
							case 1: SET_NON_MODAL (next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS);
							case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS);
							case 3: SET_NON_MODAL (next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS); 
							default: status = TG_UNRECOGNIZED_COMMAND;
						}
						break;
					}
					case 93: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, true);
					case 94: SET_MODAL (MODAL_GROUP_G5, inverse_feed_rate_mode, false);
					default: status = TG_UNRECOGNIZED_COMMAND;
				}
				break;

			case 'M':
				switch((uint8_t)value) {
					case 0: case 1: 
							SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_STOP);
					case 2: case 30: case 60:
							SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_END);
					case 3: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CW);
					case 4: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW);
					case 5: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF);
					case 6: SET_NON_MODAL (change_tool, true);
					case 7: SET_MODAL (MODAL_GROUP_M8, mist_coolant, true);
					case 8: SET_MODAL (MODAL_GROUP_M8, flood_coolant, true);
					case 9: SET_MODAL (MODAL_GROUP_M8, flood_coolant, false);
					case 48: SET_MODAL (MODAL_GROUP_M9, feed_override_enable, true);
					case 49: SET_MODAL (MODAL_GROUP_M9, feed_override_enable, false);
					default: status = TG_UNRECOGNIZED_COMMAND;
				}
				break;

			case 'T': SET_NON_MODAL (tool, (uint8_t)trunc(value));
			case 'F': SET_NON_MODAL (feed_rate, value);
			case 'P': SET_NON_MODAL (dwell_time, value); 	// also used as G10 coord system select
			case 'S': SET_NON_MODAL (spindle_speed, value); 
			case 'X': SET_NON_MODAL (target[X], value);
			case 'Y': SET_NON_MODAL (target[Y], value);
			case 'Z': SET_NON_MODAL (target[Z], value);
			case 'A': SET_NON_MODAL (target[A], value);
			case 'B': SET_NON_MODAL (target[B], value);
			case 'C': SET_NON_MODAL (target[C], value);
		//	case 'U': SET_NON_MODAL (target[U], value);		// reserved
		//	case 'V': SET_NON_MODAL (target[V], value);		// reserved
		//	case 'W': SET_NON_MODAL (target[W], value);		// reserved
			case 'I': SET_NON_MODAL (arc_offset[0], value);
			case 'J': SET_NON_MODAL (arc_offset[1], value);
			case 'K': SET_NON_MODAL (arc_offset[2], value);
			case 'R': SET_NON_MODAL (arc_radius, value);
			case 'N': SET_NON_MODAL (linenum,(uint32_t)value);// line number
			case 'L': break;								// not used for anything
			default: status = TG_UNRECOGNIZED_COMMAND;
		}
		if(status != TG_OK) break;
	}
	if ((status != TG_OK) && (status != TG_COMPLETE)) return (status);
	ritorno(_check_gcode_block());			// perform Gcode error checking
	return (_execute_gcode_block());		// if successful execute the block
}
Esempio n. 8
0
// This procedure assumes that poly is monic and that babyStep contains
// at least k+delta powers, where delta = deg(poly) mod k
static long 
recursivePolyEval(const ZZX& poly, long k, DynamicPtxtPowers& babyStep,
		  DynamicPtxtPowers& giantStep, long mod,
		  long& recursiveDepth)
{
  if (deg(poly)<=babyStep.size()) { // Edge condition, use simple eval
    long ret = simplePolyEval(poly, babyStep, mod);
    recursiveDepth = babyStep.getDepth(deg(poly));
    return ret;
  }

  if (verbose) cerr << "recursivePolyEval("<<poly<<")\n";

  long delta = deg(poly) % k; // deg(poly) mod k
  long n = divc(deg(poly),k); // ceil( deg(poly)/k )
  long t = 1L<<(NextPowerOfTwo(n)); // t >= n, so t*k >= deg(poly)

  // Special case for deg(poly) = k * 2^e +delta
  if (n==t)
    return degPowerOfTwo(poly, k, babyStep, giantStep, mod, recursiveDepth);

  // When deg(poly) = k*(2^e -1) we use the Paterson-Stockmeyer recursion
  if (n == t-1 && delta==0)
    return PatersonStockmeyer(poly, k, t/2, delta,
			      babyStep, giantStep, mod, recursiveDepth);

  t = t/2;

  // In any other case we have kt < deg(poly) < k(2t-1). We then set 
  // u = deg(poly) - k*(t-1) and poly = q*X^u + r with deg(r)<u
  // and recurse on poly = (q-1)*X^u + (X^u+r)

  long u = deg(poly) - k*(t-1);
  ZZX r = trunc(poly, u);      // degree <= u-1
  ZZX q = RightShift(poly, u); // degree == k*(t-1)
  q -= 1;
  SetCoeff(r, u);              // degree == u

  long ret, tmp;
  long subDepth1=0, subDepth2=0;
  if (verbose) 
    cerr << " {deg(poly)="<<deg(poly)<<"<k*(2t-1)="<<k*(2*t-1)
	 << "} recursing on "<<r<<" + X^"<<u<<"*"<<q<<endl;
  ret = PatersonStockmeyer(q, k, t/2, 0, 
			   babyStep, giantStep, mod, subDepth1);

  if (verbose) {
    cerr << "  PatersonStockmeyer("<<q<<") returns "<<ret<<", depth="<<subDepth1<<endl;
    if (ret != polyEvalMod(q,babyStep[0], mod)) {
      cerr << "  @@1st recursive call failed, q="<<q
     	   << ", ret="<<ret<<"!=" << polyEvalMod(q,babyStep[0], mod)<<endl;
      exit(0);
    }
  }

  tmp = giantStep.getPower(u/k);
  subDepth2 = giantStep.getDepth(u/k);
  if (delta!=0) { // if u is not divisible by k then compute it
    if (verbose) 
      cerr <<"  multiplying by X^"<<u
	   <<"=giantStep.getPower("<<(u/k)<<")*babyStep.getPower("<<delta<<")="
	   << giantStep.getPower(u/k)<<"*"<<babyStep.getPower(delta)
	   << "="<<tmp<<endl;
    tmp = MulMod(tmp, babyStep.getPower(delta), mod);
    nMults++;
    subDepth2++;
  }
  ret = MulMod(ret, tmp, mod);
  nMults ++;
  subDepth1 = max(subDepth1, subDepth2)+1;

  if (verbose) cerr << "  after mult by X^{k*"<<u<<"+"<<delta<<"}, depth="<< subDepth1<<endl;

  tmp = recursivePolyEval(r, k, babyStep, giantStep, mod, subDepth2);
  if (verbose)
    cerr << "  recursivePolyEval("<<r<<") returns "<<tmp<<", depth="<<subDepth2<<endl;
  if (tmp != polyEvalMod(r,babyStep[0], mod)) {
    cerr << "  @@2nd recursive call failed, r="<<r
	 << ", ret="<<tmp<<"!=" << polyEvalMod(r,babyStep[0], mod)<<endl;
    exit(0);
  }
  recursiveDepth = max(subDepth1, subDepth2);
  return AddMod(ret, tmp, mod);
}
Esempio n. 9
0
static CYTHON_INLINE struct ZZX* ZZX_truncate(struct ZZX* x, long m)
{
    ZZX* t = new ZZX();
    trunc(*t, *x, m);
    return t;
}
Esempio n. 10
0
int main(){	
	int chave = 0;	
	FILE *arq, *arqo, *arqw;
	
	arq = fopen("entrada-transposicao.txt", "r");
	arqo = fopen("texto-original.txt", "r");
	arqw = fopen("saida-transposicao.txt", "w");
	
	if(arq == NULL) printf("Não abriu");
	
	char dados[10000], dados1[10000];
	fgets(dados, 10000, arq);
	fgets(dados1, 10000, arqo);
	
	int i=0, t, j, ic; 
	
	for(i=0; i<strlen(dados1); i++){
		if(dados1[i]==10) dados1[i] = '\0';
	}
	
	if(dados == NULL) printf("Não leu fgets");
	
	
	for(t=1; t<256; t++){
		char *chavx = (char *)malloc(sizeof(char)*strlen(dados));
		int tamanho_codificado = strlen(dados);
		int ldecod = trunc(tamanho_codificado / t); //quantidade de linhas
		int cdecod = trunc(tamanho_codificado / ldecod);
			
		int mdecod[cdecod][ldecod];
		ic = 0;
		int cc=0, cl=0;
		int max_linha = strlen(dados) / t; //t vai ser a chave agora
		int inc = 0;
		for(i=0; i<cdecod; i++){
			for(j=0; j<ldecod; j++){
				if(dados[inc]!=10){
					mdecod[i][j] = dados[inc++];
				}
			}
		}	
		
		inc = 0;
		for(i=0; i<ldecod; i++){
			for(j=0; j<cdecod; j++){
				chavx[inc++] = mdecod[j][i];
			}
		}
		int ehc = 0;
		for(i=strlen(chavx)-1; i>0; i--){
			if(chavx[i]!=' ') break;
			if(chavx[i]==' ') chavx[i] = '\0';
		}
		if(strcmp(chavx, dados1)==0){
			printf("---------------\nChave descoberta: %d\n--------------------\n", t);
			break;
		}else{
			//printf("chavx =>> [%s]==[%s]\n", chavx, dados1);
		}
	}
	
	
	/*int i = 0, j = 0, tamanho_linhas = 0, icol = 0, ilinha = 0;
	tamanho_linhas = trunc(strlen(dados) / chave) + 1;
	int matrix[tamanho_linhas][chave];
	for(i=0; i<tamanho_linhas; i++){
		for(j=0; j<chave; j++){
			matrix[i][j] = ' '; //"Limpar" a matriz
		}
	}	
	for(i=0; i<strlen(dados); i++){
		int k = (int)dados[i];
		if(k!=10) matrix[ilinha][icol] = k; //ignorar a quebra de linha
		//int gerado = codificar(k, chave);
		//int decodificado = decodificar(gerado, chave);
		//printf(">%c - %c\n", gerado, decodificado);
		icol++;
		if(icol >= chave){
			icol = 0;
			ilinha++;
		}
	}
	
	printf("--------------%dx%d----------------\n", tamanho_linhas, chave);
	for(i=0; i<tamanho_linhas; i++){
		for(j=0; j<chave; j++){
			printf("%c", matrix[i][j]);
		}
		printf("\n");
	}
	printf("------------------------------\n");
	
	printf("Texto codificado: \n");
	char codificado[tamanho_linhas*chave];
	int ic = 0;
	for(i=0; i<chave; i++){
		for(j=0; j<tamanho_linhas; j++){
			printf("%c", matrix[j][i]);
			codificado[ic++] = matrix[j][i];
		}
	}
	printf("\n");
	printf("=================================\nTexto decodificado:\n");
	int tamanho_codificado = strlen(codificado);
	int ldecod = tamanho_codificado / chave; //quantidade de linhas
	int cdecod = tamanho_codificado / ldecod;
	int mdecod[cdecod][ldecod];
	ic = 0;
	int cc=0, cl=0;
	for(i=0; i<strlen(codificado); i++){
		mdecod[cc][cl] = codificado[i];
		cc++;
		if(cc>=chave){
			cc=0;
			cl++;
		}
	}
	for(i=0; i<ldecod; i++){
		for(j=0; j<cdecod; j++){
			printf("%c", matrix[i][j]);
		}
	}
	printf("\n========================================\n"); */
	return 0; 
}
Esempio n. 11
0
// This procedure assumes that poly is monic, deg(poly)=k*(2t-1)+delta
// with t=2^e, and that babyStep contains k+delta powers
static long 
PatersonStockmeyer(const ZZX& poly, long k, long t, long delta,
		   DynamicPtxtPowers& babyStep, DynamicPtxtPowers& giantStep,
		   long mod, long& recursiveDepth)
{
  if (verbose) cerr << "PatersonStockmeyer("<<poly<<"), k="<<k<<", t="<<t<<endl;

  if (deg(poly)<=babyStep.size()) { // Edge condition, use simple eval
    long ret = simplePolyEval(poly, babyStep, mod);
    recursiveDepth = babyStep.getDepth(deg(poly));
    return ret;
  }
  long subDepth1=0, subDepth2=0;
  long ret, tmp;

  ZZX r = trunc(poly, k*t);      // degree <= k*2^e-1
  ZZX q = RightShift(poly, k*t); // degree == k(2^e-1) +delta

  if (verbose) cerr << "  r ="<<r<< ", q="<<q;

  const ZZ& coef = coeff(r,deg(q));
  SetCoeff(r, deg(q), coef-1);  // r' = r - X^{deg(q)}

  if (verbose) cerr << ", r'="<<r;

  ZZX c,s;
  DivRem(c,s,r,q); // r' = c*q + s
  // deg(s)<deg(q), and if c!= 0 then deg(c)<k-delta

  if (verbose) cerr << ", c="<<c<< ", s ="<<s<<endl;

  assert(deg(s)<deg(q));
  assert(IsZero(c) || deg(c)<k-delta);
  SetCoeff(s,deg(q)); // s' = s + X^{deg(q)}, deg(s)==deg(q)

  // reduce the coefficients modulo mod
  for (long i=0; i<=deg(c); i++) rem(c[i],c[i], to_ZZ(mod));
  c.normalize();
  for (long i=0; i<=deg(s); i++) rem(s[i],s[i], to_ZZ(mod));
  s.normalize();

  if (verbose) cerr << " {t==n+1} recursing on "<<s<<" + (X^"<<t*k<<"+"<<c<<")*"<<q<<endl;

  // Evaluate recursively poly = (c+X^{kt})*q + s'
  tmp = simplePolyEval(c, babyStep, mod);
  tmp = AddMod(tmp, giantStep.getPower(t), mod);
  subDepth1 = max(babyStep.getDepth(deg(c)), giantStep.getDepth(t));

  ret = PatersonStockmeyer(q, k, t/2, delta,
			   babyStep, giantStep, mod, subDepth2);

  if (verbose) {
    cerr << "  PatersonStockmeyer("<<q<<") returns "<<ret
	 << ", depth="<<subDepth2<<endl;
    if (ret != polyEvalMod(q,babyStep[0], mod)) {
      cerr << "  **1st recursive call failed, q="<<q<<endl;
      exit(0);
    }
  }
  ret = MulMod(ret, tmp, mod);
  nMults++;
  subDepth1 = max(subDepth1, subDepth2)+1;

  tmp = PatersonStockmeyer(s, k, t/2, delta,
			   babyStep, giantStep, mod, subDepth2);
  if (verbose) {
    cerr << "  PatersonStockmeyer("<<s<<") returns "<<tmp
	 << ", depth="<<subDepth2<<endl;
    if (tmp != polyEvalMod(s,babyStep[0], mod)) {
      cerr << "  **2nd recursive call failed, s="<<s<<endl;
      exit(0);
    }
  }
  ret = AddMod(ret,tmp,mod);
  recursiveDepth = max(subDepth1, subDepth2);
  return ret;
}
Esempio n. 12
0
int main() {

   /* Host/device data structures */
   cl_device_id device;
   cl_context context;
   cl_command_queue queue;
   cl_program program;
   cl_kernel kernel_init, kernel_stage_0, kernel_stage_n, kernel_merge,
         kernel_merge_last;
   cl_int i, err, check, direction;

   /* Data and buffers */
   float data[NUM_FLOATS];
   cl_mem data_buffer;
   cl_uint stage, high_stage, num_stages;
   size_t local_size, global_size;

   /* Initialize data */
   srand(time(NULL));
   for(i=0; i<NUM_FLOATS; i++) {
      data[i] = rand();
   }

   /* Create a device and context */
   device = create_device();
   context = clCreateContext(NULL, 1, &device, NULL, NULL, &err);
   if(err < 0) {
      perror("Couldn't create a context");
      exit(1);   
   }

   /* Build the program */
   program = build_program(context, device, PROGRAM_FILE);

   /* Create kernels */
   kernel_init = clCreateKernel(program, BSORT_INIT, &err);
   if(err < 0) {
      perror("Couldn't create the initial kernel");
      exit(1);   
   };
   kernel_stage_0 = clCreateKernel(program, BSORT_STAGE_0, &err);
   if(err < 0) {
      perror("Couldn't create the stage_0 kernel");
      exit(1);   
   };
   kernel_stage_n = clCreateKernel(program, BSORT_STAGE_N, &err);
   if(err < 0) {
      perror("Couldn't create the stage_n kernel");
      exit(1);   
   };
   kernel_merge = clCreateKernel(program, BSORT_MERGE, &err);
   if(err < 0) {
      perror("Couldn't create the merge kernel");
      exit(1);   
   };
   kernel_merge_last = clCreateKernel(program, BSORT_MERGE_LAST, &err);
   if(err < 0) {
      perror("Couldn't create the merge_last kernel");
      exit(1);   
   };

   /* Determine maximum work-group size */
   err = clGetKernelWorkGroupInfo(kernel_init, device, CL_KERNEL_WORK_GROUP_SIZE,
      sizeof(local_size), &local_size, NULL);
   if(err < 0) {
      perror("Couldn't find the maximum work-group size");
      exit(1);   
   };
   local_size = (int)pow(2, trunc(log2(local_size))); 


   /* Create buffer */
   data_buffer = clCreateBuffer(context, CL_MEM_READ_WRITE |
         CL_MEM_COPY_HOST_PTR, sizeof(data), data, &err);
   if(err < 0) {
      perror("Couldn't create a buffer");
      exit(1);   
   };

   /* Create kernel argument */
   err = clSetKernelArg(kernel_init, 0, sizeof(cl_mem), &data_buffer);
   err |= clSetKernelArg(kernel_stage_0, 0, sizeof(cl_mem), &data_buffer);
   err |= clSetKernelArg(kernel_stage_n, 0, sizeof(cl_mem), &data_buffer);
   err |= clSetKernelArg(kernel_merge, 0, sizeof(cl_mem), &data_buffer);
   err |= clSetKernelArg(kernel_merge_last, 0, sizeof(cl_mem), &data_buffer);
   if(err < 0) {
      printf("Couldn't set a kernel argument");
      exit(1);
   };

   /* Create kernel argument */
   err = clSetKernelArg(kernel_init, 1, 8*local_size*sizeof(float), NULL);
   err |= clSetKernelArg(kernel_stage_0, 1, 8*local_size*sizeof(float), NULL);
   err |= clSetKernelArg(kernel_stage_n, 1, 8*local_size*sizeof(float), NULL);
   err |= clSetKernelArg(kernel_merge, 1, 8*local_size*sizeof(float), NULL);
   err |= clSetKernelArg(kernel_merge_last, 1, 8*local_size*sizeof(float), NULL);
   if(err < 0) {
      printf("Couldn't set a kernel argument");
      exit(1);
   };

   /* Create a command queue */
   queue = clCreateCommandQueue(context, device, 0, &err);
   if(err < 0) {
      perror("Couldn't create a command queue");
      exit(1);   
   };

   /* Enqueue initial sorting kernel */
   global_size = NUM_FLOATS/8;
   if(global_size < local_size) {
      local_size = global_size;
   }
   err = clEnqueueNDRangeKernel(queue, kernel_init, 1, NULL, &global_size, 
         &local_size, 0, NULL, NULL); 
   if(err < 0) {
      perror("Couldn't enqueue the kernel");
      exit(1);   
   }

   /* Execute further stages */
   num_stages = global_size/local_size;
   for(high_stage = 2; high_stage < num_stages; high_stage <<= 1) {

      err = clSetKernelArg(kernel_stage_0, 2, sizeof(int), &high_stage);      
      err |= clSetKernelArg(kernel_stage_n, 3, sizeof(int), &high_stage);
      if(err < 0) {
         printf("Couldn't set a kernel argument");
         exit(1);
      };

      for(stage = high_stage; stage > 1; stage >>= 1) {

         err = clSetKernelArg(kernel_stage_n, 2, sizeof(int), &stage);
         if(err < 0) {
            printf("Couldn't set a kernel argument");
            exit(1);
         };

         err = clEnqueueNDRangeKernel(queue, kernel_stage_n, 1, NULL, 
               &global_size, &local_size, 0, NULL, NULL); 
         if(err < 0) {
            perror("Couldn't enqueue the kernel");
            exit(1);   
         }
      }

      err = clEnqueueNDRangeKernel(queue, kernel_stage_0, 1, NULL, 
            &global_size, &local_size, 0, NULL, NULL); 
      if(err < 0) {
         perror("Couldn't enqueue the kernel");
         exit(1);   
      }
   }

   /* Set the sort direction */
   direction = DIRECTION;
   err = clSetKernelArg(kernel_merge, 3, sizeof(int), &direction);
   err |= clSetKernelArg(kernel_merge_last, 2, sizeof(int), &direction);
   if(err < 0) {
      printf("Couldn't set a kernel argument");
      exit(1);
   };

   /* Perform the bitonic merge */
   for(stage = num_stages; stage > 1; stage >>= 1) {

      err = clSetKernelArg(kernel_merge, 2, sizeof(int), &stage);
      if(err < 0) {
         printf("Couldn't set a kernel argument");
         exit(1);
      };

      err = clEnqueueNDRangeKernel(queue, kernel_merge, 1, NULL, 
            &global_size, &local_size, 0, NULL, NULL); 
      if(err < 0) {
         perror("Couldn't enqueue the kernel");
         exit(1);   
      }
   }
   err = clEnqueueNDRangeKernel(queue, kernel_merge_last, 1, NULL, 
         &global_size, &local_size, 0, NULL, NULL); 
   if(err < 0) {
      perror("Couldn't enqueue the kernel");
      exit(1);   
   }

   /* Read the result */
   err = clEnqueueReadBuffer(queue, data_buffer, CL_TRUE, 0, 
      sizeof(data), &data, 0, NULL, NULL);
   if(err < 0) {
      perror("Couldn't read the buffer");
      exit(1);   
   }

   check = 1;

   /* Check ascending sort */
   if(direction == 0) {
      for(i=1; i<NUM_FLOATS; i++) {
         if(data[i] < data[i-1]) {
            check = 0;
            break;
         }
      }
   }
   /* Check descending sort */
   if(direction == -1) {
      for(i=1; i<NUM_FLOATS; i++) {
         if(data[i] > data[i-1]) {
            check = 0;
            break;
         }
      }
   }

   /* Display check result */
   printf("Local size: %zu\n", local_size);
   printf("Global size: %zu\n", global_size);
   if(check)
      printf("Bitonic sort succeeded.\n");
   else
      printf("Bitonic sort failed.\n");

   /* Deallocate resources */
   clReleaseMemObject(data_buffer);
   clReleaseKernel(kernel_init);
   clReleaseKernel(kernel_stage_0);
   clReleaseKernel(kernel_stage_n);
   clReleaseKernel(kernel_merge);
   clReleaseKernel(kernel_merge_last);
   clReleaseCommandQueue(queue);
   clReleaseProgram(program);
   clReleaseContext(context);
   return 0;
}
Esempio n. 13
0
void SendAttitude(void)
{ 
  static byte i,j;

  //Serial.print("!");

#if PRINT_EULER == 1




  
float  AccMagnitude = sqrt(Acc_V[0]*Acc_V[0] + Acc_V[1]*Acc_V[1] + Acc_V[2]*Acc_V[2]);
  AccMagnitude = (AccMagnitude / (float)GRAVITY); // Scale to gravity.
  
    if(abs(last_roll-(Roll* 57.2957795131)) > 1 ||  abs(last_pitch-(Pitch* 57.2957795131)) > 1 || abs(last_yaw-(Yaw* 57.2957795131)) > 1  )
    { 
      
      last_roll  = Roll* 57.2957795131;
       last_pitch = Pitch* 57.2957795131; 
       last_yaw = Yaw* 57.2957795131; 
       
       
  Serial.print(Roll * 57.2957795131);

  Serial.print(",");
  Serial.print(Pitch * 57.2957795131);
  Serial.print(",");
  Serial.print(Yaw * 57.2957795131);
  Serial.print(",");
  Serial.println(AccMagnitude); 
    
    
    
    
int roll_d1 =(Roll * 57.2957795131);            // Get the integer part (678).
float roll_f2 = (Roll * 57.2957795131) - roll_d1;     // Get fractional part (678.0123 - 678 = 0.0123).
int roll_d2 = trunc(roll_f2 * 100);   

int pitch_d1 =(Pitch * 57.2957795131);            // Get the integer part (678).
float pitch_f2 = (Pitch * 57.2957795131) - pitch_d1;     // Get fractional part (678.0123 - 678 = 0.0123).
int pitch_d2 = trunc(pitch_f2 * 100);   

int yaw_d1 =(Yaw * 57.2957795131);            // Get the integer part (678).
float yaw_f2 = (Yaw * 57.2957795131) - yaw_d1;     // Get fractional part (678.0123 - 678 = 0.0123).
int yaw_d2 = trunc(yaw_f2 * 100);   



int acc_d1 =AccMagnitude;        // Get the integer part (678).
float acc_f2 = AccMagnitude - acc_d1;     // Get fractional part (678.0123 - 678 = 0.0123).
int acc_d2 = trunc(acc_f2 * 100);   





  // enable Slave Select
  digitalWrite(SS, LOW);    // SS is pin 10
  SPI.transfer (roll_d1);
  SPI.transfer (roll_d2);
  SPI.transfer (pitch_d1);
  SPI.transfer (pitch_d2);
  SPI.transfer (yaw_d1);
  SPI.transfer (yaw_d2);
  SPI.transfer (acc_d1);
  SPI.transfer (acc_d2);
  SPI.transfer ('\n');
  // disable Slave Select
  digitalWrite(SS, HIGH);
    }



  

 
#endif // PRINT_EULER

#if PRINT_ANALOGS == 1

  Serial.print("AN:");
  for ( i = 0; i < 3 ; i++ )
  {
    Serial.print(GyroADC[Map[i]]); 
    Serial.print(",");
  }
  for ( i = 0; i < 3 ; i++ )
  {
    Serial.print(AccADC[i]);
    Serial.print (",");
  }
  for ( i = 0; i < 3 ; i++ )
  { 
    Serial.print(Mag[i]); 
    Serial.print (","); 
  }

#endif // PRINT_ANALOGS

#if PRINT_DCM == 1

  Serial.print ("DCM:");
  for ( i = 0; i < 3 ; i++ )
    for ( j = 0; j < 3 ; j++ )
    {
      Serial.print(DCM_Matrix[i][j]*10000000);
      Serial.print (",");
    }

#endif // PRINT_DCM
 

} // SendAttitude
Esempio n. 14
0
/*
 * Save all of the undetermined messages at the top of "mbox"
 * Save all untouched messages back in the system mailbox.
 * Remove the system mailbox, if none saved there.
 */
void
quit()
{
	int mcount, p, modify, autohold, anystat, holdbit, nohold;
	FILE *ibuf, *obuf, *fbuf, *rbuf, *readstat, *abuf;
	register struct message *mp;
	register int c;
	extern char tempQuit[], tempResid[];
	struct stat minfo;
	char *mbox;

	/*
	 * If we are read only, we can't do anything,
	 * so just return quickly.
	 */
	if (readonly)
		return;
	/*
	 * If editing (not reading system mail box), then do the work
	 * in edstop()
	 */
	if (edit) {
		edstop();
		return;
	}

	/*
	 * See if there any messages to save in mbox.  If no, we
	 * can save copying mbox to /tmp and back.
	 *
	 * Check also to see if any files need to be preserved.
	 * Delete all untouched messages to keep them out of mbox.
	 * If all the messages are to be preserved, just exit with
	 * a message.
	 */

	fbuf = Fopen(mailname, "r");
	if (fbuf == NULL)
		goto newmail;
	flock(fileno(fbuf), LOCK_EX);
	rbuf = NULL;
	if (fstat(fileno(fbuf), &minfo) >= 0 && minfo.st_size > mailsize) {
		printf("New mail has arrived.\n");
		rbuf = Fopen(tempResid, "w");
		if (rbuf == NULL || fbuf == NULL)
			goto newmail;
#ifdef APPEND
		fseek(fbuf, (long)mailsize, 0);
		while ((c = getc(fbuf)) != EOF)
			(void) putc(c, rbuf);
#else
		p = minfo.st_size - mailsize;
		while (p-- > 0) {
			c = getc(fbuf);
			if (c == EOF)
				goto newmail;
			(void) putc(c, rbuf);
		}
#endif
		Fclose(rbuf);
		if ((rbuf = Fopen(tempResid, "r")) == NULL)
			goto newmail;
		rm(tempResid);
	}

	/*
	 * Adjust the message flags in each message.
	 */

	anystat = 0;
	autohold = value("hold") != NOSTR;
	holdbit = autohold ? MPRESERVE : MBOX;
	nohold = MBOX|MSAVED|MDELETED|MPRESERVE;
	if (value("keepsave") != NOSTR)
		nohold &= ~MSAVED;
	for (mp = &message[0]; mp < &message[msgCount]; mp++) {
		if (mp->m_flag & MNEW) {
			mp->m_flag &= ~MNEW;
			mp->m_flag |= MSTATUS;
		}
		if (mp->m_flag & MSTATUS)
			anystat++;
		if ((mp->m_flag & MTOUCH) == 0)
			mp->m_flag |= MPRESERVE;
		if ((mp->m_flag & nohold) == 0)
			mp->m_flag |= holdbit;
	}
	modify = 0;
	if (Tflag != NOSTR) {
		if ((readstat = Fopen(Tflag, "w")) == NULL)
			Tflag = NOSTR;
	}
	for (c = 0, p = 0, mp = &message[0]; mp < &message[msgCount]; mp++) {
		if (mp->m_flag & MBOX)
			c++;
		if (mp->m_flag & MPRESERVE)
			p++;
		if (mp->m_flag & MODIFY)
			modify++;
		if (Tflag != NOSTR && (mp->m_flag & (MREAD|MDELETED)) != 0) {
			char *id;

			if ((id = hfield("article-id", mp)) != NOSTR)
				fprintf(readstat, "%s\n", id);
		}
	}
	if (Tflag != NOSTR)
		Fclose(readstat);
	if (p == msgCount && !modify && !anystat) {
		printf("Held %d message%s in %s\n",
			p, p == 1 ? "" : "s", mailname);
		Fclose(fbuf);
		return;
	}
	if (c == 0) {
		if (p != 0) {
			writeback(rbuf);
			Fclose(fbuf);
			return;
		}
		goto cream;
	}

	/*
	 * Create another temporary file and copy user's mbox file
	 * darin.  If there is no mbox, copy nothing.
	 * If he has specified "append" don't copy his mailbox,
	 * just copy saveable entries at the end.
	 */

	mbox = expand("&");
	mcount = c;
	if (value("append") == NOSTR) {
		if ((obuf = Fopen(tempQuit, "w")) == NULL) {
			perror(tempQuit);
			Fclose(fbuf);
			return;
		}
		if ((ibuf = Fopen(tempQuit, "r")) == NULL) {
			perror(tempQuit);
			rm(tempQuit);
			Fclose(obuf);
			Fclose(fbuf);
			return;
		}
		rm(tempQuit);
		if ((abuf = Fopen(mbox, "r")) != NULL) {
			while ((c = getc(abuf)) != EOF)
				(void) putc(c, obuf);
			Fclose(abuf);
		}
		if (ferror(obuf)) {
			perror(tempQuit);
			Fclose(ibuf);
			Fclose(obuf);
			Fclose(fbuf);
			return;
		}
		Fclose(obuf);
		close(creat(mbox, 0600));
		if ((obuf = Fopen(mbox, "r+")) == NULL) {
			perror(mbox);
			Fclose(ibuf);
			Fclose(fbuf);
			return;
		}
	}
	if (value("append") != NOSTR) {
		if ((obuf = Fopen(mbox, "a")) == NULL) {
			perror(mbox);
			Fclose(fbuf);
			return;
		}
		fchmod(fileno(obuf), 0600);
	}
	for (mp = &message[0]; mp < &message[msgCount]; mp++)
		if (mp->m_flag & MBOX)
			if (send(mp, obuf, saveignore, NOSTR) < 0) {
				perror(mbox);
				Fclose(ibuf);
				Fclose(obuf);
				Fclose(fbuf);
				return;
			}

	/*
	 * Copy the user's old mbox contents back
	 * to the end of the stuff we just saved.
	 * If we are appending, this is unnecessary.
	 */

	if (value("append") == NOSTR) {
		rewind(ibuf);
		c = getc(ibuf);
		while (c != EOF) {
			(void) putc(c, obuf);
			if (ferror(obuf))
				break;
			c = getc(ibuf);
		}
		Fclose(ibuf);
	}
	fflush(obuf);
	trunc(obuf);
	if (ferror(obuf)) {
		perror(mbox);
		Fclose(obuf);
		Fclose(fbuf);
		return;
	}
	Fclose(obuf);
	if (mcount == 1)
		printf("Saved 1 message in mbox\n");
	else
		printf("Saved %d messages in mbox\n", mcount);

	/*
	 * Now we are ready to copy back preserved files to
	 * the system mailbox, if any were requested.
	 */

	if (p != 0) {
		writeback(rbuf);
		Fclose(fbuf);
		return;
	}

	/*
	 * Finally, remove his /usr/mail file.
	 * If new mail has arrived, copy it back.
	 */

cream:
	if (rbuf != NULL) {
		abuf = Fopen(mailname, "r+");
		if (abuf == NULL)
			goto newmail;
		while ((c = getc(rbuf)) != EOF)
			(void) putc(c, abuf);
		Fclose(rbuf);
		trunc(abuf);
		Fclose(abuf);
		alter(mailname);
		Fclose(fbuf);
		return;
	}
	demail();
	Fclose(fbuf);
	return;

newmail:
	printf("Thou hast new mail.\n");
	if (fbuf != NULL)
		Fclose(fbuf);
}
Esempio n. 15
0
//Latest version
int getGridValuesFromExtent(QString extent,QSqlDatabase db, double &dbCellSize, double &xllCenter, double &yllCenter, int &ncols, int &nrows, int &bottom, int &left)
{
    //(1.3333,32.1212321) (-4.12121,41.212121)
    if (!extent.count(" ") == 1)
    {
        gbtLog(QObject::tr("Extent is invalid"));
        return 1;
    }
    if (!extent.count(",") == 2)
    {
        gbtLog(QObject::tr("Extent is invalid"));
        return 1;
    }
    if (!extent.count("(") == 2)
    {
        gbtLog(QObject::tr("Extent is invalid"));
        return 1;
    }
    if (!extent.count(")") == 2)
    {
        gbtLog(QObject::tr("Extent is invalid"));
        return 1;
    }
    int pos;
    pos = extent.indexOf(" ");
    QString from;
    QString to;
    from = extent.left(pos);
    to = extent.mid(pos+1,extent.length()-pos+1);
    from.replace("(","");
    from.replace(")","");
    to.replace("(","");
    to.replace(")","");

    //Get UpperLeft
    QPointF dupperLeft;
    pos = from.indexOf(",");
    dupperLeft.setX(from.left(pos).toDouble());
    dupperLeft.setY(from.mid(pos+1,from.length()-pos+1).toDouble());


    //Get lower right
    QPointF dlowerRight;
    pos = to.indexOf(",");
    dlowerRight.setX(to.left(pos).toDouble());
    dlowerRight.setY(to.mid(pos+1,to.length()-pos+1).toDouble());

    QSqlQuery qry(db);
    QString sql;

    dbCellSize = 0.0;

    sql = "SELECT cellSize FROM gbtconfig";
    if (qry.exec(sql))
    {
        if (qry.first())
            dbCellSize = qry.value(0).toDouble();
        else
            return 1;
    }
    else
        return 1;

    if (dbCellSize == 0.0)
        return 1;



    //*************************
    double v_xMin;
    double v_xMax;
    double v_yMin;
    double v_yMax;
    double halfx;
    int v_cols;
    int v_rows;


    v_xMin = trunc(( dupperLeft.x() *1.0/ dbCellSize )) * dbCellSize;
    v_yMin = trunc(( dlowerRight.y() *1.0/ dbCellSize )) * dbCellSize;
    v_cols = trunc( ( dlowerRight.x() - dupperLeft.x() ) *1.0/ dbCellSize ) + 1;
    v_rows = trunc( ( dupperLeft.y() - dlowerRight.y() ) *1.0/ dbCellSize ) + 1;
    v_xMax = v_xMin + v_cols * dbCellSize;
    v_yMax = v_yMin + v_rows * dbCellSize;

    halfx = dbCellSize *1.0/ 2;

    xllCenter = v_xMin + halfx;
    yllCenter = (v_yMin + halfx) - 0.0200;
    nrows = trunc( fabs( v_yMax - v_yMin ) *1.0/ dbCellSize );
    ncols = trunc(fabs(v_xMax - v_xMin) / dbCellSize);

    //*******************

    left = ceil((dupperLeft.x() + 180) / dbCellSize);
    bottom = ceil((90 - dupperLeft.y()) / dbCellSize);

    return 0;


}
Esempio n. 16
0
double JSValue::toIntegerPreserveNaN(ExecState* exec) const
{
    if (isInt32())
        return asInt32();
    return trunc(toNumber(exec));
}
Esempio n. 17
0
//Latest and most updated version
int calcBoundFromShapes(QString shapeConstraint,QSqlDatabase db, double &dbCellSize, double &xllCenter, double &yllCenter, int &ncols, int &nrows, int &bottom, int &left)
{
    //ShapeDataSet:shapeID,ShapeID,
    if (shapeConstraint.count(":") != 1)
    {
        gbtLog(QObject::tr("Error in shape constraint"));
        return 1;
    }

    int pos;
    pos = shapeConstraint.indexOf(":");

    QString dataset = shapeConstraint.left(pos);

    QSqlQuery qry(db);
    QString sql;

    dbCellSize = 0.0;

    sql = "SELECT cellSize FROM gbtconfig";
    if (qry.exec(sql))
    {
        if (qry.first())
            dbCellSize = qry.value(0).toDouble();
        else
            return 1;
    }
    else
        return 1;

    sql = "SELECT count(*) FROM datasetinfo WHERE dataset_id = '" + dataset + "' and dataset_type = 2";

    if (qry.exec(sql))
    {
        if (qry.first())
        {
            if (qry.value(0).toInt() == 0)
            {
                gbtLog(QObject::tr(" is not a shape dataset"));
                return 1;
            }
        }
        else
            return 1;
    }
    else
        return 1;

    sql = "SELECT shapeid,AsText(max(envelope(ogc_geom))) as boundbox FROM " + dataset + " WHERE ";
    QString shapes = shapeConstraint.mid(pos+1,shapeConstraint.length()-pos+1);
    QString shape;

    pos =0;
    while (pos <= shapes.length()-1)
    {
        if (shapes[pos] != ',')
        {
            shape = shape + shapes[pos];
            pos++;
        }
        else
        {
            sql = sql + " shapeid = " + shape + " OR ";
            shape = "";
            shapes = shapes.mid(pos+1,shapes.length()-pos+1);
            pos = 0;
        }
    }
    sql = sql + "shapeid = " + shape;
    sql = sql + " GROUP BY shapeid";

    double maxX;
    double maxY;
    double minX;
    double minY;

    maxX = -3000;
    maxY = -3000;
    minX = 3000;
    minY = 3000;


    QString cadena;
    QString geopos;

    double temp;
    if (qry.exec(sql))
    {
        while (qry.next())
        {
            cadena = qry.value(1).toString();
            pos = cadena.indexOf("(");
            cadena = cadena.mid(pos+1,cadena.length()-pos+1); //Remove the type of shape
            cadena = cadena.replace("(","");
            cadena = cadena.replace(")","");
            pos = 0;
            while (pos <= cadena.length()-1)
            {
                if (cadena[pos] != ',')
                {
                    geopos = geopos + cadena[pos];
                    pos++;
                }
                else
                {
                    cadena = cadena.mid(pos+1,cadena.length()-pos+1);

                    pos = geopos.indexOf(" ");
                    temp = geopos.left(pos).toDouble();
                    if (temp > maxX)
                        maxX = temp;
                    if (temp < minX)
                        minX = temp;

                    temp = geopos.mid(pos+1,geopos.length()-pos+1).toDouble();

                    if (temp > maxY)
                        maxY = temp;
                    if (temp < minY)
                        minY = temp;

                    pos = 0;
                    geopos = "";
                }
            }

            //Final string
            pos = geopos.indexOf(" ");
            temp = geopos.left(pos).toDouble();
            if (temp > maxX)
                maxX = temp;
            if (temp < minX)
                minX = temp;

            temp = geopos.mid(pos+1,geopos.length()-pos+1).toDouble();

            if (temp > maxY)
                maxY = temp;
            if (temp < minY)
                minY = temp;

        }
        QPointF UL;
        QPointF LR;

        UL.setX(minX);
        UL.setY(maxY);

        LR.setX(maxX);
        LR.setY(minY);


        double v_xMin,v_xMax,v_yMin,v_yMax;
        double halfx;
        int v_cols,v_rows;

        v_xMin = trunc(UL.x() / dbCellSize) * dbCellSize;
        v_yMin = trunc(LR.y() / dbCellSize) * dbCellSize;
        v_cols = trunc((LR.x() -  UL.x()) / dbCellSize) + 1;
        v_rows = trunc((UL.y() -  LR.y()) / dbCellSize) + 1;
        v_xMax = v_xMin + v_cols * dbCellSize;
        v_yMax = v_yMin + v_rows * dbCellSize;

        halfx = dbCellSize / 2;
        xllCenter = v_xMin + halfx;
        yllCenter = (v_yMin + halfx) - 0.0200;
        ncols = trunc(fabs(v_xMax - v_xMin) / dbCellSize);
        nrows = trunc(fabs(v_yMax - v_yMin) / dbCellSize);

        left = ceil((UL.x() + 180) / dbCellSize);
        bottom = ceil((90 - UL.y()) / dbCellSize);


        return 0;

    }
    else
        return 1;
}
Esempio n. 18
0
float truncf (float x)
{
	return (float) trunc( (double)x );
}
Esempio n. 19
0
void CCareer::OnTextChangedName(LPTSTR text) 
{
	trunc(text,PLAYERNAMELEN - 1);		
}
Esempio n. 20
0
void Triangle::draw(Canvas& canvas, Texture* texture = 0) {
    std::vector<TexturedPoint> points;
    transform(points);
    std::sort(points.begin(), points.end());

    std::vector<Edge> edges;

    int minY = (points.front().y() < 0) ? 0: points.front().y();
    int maxY = (points.back().y() < this->maxY) ? points.back().y() : this->maxY - 1;

    int curY = minY;
    int i = 0;

    while (curY < maxY) {
        int nextY = maxY;

        while ( i != (int)points.size() && trunc( points[i].y()) <= curY ){
            TexturedPoint a = points[i];
            TexturedPoint b = points[(points.size() - i - 1 ) % points.size()];
            TexturedPoint c = points[(i + 1) % points.size()];

            if (b.y() > curY ) {
                edges.push_back(Edge(a,b));
                if ( b.y() < nextY ) {
                    nextY = b.y() ;
                }
            }
            if (c.y() > curY) {
                edges.push_back(Edge(a,c));
                if ( c.y() < nextY) {
                    nextY = c.y();
                }
            }
            ++i;
        }

        while(curY <= nextY && curY <= maxY) {

            std::vector<TexturedPoint> borderX;
            for (int i = 0; i < (int)edges.size(); ++i) {
                int n = curY - edges[i].getA().y();
                double curX = (edges[i].getA().x()) + n * edges[i].getK();
                TexturedPoint texCoord(curX, curY);
                texCoord.calcTextureCoordinates(edges[i].getA(), edges[i].getB());
                borderX.push_back(texCoord);
            }

            std::sort(borderX.begin(), borderX.end(), TexturedPoint::compX);
            int begin = borderX.front().x() > 0 ? borderX.front().x() : 0;

            for (int x = begin; x < borderX.back().x() && x < maxX; ++x) {
                TexturedPoint curPoint(x, curY);

                curPoint.calcTextureCoordinates(borderX.front(),borderX.back());

                if (0 == texture) {

                    canvas.drawPixel(x, curY, TexturedPoint::transformToColor(curPoint.getTexX(), curPoint.getTexY()));
                }
                else {

                    canvas.drawPixel(x, curY, texture->get_color(curPoint));
                }
            }

            ++curY;
        }

        std::vector<Edge>::iterator iter = edges.begin();
        while (iter != edges.end()) {
           if ( (*iter).getB().y() < curY) {
               edges.erase(iter);
           }
           else {
               ++iter;
           }
        }

   }
}
Esempio n. 21
0
static uint8_t _point(double value) 
{
	return((uint8_t)(value*10 - trunc(value)*10));	// isolate the decimal point as an int
}
Esempio n. 22
0
File: system.c Progetto: proto3/grbl
// Directs and executes one line of formatted input from protocol_process. While mostly
// incoming streaming g-code blocks, this also executes Grbl internal commands, such as
// settings, initiating the homing cycle, and toggling switch states. This differs from
// the realtime command module by being susceptible to when Grbl is ready to execute the
// next line during a cycle, so for switches like block delete, the switch only effects
// the lines that are processed afterward, not necessarily real-time during a cycle,
// since there are motions already stored in the buffer. However, this 'lag' should not
// be an issue, since these commands are not typically used during a cycle.
uint8_t system_execute_line(char *line)
{
  uint8_t char_counter = 1;
  uint8_t helper_var = 0; // Helper variable
  float parameter, value;
  switch( line[char_counter] ) {
    case 0 : report_grbl_help(); break;
    case '$': case 'G': case 'C': case 'X':
      if ( line[(char_counter+1)] != 0 ) { return(STATUS_INVALID_STATEMENT); }
      switch( line[char_counter] ) {
        case '$' : // Prints Grbl settings
          if ( sys.state & (STATE_CYCLE | STATE_HOLD) ) { return(STATUS_IDLE_ERROR); } // Block during cycle. Takes too long to print.
          else { report_grbl_settings(); }
          break;
        case 'G' : // Prints gcode parser state
          // TODO: Move this to realtime commands for GUIs to request this data during suspend-state.
          report_gcode_modes();
          break;
        case 'C' : // Set check g-code mode [IDLE/CHECK]
          // Perform reset when toggling off. Check g-code mode should only work if Grbl
          // is idle and ready, regardless of alarm locks. This is mainly to keep things
          // simple and consistent.
          if ( sys.state == STATE_CHECK_MODE ) {
            mc_reset();
            report_feedback_message(MESSAGE_DISABLED);
          } else {
            if (sys.state) { return(STATUS_IDLE_ERROR); } // Requires no alarm mode.
            sys.state = STATE_CHECK_MODE;
            report_feedback_message(MESSAGE_ENABLED);
          }
          break;
        case 'X' : // Disable alarm lock [ALARM]
          if (sys.state == STATE_ALARM) {
            report_feedback_message(MESSAGE_ALARM_UNLOCK);
            sys.state = STATE_IDLE;
            // Don't run startup script. Prevents stored moves in startup from causing accidents.
            if (system_check_safety_door_ajar()) { // Check safety door switch before returning.
              bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
              protocol_execute_realtime(); // Enter safety door mode.
            }
          } // Otherwise, no effect.
          break;
    //  case 'J' : break;  // Jogging methods
          // TODO: Here jogging can be placed for execution as a seperate subprogram. It does not need to be
          // susceptible to other realtime commands except for e-stop. The jogging function is intended to
          // be a basic toggle on/off with controlled acceleration and deceleration to prevent skipped
          // steps. The user would supply the desired feedrate, axis to move, and direction. Toggle on would
          // start motion and toggle off would initiate a deceleration to stop. One could 'feather' the
          // motion by repeatedly toggling to slow the motion to the desired location. Location data would
          // need to be updated real-time and supplied to the user through status queries.
          //   More controlled exact motions can be taken care of by inputting G0 or G1 commands, which are
          // handled by the planner. It would be possible for the jog subprogram to insert blocks into the
          // block buffer without having the planner plan them. It would need to manage de/ac-celerations
          // on its own carefully. This approach could be effective and possibly size/memory efficient.
//       }
//       break;
      }
      break;
    default :
      // Block any system command that requires the state as IDLE/ALARM. (i.e. EEPROM, homing)
      if ( !(sys.state == STATE_IDLE || sys.state == STATE_ALARM) ) { return(STATUS_IDLE_ERROR); }
      switch( line[char_counter] ) {
        case '#' : // Print Grbl NGC parameters
          if ( line[++char_counter] != 0 ) { return(STATUS_INVALID_STATEMENT); }
          else { report_ngc_parameters(); }
          break;
        case 'H' : // Perform homing cycle [IDLE/ALARM]
          if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) {
            sys.state = STATE_HOMING; // Set system state variable
            // Only perform homing if Grbl is idle or lost.

            // TODO: Likely not required.
            if (system_check_safety_door_ajar()) { // Check safety door switch before homing.
              bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
              protocol_execute_realtime(); // Enter safety door mode.
            }


            mc_homing_cycle();
            if (!sys.abort) {  // Execute startup scripts after successful homing.
              sys.state = STATE_IDLE; // Set to IDLE when complete.
              st_go_idle(); // Set steppers to the settings idle state before returning.
              system_execute_startup(line);
            }
          } else { return(STATUS_SETTING_DISABLED); }
          break;
        case 'I' : // Print or store build info. [IDLE/ALARM]
          if ( line[++char_counter] == 0 ) {
            settings_read_build_info(line);
            report_build_info(line);
          } else { // Store startup line [IDLE/ALARM]
            if(line[char_counter++] != '=') { return(STATUS_INVALID_STATEMENT); }
            helper_var = char_counter; // Set helper variable as counter to start of user info line.
            do {
              line[char_counter-helper_var] = line[char_counter];
            } while (line[char_counter++] != 0);
            settings_store_build_info(line);
          }
          break;
        case 'R' : // Restore defaults [IDLE/ALARM]
          if (line[++char_counter] != 'S') { return(STATUS_INVALID_STATEMENT); }
          if (line[++char_counter] != 'T') { return(STATUS_INVALID_STATEMENT); }
          if (line[++char_counter] != '=') { return(STATUS_INVALID_STATEMENT); }
          if (line[char_counter+2] != 0) { return(STATUS_INVALID_STATEMENT); }
          switch (line[++char_counter]) {
            case '$': settings_restore(SETTINGS_RESTORE_DEFAULTS); break;
            case '#': settings_restore(SETTINGS_RESTORE_PARAMETERS); break;
            case '*': settings_restore(SETTINGS_RESTORE_ALL); break;
            default: return(STATUS_INVALID_STATEMENT);
          }
          report_feedback_message(MESSAGE_RESTORE_DEFAULTS);
          mc_reset(); // Force reset to ensure settings are initialized correctly.
          break;
        case 'N' : // Startup lines. [IDLE/ALARM]
          if ( line[++char_counter] == 0 ) { // Print startup lines
            for (helper_var=0; helper_var < N_STARTUP_LINE; helper_var++) {
              if (!(settings_read_startup_line(helper_var, line))) {
                report_status_message(STATUS_SETTING_READ_FAIL);
              } else {
                report_startup_line(helper_var,line);
              }
            }
            break;
          } else { // Store startup line [IDLE Only] Prevents motion during ALARM.
            if (sys.state != STATE_IDLE) { return(STATUS_IDLE_ERROR); } // Store only when idle.
            helper_var = true;  // Set helper_var to flag storing method.
            // No break. Continues into default: to read remaining command characters.
          }
        default :  // Storing setting methods [IDLE/ALARM]
          if(!read_float(line, &char_counter, &parameter)) { return(STATUS_BAD_NUMBER_FORMAT); }
          if(line[char_counter++] != '=') { return(STATUS_INVALID_STATEMENT); }
          if (helper_var) { // Store startup line
            // Prepare sending gcode block to gcode parser by shifting all characters
            helper_var = char_counter; // Set helper variable as counter to start of gcode block
            do {
              line[char_counter-helper_var] = line[char_counter];
            } while (line[char_counter++] != 0);
            // Execute gcode block to ensure block is valid.
            helper_var = gc_execute_line(line); // Set helper_var to returned status code.
            if (helper_var) { return(helper_var); }
            else {
              helper_var = trunc(parameter); // Set helper_var to int value of parameter
              settings_store_startup_line(helper_var,line);
            }
          } else { // Store global setting.
            if(!read_float(line, &char_counter, &value)) { return(STATUS_BAD_NUMBER_FORMAT); }
            if((line[char_counter] != 0) || (parameter > 255)) { return(STATUS_INVALID_STATEMENT); }
            return(settings_store_global_setting((uint8_t)parameter, value));
          }
      }
  }
  return(STATUS_OK); // If '$' command makes it to here, then everything's ok.
}
Esempio n. 23
0
double Frac(double x)
{
    return x-trunc(x);
}
/*
 * getPacket
 *     description: retrieve data from the provides (input) port
 *
 *  timeout: the amount of time to wait for data before a NULL is returned.
 *           Use 0.0 for non-blocking and -1 for blocking.
 */
BULKIO_dataFloat_In_i::dataTransfer *BULKIO_dataFloat_In_i::getPacket(float timeout, std::string streamID )
{
    if (breakBlock) {
        return NULL;
    }
    //if (workQueue.size() == 0) {
    if ( (workQueue.size() == 0 ) or (( workQueue.size() != 0 ) and ( workQueue.size() == lastQueueSize )) ){
        if (timeout == 0.0) {
            lastQueueSize = workQueue.size();
            return NULL;
        } else if (timeout > 0){
             uint64_t secs = (unsigned long)(trunc(timeout));
            uint64_t msecs = (unsigned long)((timeout - secs) * 1e6);
            boost::system_time to_time  = boost::get_system_time() + boost::posix_time::seconds(secs) + boost::posix_time::microseconds(msecs);
            boost::unique_lock< boost::mutex > lock(dataAvailableMutex);
            if ( dataAvailable.timed_wait( lock, to_time) == false ) {
                return NULL;
            }

            if (breakBlock) {
                return NULL;
            }
        } else {
            boost::unique_lock< boost::mutex > lock(dataAvailableMutex);
            dataAvailable.wait(lock);
            if (breakBlock) {
                return NULL;
            }
        }       
        
    }
    boost::mutex::scoped_lock lock1(dataBufferLock);
    BULKIO_dataFloat_In_i::dataTransfer *tmp = NULL;
    if ( streamID == "" ){
        tmp = workQueue.front();
        workQueue.pop_front();
    } else {
        std::deque< dataTransfer * >::iterator p = workQueue.begin();
        while ( p != workQueue.end() ) {
            if ( (*p)->streamID == streamID ) {
                tmp = *p;
                workQueue.erase(p);
                break;
            }
            p++;
       }
    
    }
    
    if ( tmp == NULL ) {
        lastQueueSize = workQueue.size();
        return NULL;
    }
    
    boost::mutex::scoped_lock lock2(sriUpdateLock);
    if (tmp->EOS) {
	    RH_SRIMap::iterator target = currentHs.find(std::string(tmp->streamID));
        if (target != currentHs.end()) {
            if (target->second.first.blocking) {
                RH_SRIMap::iterator currH;
                bool keepBlocking = false;
                for (currH = currentHs.begin(); currH != currentHs.end(); currH++) {
                    if (currH->second.first.blocking) {
                        keepBlocking = true;
                        break;
                    }
                }

                if (!keepBlocking) {
                    queueSem->setCurrValue(0);
                    blocking = false;
                }
            }
            currentHs.erase(target);
        }
    }

    if (blocking) {
        queueSem->decr();
    }
    
    lastQueueSize=0;
    return tmp;
}
Esempio n. 25
0
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
// characters and signed floating point values (no whitespace). Comments and block delete
// characters have been removed. All units and positions are converted and exported to grbl's
// internal functions in terms of (mm, mm/min) and absolute machine coordinates, respectively.
uint8_t gc_execute_line(char *line) 
{

  // If in alarm state, don't process. Immediately return with error.
  // NOTE: Might not be right place for this, but also prevents $N storing during alarm.
  if (sys.state == STATE_ALARM) { return(STATUS_ALARM_LOCK); }
 
  uint8_t char_counter = 0;  
  char letter;
  float value;
  int int_value;
  
  uint16_t modal_group_words = 0;  // Bitflag variable to track and check modal group words in block
  uint8_t axis_words = 0;          // Bitflag to track which XYZ(ABC) parameters exist in block

  float inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified
  uint8_t absolute_override = false; // true(1) = absolute motion for this block only {G53}
  uint8_t non_modal_action = NON_MODAL_NONE; // Tracks the actions of modal group 0 (non-modal)
  
  float target[3], offset[3];  
  clear_vector(target); // XYZ(ABC) axes parameters.
  clear_vector(offset); // IJK Arc offsets are incremental. Value of zero indicates no change.
    
  gc.status_code = STATUS_OK;
  
  /* Pass 1: Commands and set all modes. Check for modal group violations.
     NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */
  uint8_t group_number = MODAL_GROUP_NONE;
  while(next_statement(&letter, &value, line, &char_counter)) {
    int_value = trunc(value);
    switch(letter) {
      case 'G':
        // Set modal group values
        switch(int_value) {
          case 4: case 10: case 28: case 30: case 53: case 92: group_number = MODAL_GROUP_0; break;
          case 0: case 1: case 2: case 3: case 80: group_number = MODAL_GROUP_1; break;
          case 17: case 18: case 19: group_number = MODAL_GROUP_2; break;
          case 90: case 91: group_number = MODAL_GROUP_3; break;
          case 93: case 94: group_number = MODAL_GROUP_5; break;
          case 20: case 21: group_number = MODAL_GROUP_6; break;
          case 54: case 55: case 56: case 57: case 58: case 59: group_number = MODAL_GROUP_12; break;
        }          
        // Set 'G' commands
        switch(int_value) {
          case 0: gc.motion_mode = MOTION_MODE_SEEK; break;
          case 1: gc.motion_mode = MOTION_MODE_LINEAR; break;
          case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break;
          case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break;
          case 4: non_modal_action = NON_MODAL_DWELL; break;
          case 10: non_modal_action = NON_MODAL_SET_COORDINATE_DATA; break;
          case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break;
          case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break;
          case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break;
          case 20: gc.inches_mode = true; break;
          case 21: gc.inches_mode = false; break;
          case 28: case 30: 
            int_value = trunc(10*value); // Multiply by 10 to pick up Gxx.1
            switch(int_value) {
              case 280: non_modal_action = NON_MODAL_GO_HOME_0; break;
              case 281: non_modal_action = NON_MODAL_SET_HOME_0; break;
              case 300: non_modal_action = NON_MODAL_GO_HOME_1; break;
              case 301: non_modal_action = NON_MODAL_SET_HOME_1; break;
              default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
            }
            break;
          case 53: absolute_override = true; break;
          case 54: case 55: case 56: case 57: case 58: case 59:
            gc.coord_select = int_value-54;
            break;
          case 80: gc.motion_mode = MOTION_MODE_CANCEL; break;
          case 90: gc.absolute_mode = true; break;
          case 91: gc.absolute_mode = false; break;
          case 92: 
            int_value = trunc(10*value); // Multiply by 10 to pick up G92.1
            switch(int_value) {
              case 920: non_modal_action = NON_MODAL_SET_COORDINATE_OFFSET; break;        
              case 921: non_modal_action = NON_MODAL_RESET_COORDINATE_OFFSET; break;
              default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
            }
            break;
          case 93: gc.inverse_feed_rate_mode = true; break;
          case 94: gc.inverse_feed_rate_mode = false; break;
          default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
        }
        break;        
      case 'M':
        // Set modal group values
        switch(int_value) {
          case 0: case 1: case 2: case 30: group_number = MODAL_GROUP_4; break;
          case 3: case 4: case 5: group_number = MODAL_GROUP_7; break;
        }        
        // Set 'M' commands
        switch(int_value) {
          case 0: gc.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause
          case 1: break; // Optional stop not supported. Ignore.
          case 2: case 30: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; // Program end and reset 
          case 3: gc.spindle_direction = 1; break;
          case 4: gc.spindle_direction = -1; break;
          case 5: gc.spindle_direction = 0; break;
          #ifdef ENABLE_M7
            case 7: gc.coolant_mode = COOLANT_MIST_ENABLE; break;
          #endif
          case 8: gc.coolant_mode = COOLANT_FLOOD_ENABLE; break;
          case 9: gc.coolant_mode = COOLANT_DISABLE; break;
          default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
        }            
        break;
    }    
    // Check for modal group multiple command violations in the current block
    if (group_number) {
      if ( bit_istrue(modal_group_words,bit(group_number)) ) {
        FAIL(STATUS_MODAL_GROUP_VIOLATION);
      } else {
        bit_true(modal_group_words,bit(group_number));
      }
      group_number = MODAL_GROUP_NONE; // Reset for next command.
    }
  } 

  // If there were any errors parsing this line, we will return right away with the bad news
  if (gc.status_code) { return(gc.status_code); }
  
  /* Pass 2: Parameters. All units converted according to current block commands. Position 
     parameters are converted and flagged to indicate a change. These can have multiple connotations
     for different commands. Each will be converted to their proper value upon execution. */
  float p = 0, r = 0;
  uint8_t l = 0;
  char_counter = 0;
  while(next_statement(&letter, &value, line, &char_counter)) {
    switch(letter) {
      case 'G': case 'M': case 'N': break; // Ignore command statements and line numbers
      case 'F': 
        if (value <= 0) { FAIL(STATUS_INVALID_STATEMENT); } // Must be greater than zero
        if (gc.inverse_feed_rate_mode) {
          inverse_feed_rate = to_millimeters(value); // seconds per motion for this motion only
        } else {          
          gc.feed_rate = to_millimeters(value); // millimeters per minute
        }
        break;
      case 'I': case 'J': case 'K': offset[letter-'I'] = to_millimeters(value); break;
      case 'L': l = trunc(value); break;
      case 'P': p = value; break;                    
      case 'R': r = to_millimeters(value); break;
      case 'S': 
        if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
        // TBD: Spindle speed not supported due to PWM issues, but may come back once resolved.
        // gc.spindle_speed = value;
        break;
      case 'T': 
        if (value < 0) { FAIL(STATUS_INVALID_STATEMENT); } // Cannot be negative
        gc.tool = trunc(value); 
        break;
      case 'X': target[X_AXIS] = to_millimeters(value); bit_true(axis_words,bit(X_AXIS)); break;
      case 'Y': target[Y_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Y_AXIS)); break;
      case 'Z': target[Z_AXIS] = to_millimeters(value); bit_true(axis_words,bit(Z_AXIS)); break;
      default: FAIL(STATUS_UNSUPPORTED_STATEMENT);
    }
  }
  
  // If there were any errors parsing this line, we will return right away with the bad news
  if (gc.status_code) { return(gc.status_code); }
  
  
  /* Execute Commands: Perform by order of execution defined in NIST RS274-NGC.v3, Table 8, pg.41.
     NOTE: Independent non-motion/settings parameters are set out of this order for code efficiency 
     and simplicity purposes, but this should not affect proper g-code execution. */
  
  // ([F]: Set feed and seek rates.)
  // TODO: Seek rates can change depending on the direction and maximum speeds of each axes. When
  // max axis speed is installed, the calculation can be performed here, or maybe in the planner.
    
  if (sys.state != STATE_CHECK_MODE) { 
    //  ([M6]: Tool change should be executed here.)

    // [M3,M4,M5]: Update spindle state
    spindle_run(gc.spindle_direction);
  
    // [*M7,M8,M9]: Update coolant state
    coolant_run(gc.coolant_mode);
  }
  
  // [G54,G55,...,G59]: Coordinate system selection
  if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_12)) ) { // Check if called in block
    float coord_data[N_AXIS];
    if (!(settings_read_coord_data(gc.coord_select,coord_data))) { return(STATUS_SETTING_READ_FAIL); } 
    memcpy(gc.coord_system,coord_data,sizeof(coord_data));
  }
  
  // [G4,G10,G28,G30,G92,G92.1]: Perform dwell, set coordinate system data, homing, or set axis offsets.
  // NOTE: These commands are in the same modal group, hence are mutually exclusive. G53 is in this
  // modal group and do not effect these actions.
  switch (non_modal_action) {
    case NON_MODAL_DWELL:
      if (p < 0) { // Time cannot be negative.
        FAIL(STATUS_INVALID_STATEMENT); 
      } else {
        // Ignore dwell in check gcode modes
        if (sys.state != STATE_CHECK_MODE) { mc_dwell(p); }
      }
      break;
    case NON_MODAL_SET_COORDINATE_DATA:
      int_value = trunc(p); // Convert p value to int.
      if ((l != 2 && l != 20) || (int_value < 1 || int_value > N_COORDINATE_SYSTEM)) { // L2 and L20. P1=G54, P2=G55, ... 
        FAIL(STATUS_UNSUPPORTED_STATEMENT); 
      } else if (!axis_words && l==2) { // No axis words.
        FAIL(STATUS_INVALID_STATEMENT);
      } else {
        int_value--; // Adjust P index to EEPROM coordinate data indexing.
        if (l == 20) {
          settings_write_coord_data(int_value,gc.position);
          // Update system coordinate system if currently active.
          if (gc.coord_select == int_value) { memcpy(gc.coord_system,gc.position,sizeof(gc.position)); }
        } else {
          float coord_data[N_AXIS];
          if (!settings_read_coord_data(int_value,coord_data)) { return(STATUS_SETTING_READ_FAIL); }
          // Update axes defined only in block. Always in machine coordinates. Can change non-active system.
          uint8_t i;
          for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
            if ( bit_istrue(axis_words,bit(i)) ) { coord_data[i] = target[i]; }
          }
          settings_write_coord_data(int_value,coord_data);
          // Update system coordinate system if currently active.
          if (gc.coord_select == int_value) { memcpy(gc.coord_system,coord_data,sizeof(coord_data)); }
        }
      }
      axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
      break;
    case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1: 
      // Move to intermediate position before going home. Obeys current coordinate system and offsets 
      // and absolute and incremental modes.
      if (axis_words) {
        // Apply absolute mode coordinate offsets or incremental mode offsets.
        uint8_t i;
        for (i=0; i<N_AXIS; i++) { // Axes indices are consistent, so loop may be used.
          if ( bit_istrue(axis_words,bit(i)) ) {
            if (gc.absolute_mode) {
              target[i] += gc.coord_system[i] + gc.coord_offset[i];
            } else {
              target[i] += gc.position[i];
            }
          } else {
            target[i] = gc.position[i];
          }
        }
        mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false);
      }
      // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM
      float coord_data[N_AXIS];
      if (non_modal_action == NON_MODAL_GO_HOME_1) { 
        if (!settings_read_coord_data(SETTING_INDEX_G30 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }     
      } else {
        if (!settings_read_coord_data(SETTING_INDEX_G28 ,coord_data)) { return(STATUS_SETTING_READ_FAIL); }     
      }      
      mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], settings.default_seek_rate, false); 
      memcpy(gc.position, coord_data, sizeof(coord_data)); // gc.position[] = coord_data[];
      axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
      break;
    case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1:
      if (non_modal_action == NON_MODAL_SET_HOME_1) { 
        settings_write_coord_data(SETTING_INDEX_G30,gc.position);
      } else {
        settings_write_coord_data(SETTING_INDEX_G28,gc.position);
      }
      break;    
    case NON_MODAL_SET_COORDINATE_OFFSET:
      if (!axis_words) { // No axis words
        FAIL(STATUS_INVALID_STATEMENT);
      } else {
        // Update axes defined only in block. Offsets current system to defined value. Does not update when
        // active coordinate system is selected, but is still active unless G92.1 disables it. 
        uint8_t i;
        for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used.
          if (bit_istrue(axis_words,bit(i)) ) {
            gc.coord_offset[i] = gc.position[i]-gc.coord_system[i]-target[i];
          }
        }
      }
      axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
      break;
    case NON_MODAL_RESET_COORDINATE_OFFSET: 
      clear_vector(gc.coord_offset); // Disable G92 offsets by zeroing offset vector.
      break;
  }

  // [G0,G1,G2,G3,G80]: Perform motion modes. 
  // NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes. 
  // Enter motion modes only if there are axis words or a motion mode command word in the block.
  if ( bit_istrue(modal_group_words,bit(MODAL_GROUP_1)) || axis_words ) {

    // G1,G2,G3 require F word in inverse time mode.  
    if ( gc.inverse_feed_rate_mode ) { 
      if (inverse_feed_rate < 0 && gc.motion_mode != MOTION_MODE_CANCEL) {
        FAIL(STATUS_INVALID_STATEMENT);
      }
    }
    // Absolute override G53 only valid with G0 and G1 active.
    if ( absolute_override && !(gc.motion_mode == MOTION_MODE_SEEK || gc.motion_mode == MOTION_MODE_LINEAR)) {
      FAIL(STATUS_INVALID_STATEMENT);
    }
    // Report any errors.  
    if (gc.status_code) { return(gc.status_code); }

    // Convert all target position data to machine coordinates for executing motion. Apply
    // absolute mode coordinate offsets or incremental mode offsets.
    // NOTE: Tool offsets may be appended to these conversions when/if this feature is added.
    uint8_t i;
    for (i=0; i<=2; i++) { // Axes indices are consistent, so loop may be used to save flash space.
      if ( bit_istrue(axis_words,bit(i)) ) {
        if (!absolute_override) { // Do not update target in absolute override mode
          if (gc.absolute_mode) {
            target[i] += gc.coord_system[i] + gc.coord_offset[i]; // Absolute mode
          } else {
            target[i] += gc.position[i]; // Incremental mode
          }
        }
      } else {
        target[i] = gc.position[i]; // No axis word in block. Keep same axis position.
      }
    }
  
    switch (gc.motion_mode) {
      case MOTION_MODE_CANCEL: 
        if (axis_words) { FAIL(STATUS_INVALID_STATEMENT); } // No axis words allowed while active.
        break;
      case MOTION_MODE_SEEK:
        if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} 
        else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false); }
        break;
      case MOTION_MODE_LINEAR:
        // TODO: Inverse time requires F-word with each statement. Need to do a check. Also need
        // to check for initial F-word upon startup. Maybe just set to zero upon initialization
        // and after an inverse time move and then check for non-zero feed rate each time. This
        // should be efficient and effective.
        if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} 
        else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], 
          (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); }
        break;
      case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC:
        // Check if at least one of the axes of the selected plane has been specified. If in center 
        // format arc mode, also check for at least one of the IJK axes of the selected plane was sent.
        if ( !( bit_false(axis_words,bit(gc.plane_axis_2)) ) || 
             ( !r && !offset[gc.plane_axis_0] && !offset[gc.plane_axis_1] ) ) { 
          FAIL(STATUS_INVALID_STATEMENT);
        } else {
          if (r != 0) { // Arc Radius Mode
            /* 
              We need to calculate the center of the circle that has the designated radius and passes
              through both the current position and the target position. This method calculates the following
              set of equations where [x,y] is the vector from current to target position, d == magnitude of 
              that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to
              the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to the 
              length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form the new point 
              [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc.
              
              d^2 == x^2 + y^2
              h^2 == r^2 - (d/2)^2
              i == x/2 - y/d*h
              j == y/2 + x/d*h
              
                                                                   O <- [i,j]
                                                                -  |
                                                      r      -     |
                                                          -        |
                                                       -           | h
                                                    -              |
                                      [0,0] ->  C -----------------+--------------- T  <- [x,y]
                                                | <------ d/2 ---->|
                        
              C - Current position
              T - Target position
              O - center of circle that pass through both C and T
              d - distance from C to T
              r - designated radius
              h - distance from center of CT to O
              
              Expanding the equations:
    
              d -> sqrt(x^2 + y^2)
              h -> sqrt(4 * r^2 - x^2 - y^2)/2
              i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 
              j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
             
              Which can be written:
              
              i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
              j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
              
              Which we for size and speed reasons optimize to:
    
              h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
              i = (x - (y * h_x2_div_d))/2
              j = (y + (x * h_x2_div_d))/2
              
            */
            
            // Calculate the change in position along each selected axis
            float x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0];
            float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1];
            
            clear_vector(offset);
            // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller
            // than d. If so, the sqrt of a negative number is complex and error out.
            float h_x2_div_d = 4 * r*r - x*x - y*y;
            if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); }
            // Finish computing h_x2_div_d.
            h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d)
            // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below)
            if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; }
            
            /* The counter clockwise circle lies to the left of the target direction. When offset is positive,
               the left hand circle will be generated - when it is negative the right hand circle is generated.
               
               
                                                             T  <-- Target position
                                                             
                                                             ^ 
                  Clockwise circles with this center         |          Clockwise circles with this center will have
                  will have > 180 deg of angular travel      |          < 180 deg of angular travel, which is a good thing!
                                                   \         |          /   
      center of arc when h_x2_div_d is positive ->  x <----- | -----> x <- center of arc when h_x2_div_d is negative
                                                             |
                                                             |
                                                             
                                                             C  <-- Current position                                 */
                    
    
            // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), 
            // even though it is advised against ever generating such circles in a single line of g-code. By 
            // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of
            // travel and thus we get the unadvisably long arcs as prescribed.
            if (r < 0) { 
                h_x2_div_d = -h_x2_div_d; 
                r = -r; // Finished with r. Set to positive for mc_arc
            }        
            // Complete the operation by calculating the actual center of the arc
            offset[gc.plane_axis_0] = 0.5*(x-(y*h_x2_div_d));
            offset[gc.plane_axis_1] = 0.5*(y+(x*h_x2_div_d));

          } else { // Arc Center Format Offset Mode            
            r = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); // Compute arc radius for mc_arc
          }
          
          // Set clockwise/counter-clockwise sign for mc_arc computations
          uint8_t isclockwise = false;
          if (gc.motion_mode == MOTION_MODE_CW_ARC) { isclockwise = true; }
    
          // Trace the arc
          mc_arc(gc.position, target, offset, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2,
            (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode,
            r, isclockwise);
        }            
        break;
    }
    
    // Report any errors.
    if (gc.status_code) { return(gc.status_code); }    
    
    // As far as the parser is concerned, the position is now == target. In reality the
    // motion control system might still be processing the action and the real tool position
    // in any intermediate location.
    memcpy(gc.position, target, sizeof(target)); // gc.position[] = target[];
  }
  
  // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may 
  // refill and can only be resumed by the cycle start run-time command.
  if (gc.program_flow) {
    plan_synchronize(); // Finish all remaining buffered motions. Program paused when complete.
    sys.auto_start = false; // Disable auto cycle start. Forces pause until cycle start issued.
    
    // If complete, reset to reload defaults (G92.2,G54,G17,G90,G94,M48,G40,M5,M9). Otherwise,
    // re-enable program flow after pause complete, where cycle start will resume the program.
    if (gc.program_flow == PROGRAM_FLOW_COMPLETED) { mc_reset(); }
    else { gc.program_flow = PROGRAM_FLOW_RUNNING; }
  }    
  
  return(gc.status_code);
}
Esempio n. 26
0
void trunc_l_d(double *source,long long *dest)
{
  *dest = trunc(*source);
}
Esempio n. 27
0
int main(int argc, char *argv[]) {
  wq.size = 0;
  wq.head = NULL;
  wq.tail = NULL;
  wq.push_wait = &push_wait;
  wq.peek = &peek;
  wq.pop = &pop;

  rq.size = 0;
  rq.head = NULL;
  rq.tail = NULL;
  rq.push_exponential = &push_exponential;
  rq.peek = &peek;
  rq.pop = &pop;

  dq.size = 0;
  dq.head = NULL;
  dq.tail = NULL;
  dq.push_wait = &push_wait;
  dq.peek = &peek;
  dq.pop = &pop;
  node_counter = 0;


  FILE *file = fopen(argv[1], "r");
  fseek(file, 0, SEEK_END);
  long fsize = ftell(file);
  fseek(file, 0, SEEK_SET);
  char *string  =malloc(fsize+1);
  fread(string, fsize, 1, file);
  fclose(file);

  string[fsize] = 0;

  //add stuff to node -> waiting queue
  char *token, *saveptr,*saveptr2, *temp, *ptr_end;
  int j, k, a;
  for(j = 0; ; j++, string = NULL){
    token = strtok_r(string, "\n", &saveptr);
    if (token == NULL) {
      break;
    }
    //get current time + x
    struct timeval tv;
    //printf("current%d", time);

    Node *n = (Node*)malloc(sizeof(Node));
    n->name = malloc(11);

    for (k = 0; ; k++, token = NULL){
      temp = strtok_r(token, "\' \', \t", &saveptr2);
      if (temp == NULL) {
        break;
      }
      if(k == 0) {
        strcpy(n->name, temp);
      }
      else if(k == 1) {
        a = atoi(temp);
        n->start_time = a;
      }
      else if(k == 2) {
        a = atoi(temp);
        n->cpu_time = a * 1000;
      }
      else {
        a = atoi(temp);
        n->io_count = a;
        n->io_blocks_left = trunc((n->io_count + 8191) / 8192);
        //printf("io blocks left = %d+8191 div 8192 = %d", n->io_count, n->io_blocks_left);
        n->priority = 1;
        n->cpu_completed= 0;
        n->io_block_time = n->cpu_time / n->io_blocks_left;
        n->io_block_next = n->io_block_time;
        //printf("\nblock time: %d", n->io_blocks_left);
        n->time_slice = 10;
        wq.push_wait(&wq, n);
        node_counter++;
      }
    }
  }

  // wait for things to go down
  pthread_t waiting;
  pthread_t ready;
  pthread_t finished;
  if (pthread_create(&waiting, NULL, &exponentialHold, NULL)){
    printf("Could not create thread \n");
  }
  if (pthread_create(&ready, NULL, &exponentialReady, NULL)) {
    printf("Could not create thread \n");
  }
  if (pthread_create(&finished, NULL, &done_queue, NULL)) {
    printf("Could not create thread \n");
  }
  if(pthread_join(waiting, NULL)){
    printf("Could not join thread\n");
  }
  if(pthread_join(ready, NULL)) {
    printf("Could not join thread\n");
  }
  if(pthread_join(finished, NULL)) {
    printf("Could not join thread\n");
  }

  //printf("%s", dq.head->name);
  //pop
  //run
  //cpu_time != compl_time?  push(ready)
  return 0;

}
Esempio n. 28
0
void trunc_w_d(double *source,int *dest)
{
  *dest = trunc(*source);
}
Esempio n. 29
0
npy_double npy_trunc(npy_double x)
{
    return trunc(x);
}
Esempio n. 30
0
    ///\brief Opens joystick port, reads from port and publishes while node is active
    int main(int argc, char **argv)
    {
        diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
        diagnostic_.setHardwareID("none");

        // Parameters
        ros::NodeHandle nh_param("~");
        pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
        nh_param.getParam("dev", joy_dev_);
        nh_param.param<double>("deadzone", deadzone_, 0.05);
        nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
        nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);

        // Checks on parameters
        if (autorepeat_rate_ > 1 / coalesce_interval_)
            ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);

        if (deadzone_ >= 1)
        {
            ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
            deadzone_ /= 32767;
        }

        if (deadzone_ > 0.9)
        {
            ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
            deadzone_ = 0.9;
        }

        if (deadzone_ < 0)
        {
            ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
            deadzone_ = 0;
        }

        if (autorepeat_rate_ < 0)
        {
            ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
            autorepeat_rate_ = 0;
        }

        if (coalesce_interval_ < 0)
        {
            ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
            coalesce_interval_ = 0;
        }

        // Parameter conversions
        double autorepeat_interval = 1 / autorepeat_rate_;
        double scale = -1. / (1. - deadzone_) / 32767.;
        double unscaled_deadzone = 32767. * deadzone_;

        js_event event;
        struct timeval tv;
        fd_set set;
        int joy_fd;
        event_count_ = 0;
        pub_count_ = 0;
        lastDiagTime_ = ros::Time::now().toSec();

        // Big while loop opens, publishes
        while (nh_.ok())
        {
            open_ = false;
            diagnostic_.force_update();
            bool first_fault = true;
            while (true)
            {
                ros::spinOnce();
                if (!nh_.ok())
                    goto cleanup;
                joy_fd = open(joy_dev_.c_str(), O_RDONLY);
                if (joy_fd != -1)
                {
                    // There seems to be a bug in the driver or something where the
                    // initial events that are to define the initial state of the
                    // joystick are not the values of the joystick when it was opened
                    // but rather the values of the joystick when it was last closed.
                    // Opening then closing and opening again is a hack to get more
                    // accurate initial state data.
                    close(joy_fd);
                    joy_fd = open(joy_dev_.c_str(), O_RDONLY);
                }
                if (joy_fd != -1)
                    break;
                if (first_fault)
                {
                    ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
                    first_fault = false;
                }
                sleep(1.0);
                diagnostic_.update();
            }

            ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
            open_ = true;
            diagnostic_.force_update();

            bool tv_set = false;
            bool publication_pending = false;
            tv.tv_sec = 1;
            tv.tv_usec = 0;
            sensor_msgs::Joy joy_msg; // Here because we want to reset it on device close.
            while (nh_.ok())
            {
                ros::spinOnce();

                bool publish_now = false;
                bool publish_soon = false;
                FD_ZERO(&set);
                FD_SET(joy_fd, &set);

                //ROS_INFO("Select...");
                int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
                //ROS_INFO("Tick...");
                if (select_out == -1)
                {
                    tv.tv_sec = 0;
                    tv.tv_usec = 0;
                    //ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
                    continue;
                    //				break; // Joystick is probably closed. Not sure if this case is useful.
                }

                if (FD_ISSET(joy_fd, &set))
                {
                    if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
                        break; // Joystick is probably closed. Definitely occurs.

                    //ROS_INFO("Read data...");
                    joy_msg.header.stamp = ros::Time().now();
                    event_count_++;
                    switch(event.type)
                    {
                    case JS_EVENT_BUTTON:
                    case JS_EVENT_BUTTON | JS_EVENT_INIT:
                        if(event.number >= joy_msg.buttons.size())
                        {
                            int old_size = joy_msg.buttons.size();
                            joy_msg.buttons.resize(event.number+1);
                            for(unsigned int i=old_size; i<joy_msg.buttons.size(); i++)
                                joy_msg.buttons[i] = 0.0;
                        }
                        joy_msg.buttons[event.number] = (event.value ? 1 : 0);
                        // For initial events, wait a bit before sending to try to catch
                        // all the initial events.
                        if (!(event.type & JS_EVENT_INIT))
                            publish_now = true;
                        else
                            publish_soon = true;
                        break;
                    case JS_EVENT_AXIS:
                    case JS_EVENT_AXIS | JS_EVENT_INIT:
                        if(event.number >= joy_msg.axes.size())
                        {
                            int old_size = joy_msg.axes.size();
                            joy_msg.axes.resize(event.number+1);
                            for(unsigned int i=old_size; i<joy_msg.axes.size(); i++)
                                joy_msg.axes[i] = 0.0;
                        }
                        if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
                        {
                            double val = event.value;
                            // Allows deadzone to be "smooth"
                            if (val > unscaled_deadzone)
                                val -= unscaled_deadzone;
                            else if (val < -unscaled_deadzone)
                                val += unscaled_deadzone;
                            else
                                val = 0;
                            joy_msg.axes[event.number] = val * scale;
                        }
                        // Will wait a bit before sending to try to combine events.
                        publish_soon = true;
                        break;
                    default:
                        ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
                        break;
                    }
                }
                else if (tv_set) // Assume that the timer has expired.
                    publish_now = true;

                if (publish_now)
                {
                    // Assume that all the JS_EVENT_INIT messages have arrived already.
                    // This should be the case as the kernel sends them along as soon as
                    // the device opens.
                    //ROS_INFO("Publish...");
                    pub_.publish(joy_msg);
                    publish_now = false;
                    tv_set = false;
                    publication_pending = false;
                    publish_soon = false;
                    pub_count_++;
                }

                // If an axis event occurred, start a timer to combine with other
                // events.
                if (!publication_pending && publish_soon)
                {
                    tv.tv_sec = trunc(coalesce_interval_);
                    tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
                    publication_pending = true;
                    tv_set = true;
                    //ROS_INFO("Pub pending...");
                }

                // If nothing is going on, start a timer to do autorepeat.
                if (!tv_set && autorepeat_rate_ > 0)
                {
                    tv.tv_sec = trunc(autorepeat_interval);
                    tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
                    tv_set = true;
                    //ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
                }

                if (!tv_set)
                {
                    tv.tv_sec = 1;
                    tv.tv_usec = 0;
                }

                diagnostic_.update();
            } // End of joystick open loop.

            close(joy_fd);
            ros::spinOnce();
            if (nh_.ok())
                ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
        }

cleanup:
        ROS_INFO("joy_node shut down.");

        return 0;
    }