/**
* \ingroup schedul
* gibt den aktuell gültigen Sollzustand des Systems zurück
*
*/
bool TM_SollzustandGetAktuell(SOLL_STATUS *ptrZustand)
{
uint8_t acttime = (TM_hh*10)+(TM_mm/10);
TIMER_DEBUG("\r\nTM-Debug GetsollZustand: Zeit %i Tag %i",acttime, TM_DOW);
return TM_ZustandGet(acttime, TM_DOW, ptrZustand);
}
void TM_SetDayofYear(uint16_t tage)
{
uint8_t i;
TIMER_DEBUG("\r\nSetDayofYear: %i Tage",tage);
TM_MM = 1; // Monate einzeln hochzählen und #Tage im Monat abziehen
i = AnzahlTageImMonat(TM_MM, TM_YY);
TIMER_DEBUG("\r\nMonat #%i: %i Tage - Rest: %i -Start",TM_MM,i,tage);
while ( tage > i ) {
tage -= i;
TM_MM += 1;
i = AnzahlTageImMonat(TM_MM, TM_YY);
TIMER_DEBUG("\r\nMonat #%i: %i Tage - Rest: %i",TM_MM,i,tage);
}
TM_DD = tage;
TM_SetDayOfWeek();
}
/**
* \ingroup schedul
* gibt den zur vorgegebenen Zeit gültigen Sollzustand des Systems zurück
*/
bool TM_ZustandGet(uint8_t acttime, uint8_t actdow, SOLL_STATUS *ptrZustand)
{
uint8_t i,j;
uint8_t index = 0;
uint16_t maxtime = 0;
uint16_t actTageswert = actdow * 256 + acttime; // gesuchter Wochentag + Uhrzeit + offset zur Wochentags unterscheidung
uint16_t tstTageswert;
TIMER_DEBUG("\r\nTM-Debug GetZustand: Tageswert %i",actTageswert);
// each timer
for (i=0; i<TM_MAX_SCHALTZEITEN; i++){
// check if timer > maxtime and timer < actual time
if ((TM_Schaltzeit[i].Uhrzeit != TM_SCHALTER_AUS) ) {
for (j=0; j<7; ++j) { // alle codierten Wochentage durchsuchen
if (TM_Schaltzeit[i].Wochentag & (1<<j)) { // falls Wochentag geschaltet wird
tstTageswert = TM_Schaltzeit[i].Uhrzeit + 256 * j;
TIMER_DEBUG("\r\nTM-Debug GetZustand: Testwert %i",tstTageswert);
if ( (tstTageswert <= actTageswert)
&& (tstTageswert > maxtime) ) {
index=i;
maxtime = tstTageswert;
TIMER_DEBUG("\r\nTM-Debug GetZustand: neuer Index %i",index);
}
}
}
}
}
ptrZustand->Schalter1 = TM_Schaltzeit[index].Zustand.Schalter1;
ptrZustand->Schalter2 = TM_Schaltzeit[index].Zustand.Schalter2;
ptrZustand->Schalter3 = TM_Schaltzeit[index].Zustand.Schalter3;
ptrZustand->TReglerWert = TM_Schaltzeit[index].Zustand.TReglerWert;
return true;
}
bool ContactTransition::GetSuccessors( AStarSearch<ContactTransition> *astarsearch, ContactTransition *parent_node ){
char foot;
ContactTransition *parent = this;
(parent->L_or_R == 'L')? foot='R':foot='L';
//NOTE:
//relative means always with respect to the support foot
//absolute means the position in the global world coordinate frame
//get absolute position of support foot (SF)
double sf_abs_x = parent->g.getX();
double sf_abs_y = parent->g.getY();
double sf_abs_yaw = parent->g.getYawRadian();
TIMER_DEBUG(timer->begin("prepare"));
//project all objects into the reference frame of the SF
std::vector< std::vector<double> > obj =
constraints->prepareObjectPosition(objects, sf_abs_x, sf_abs_y, sf_abs_yaw, parent->L_or_R);
TIMER_DEBUG(timer->end("prepare"));
//get the relative position of the starting free foot (FF)
double ff_rel_x = parent->rel_x_parent;
double ff_rel_y = parent->rel_y_parent;
double ff_rel_yaw = parent->rel_yaw_parent;
std::vector<double> ff_rel = vecD(ff_rel_x, ff_rel_y, ff_rel_yaw);
double ff_hash = hashit<double>(ff_rel);
bool ff_start_feasible = true;
//Check first foot first -> if it is infeasible, we do not need to test
//the rest
if(constraints->actionSpace.find(ff_hash)!=constraints->actionSpace.end()){
//support foot exists in actionSpace, which means that it is not
//the first footstep and we can check its feasibility
std::vector<double> ff_rel_table = constraints->actionSpace.find(ff_hash)->second;
ff_start_feasible = constraints->isFeasible(ff_rel_table, obj);
feasibilityChecks++;
//position constraints
//if(sf_abs_x < -0.3 || sf_abs_x > 3.3 || sf_abs_y < -0.76 || sf_abs_y > 0.76){
//ff_start_feasible = false;
//}
}
if(ff_start_feasible){
DEBUG(
if(foot=='L'){
ros::ColorFootMarker rp(parent->g.getX(), parent->g.getY(), parent->g.getYawRadian(),"white");
rp.reset();
rp.publish();
}
)
TIMER_DEBUG( timer->begin("actionExpansion") );
ActionSpace::const_iterator it;
for( it = constraints->actionSpace.begin(); it != constraints->actionSpace.end(); ++it ){
feasibilityChecks++;
if(constraints->isFeasible( it->second, obj)){
//relative position of the next proposed pos of FF
TIMER_DEBUG( timer->begin("ff") );
double next_ff_rel_x = it->second.at(0);
double next_ff_rel_y = it->second.at(1);
double next_ff_rel_yaw = it->second.at(2);//toRad(it->second.at(2)*100.0);
ros::Geometry ng = computeAbsFFfromRelFF(sf_abs_x, sf_abs_y, sf_abs_yaw, next_ff_rel_x, next_ff_rel_y, next_ff_rel_yaw, foot);
ContactTransition next(ng);
next.L_or_R = foot;
next.rel_x_parent = it->second.at(0);
next.rel_y_parent = it->second.at(1);
next.rel_yaw_parent = it->second.at(2);
next.rel_x = next_ff_rel_x;
next.rel_y = next_ff_rel_y;
next.rel_yaw = next_ff_rel_yaw;
next.cost_so_far = 0.0;
astarsearch->AddSuccessor(next);
DEBUG(
if(foot=='L'){
ros::ColorFootMarker rp(ng.getX(), ng.getY(), ng.getYawRadian(), "green");
rp.publish();
}else{
ros::ColorFootMarker rp(ng.getX(), ng.getY(), ng.getYawRadian(), "red");
rp.publish();
}
)
TIMER_DEBUG( timer->end("ff") );
continue;
}
