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();
}
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) );
}
/*@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;
}
}
}
}