Esempio n. 1
0
void motorSimAxis::process(double delta )
{
  double lastpos;
  int done = 0;

  lastpos = nextpoint_.axis[0].p;
  nextpoint_.T += delta;
  routeFind( route_, reroute_, &endpoint_, &nextpoint_ );
  /*  if (reroute_ == ROUTE_NEW_ROUTE) routePrint( route_, reroute_, &endpoint_, &nextpoint_, stdout ); */
  reroute_ = ROUTE_CALC_ROUTE;

  /* No, do a limits check */
  if (homing_ && 
    ((lastpos - home_) * (nextpoint_.axis[0].p - home_)) <= 0)
  {
    /* Homing and have crossed the home sensor - return to home */
    homing_ = 0;
    reroute_ = ROUTE_NEW_ROUTE;
    endpoint_.axis[0].p = home_;
    endpoint_.axis[0].v = 0.0;
  }
  if ( nextpoint_.axis[0].p > hiHardLimit_ && nextpoint_.axis[0].v > 0 )
  {
    if (homing_) setVelocity(-endpoint_.axis[0].v, 0.0 );
    else
    {
      reroute_ = ROUTE_NEW_ROUTE;
      endpoint_.axis[0].p = hiHardLimit_;
      endpoint_.axis[0].v = 0.0;
    }
  }
  else if (nextpoint_.axis[0].p < lowHardLimit_ && nextpoint_.axis[0].v < 0)
  {
    if (homing_) setVelocity(-endpoint_.axis[0].v, 0.0 );
    else
    {
      reroute_ = ROUTE_NEW_ROUTE;
      endpoint_.axis[0].p = lowHardLimit_;
      endpoint_.axis[0].v = 0.0;
    }
  }

  if (nextpoint_.axis[0].v ==  0) {
    if (!deferred_move_) {
      done = 1;
    }
  } else {
    done = 0;
  }

  setDoubleParam (pC_->motorPosition_,         (nextpoint_.axis[0].p+enc_offset_));
  setDoubleParam (pC_->motorEncoderPosition_,  (nextpoint_.axis[0].p+enc_offset_));
  setIntegerParam(pC_->motorStatusDirection_,  (nextpoint_.axis[0].v >  0));
  setIntegerParam(pC_->motorStatusDone_,       done);
  setIntegerParam(pC_->motorStatusHighLimit_,  (nextpoint_.axis[0].p >= hiHardLimit_));
  setIntegerParam(pC_->motorStatusHome_,       (nextpoint_.axis[0].p == home_));
  setIntegerParam(pC_->motorStatusMoving_,     !done);
  setIntegerParam(pC_->motorStatusLowLimit_,   (nextpoint_.axis[0].p <= lowHardLimit_));
  callParamCallbacks();
}
Esempio n. 2
0
static void motorSimProcess( AXIS_HDL pAxis, double delta )
{
  double lastpos = pAxis->nextpoint.axis[0].p;
  int done = 0;

  pAxis->nextpoint.T += delta;
  routeFind( pAxis->route, pAxis->reroute, &(pAxis->endpoint), &(pAxis->nextpoint) );
 /*  if (pAxis->reroute == ROUTE_NEW_ROUTE) routePrint( pAxis->route, pAxis->reroute, &(pAxis->endpoint), &(pAxis->nextpoint), stdout ); */
  pAxis->reroute = ROUTE_CALC_ROUTE;

  /* No, do a limits check */
  if (pAxis->homing && 
      ((lastpos - pAxis->home) * (pAxis->nextpoint.axis[0].p - pAxis->home)) <= 0)
    {
      /* Homing and have crossed the home sensor - return to home */
      pAxis->homing = 0;
      pAxis->reroute = ROUTE_NEW_ROUTE;
      pAxis->endpoint.axis[0].p = pAxis->home;
      pAxis->endpoint.axis[0].v = 0.0;
    }
  if ( pAxis->nextpoint.axis[0].p > pAxis->hiHardLimit && pAxis->nextpoint.axis[0].v > 0 )
    {
      if (pAxis->homing) motorAxisVelocity( pAxis, -pAxis->endpoint.axis[0].v, 0.0 );
      else
	{
	  pAxis->reroute = ROUTE_NEW_ROUTE;
	  pAxis->endpoint.axis[0].p = pAxis->hiHardLimit;
	  pAxis->endpoint.axis[0].v = 0.0;
	}
    }
  else if (pAxis->nextpoint.axis[0].p < pAxis->lowHardLimit && pAxis->nextpoint.axis[0].v < 0)
    {
      if (pAxis->homing) motorAxisVelocity( pAxis, -pAxis->endpoint.axis[0].v, 0.0 );
      else
	{
	  pAxis->reroute = ROUTE_NEW_ROUTE;
	  pAxis->endpoint.axis[0].p = pAxis->lowHardLimit;
	  pAxis->endpoint.axis[0].v = 0.0;
	}
    }

  if (pAxis->nextpoint.axis[0].v ==  0) {
    if (!pAxis->deferred_move) {
      done = 1;
    }
  } else {
    done = 0;
  }

  motorParam->setDouble(  pAxis->params, motorAxisPosition,      (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) );
  motorParam->setDouble(  pAxis->params, motorAxisEncoderPosn,   (pAxis->nextpoint.axis[0].p+pAxis->enc_offset) );
  motorParam->setInteger( pAxis->params, motorAxisDirection,     (pAxis->nextpoint.axis[0].v >  0) );
  motorParam->setInteger( pAxis->params, motorAxisDone,          done );
  motorParam->setInteger( pAxis->params, motorAxisHighHardLimit, (pAxis->nextpoint.axis[0].p >= pAxis->hiHardLimit) );
  motorParam->setInteger( pAxis->params, motorAxisHomeSignal,    (pAxis->nextpoint.axis[0].p == pAxis->home) );
  motorParam->setInteger( pAxis->params, motorAxisMoving,        !done );
  motorParam->setInteger( pAxis->params, motorAxisLowHardLimit,  (pAxis->nextpoint.axis[0].p <= pAxis->lowHardLimit) );
}
Esempio n. 3
0
/*@maynotreturn@*/ 
void routeParser(void)
{
	struct sRoute *		pRoute = NULL;
	unsigned short		nr = 0;
	unsigned short		i = 0;
	unsigned short		j = 0;
	enum eToken		tok = tokNone;
	char			name[NAMELEN];
	enum eRoutePartType	ePartType;
 
	parserExpectTokenNeu(tokLBracket);
	parserExpectTokenNeu(tokZuFas);
	nr = parserExpectNumber("Anzahl Zugfahrstrassen erwartet");
	routes(nr);		/* construct route structure */

	parserExpectTokenNeu(tokLBracket);
	while(NULL != (pRoute = routeGet(i)))
	{
		tok = parserGetWord(name);
		parserAssert(tokNone == tok, "Name der Zugfahrstrasse erwartet");
		routeSetName(pRoute, name);
		routeSingleParser(pRoute);
		i++;
	}

	tok = parserGetToken();
	parserAssert(tokRBracket == tok, "zu viele Zugfahrstrassen angegeben");
	/* I did not use parserExpectToken, because a better error 
	 * message can be given
	 */

	parserExpectTokenNeu(tokRBracket);

	/* now make the resolving of the route names given as illegal route
	 *  or conditional route. Note, these routes were probably not known
	 *  before the parsing of the last route
	 */
	i = 0;
	while(NULL != (pRoute = routeGet(i)))
	{
		for(j = 0; j < routeGetParts(pRoute); j++)
		{
			switch(routeGetPartType(pRoute, j))
			{
			case RoutePartIllegalRoute:
				/* now get the name of the illegal route, search for it
				 *  in the routes and store the pointer to the route found
				 */
				(void) routeGetIllegalRouteName(pRoute, name, j);
				routeSetIllegalRoute(pRoute, routeFind(name), j);
				break;
			
			case RoutePartCondRoute:
				(void) routeGetCondRouteName(pRoute, name, j);
				routeSetCondRoute(pRoute, routeFind(name), j);
				break;

			default:
				break;
			}
		}
	}
}