double upm_staged_rpg::compute_rpg(){
double curr_mu_time = mu_TIME.value();
std::set<int> ha;
bounds = get_initial_bounds();
double curr_time = 0;
action_pre.clear();
achieved_by.clear();
achieved_by_first.clear();
rpg_state_facts.clear();
rpg_facts.clear();
init_facts.clear();
temporal_layers.clear();
if (bool_goal_fact_layer.size() == 0 && num_goal_fact_layer.size() == 0) return 999999;
double rpg_length = 999;
static state* orig = new state;
StateCopy(orig, workingstate);
state* temp_ws = new state(workingstate);
state* backup_state_lower = new state(workingstate);
state* backup_state_upper = new state(workingstate);
state* next_rpg_state_lower = new state(workingstate);
StateCopy(backup_state_lower, workingstate);
state* next_rpg_state_upper = new state(workingstate);
StateCopy(backup_state_upper, workingstate);
state* trpg0 = new state(workingstate);
temporal_layers.push_back( std::make_pair( std::make_pair(trpg0, trpg0), mu_TIME.value()) );
init_facts = get_fact_layer(temp_ws);
std::set<mu_0_boolean*> f_l;
while (true){
std::map<mu__any*, std::set<int> > curr_ach_by;
std::map<int, std::vector<mu__any*> > curr_a_pre;
std::map<mu__any*, int> curr_ach_by_first;
achieved_by.push_back(curr_ach_by);
action_pre.push_back(curr_a_pre);
achieved_by_first.push_back(curr_ach_by_first);
StateCopy(backup_state_lower, temporal_layers.at(temporal_layers.size()-1).first.first);
StateCopy(backup_state_upper, temporal_layers.at(temporal_layers.size()-1).first.second);
StateCopy(workingstate, backup_state_lower);
// curr_time += (10*mu_T);
for (int ix = 0; ix < RULES_IN_WORLD; ix++){
StateCopy(workingstate, backup_state_lower);
if (metric_condition(ix) == true){
action_pre.at(action_pre.size()-1).insert(std::make_pair(ix, Rules->generator->all_precond_array(ix)));
std::vector<mu__any*> aef = Rules->generator->effects_all_array(ix);
for (int ix44 = 0; ix44 < aef.size(); ix44++){
if (init_facts.count(aef.at(ix44))==0 && ix != 0){
achieved_by_first.at(achieved_by_first.size()-1).insert(std::make_pair(aef.at(ix44), ix));
StateCopy(workingstate, orig);
if (Rules->generator->Condition(ix)){
achieved_by.at(achieved_by.size()-1)[aef.at(ix44)].insert(ix);
}
StateCopy(workingstate, backup_state_lower);
}
}
StateCopy(workingstate, next_rpg_state_lower);
Rules->generator->Code_numeric_ff_minus(ix);
StateCopy(next_rpg_state_lower, workingstate);
StateCopy(workingstate, next_rpg_state_upper);
Rules->generator->Code_numeric_ff_plus(ix);
StateCopy(next_rpg_state_upper, workingstate);
}
} // FOR LOOP END
StateCopy(workingstate, next_rpg_state_upper);
workingstate->fire_processes_plus(1);
mu_event_check();
StateCopy(next_rpg_state_upper, workingstate);
StateCopy(workingstate, next_rpg_state_lower);
workingstate->fire_processes_minus(1);
mu_event_check();
StateCopy(next_rpg_state_lower, workingstate);
state* trpg_l = new state(next_rpg_state_lower);
// cout << "\n\n*****LOWER STATE PRINT*****\n" << endl;
// next_rpg_state_lower->print();
update_lower_bound(bounds);
update_lower_bound(goal_bounds);
StateCopy(workingstate, next_rpg_state_upper);
state* trpg_u = new state(next_rpg_state_upper);
// cout << "\n\n*****UPPER STATE PRINT*****\n" << endl;
// next_rpg_state_upper->print();
update_upper_bound(bounds);
update_upper_bound(goal_bounds);
temporal_layers.push_back(std::make_pair(std::make_pair(trpg_l, trpg_u), mu_TIME.value()));
// print_bounds();
if (goal_check()){
delete trpg_u;
delete trpg_l;
break;
}
if (
(mu_TIME.value() > args->SRPG_horizon.value)
){
StateCopy(workingstate, temp_ws);
workingstate->set_h_val(999);
workingstate->set_g_val(0);
workingstate->set_f_val();
delete trpg0;
delete backup_state_lower;
delete backup_state_upper;
delete next_rpg_state_upper;
delete next_rpg_state_lower;
delete temp_ws;
delete trpg_u;
delete trpg_l;
// cout << "\n\nENDING THE LOOP WITH NO GOAL!\n\n" << endl;
return 999;
}
delete trpg_u;
delete trpg_l;
} // WHILE END
/*************************
*
* BACKWARDS SEARCH
*
*/
std::set<mu__any*> backwards = all_goal_fact_layer;
std::vector<std::set<int> >rpg_final_all;
int curr_action_layer = achieved_by_first.size()-1;
std::set<mu__any*> g1 = get_fact_layer(temporal_layers.at(1).first.second);
std::set<mu__any*>::iterator itt;
std::set<mu__any*> temp_set;
while (curr_action_layer >= 0 ){
std::set<int>rpg_final;
rpg_final.clear();
for (itt=backwards.begin(); itt!=backwards.end(); ++itt){
if ((*itt)->get_mu_type() == "mu_0_boolean" && curr_action_layer >= 1 && achieved_by_first.at(curr_action_layer-1).count((*itt)) > 0) {
temp_set.insert((*itt));
continue;
}
if (rpg_final.count(achieved_by_first.at(curr_action_layer)[*itt]) >= 1) {
continue;
}
std::copy(action_pre.at(curr_action_layer)[achieved_by_first.at(curr_action_layer)[*itt]].begin(),
action_pre.at(curr_action_layer)[achieved_by_first.at(curr_action_layer)[*itt]].end(),
std::inserter(temp_set, temp_set.end()));
if (init_facts.count(*itt) == 0 ){
rpg_final.insert(achieved_by_first.at(curr_action_layer)[*itt]);
if (g1.count(*itt)==1){
ha.insert(achieved_by.at(curr_action_layer)[(*itt)].begin(), achieved_by.at(curr_action_layer)[(*itt)].end());
}
}
}
rpg_final_all.push_back(rpg_final);
backwards = temp_set;
curr_action_layer--;
temp_set.clear();
if (backwards.size() == 0) break;
}
if (ha.size() < 1){
ha.insert(0);
}
for (int i = 1; i < rpg_final_all.size()-1; i++){
if (rpg_final_all.at(i).size() == 0){
rpg_final_all.at(i).insert(0);
}
}
rpg_length = 0;
for (int i = 0; i < rpg_final_all.size(); i++){
rpg_length += rpg_final_all.at(i).size();
}
StateCopy(workingstate, temp_ws);
workingstate->set_h_val(rpg_length);
workingstate->set_g_val(0);
workingstate->set_f_val();
state * tend = new state;
StateCopy(tend, workingstate);
helpful_actions[tend] = ha;
delete next_rpg_state_upper;
delete next_rpg_state_lower;
delete backup_state_lower;
delete backup_state_upper;
delete temp_ws;
delete trpg0;
delete tend;
return (rpg_length);
}
int main()
{
punct teren[4]; punct poarta[4]; punct M;
double alfa,x,y;
data_reader(teren,poarta,&x,&y,&alfa);
M.x = x; M.y = y;
/*
+ in functie de unghiul cu care pucul se deplaseaza, exista sansa ca el sa loveasca
doua ziduri, algoritmul determina care dintre cele doua ziduri este lovit
+ colturile terenului sunt numerotate de la 0 la 3 iar zidurile sunt identificate
in functie de colturi: 30 10 12 23
(3)----------------------------(2)
| 90 |
| |
| 180 0 |
| |
| 270 |
(0)----------------------------(1)
*/
while(1)//programul ruleaza pana cand energia cinetica devine 0
{
if(alfa == 0 || alfa == 90 || alfa == 180 || alfa == 270)
{
if(alfa == 180 && poarta[0].y <= M.y && poarta[1].y >= M.y)
{
printf("%3.3f %3.3f\n",x,y);
printf("%3.3f %3.3f\n",poarta[0].x,M.y);
printf("%i\n",-1);
return 0;
}else if(alfa == 0 && poarta[2].y <= M.y && poarta[3].y >= M.y)
{
printf("%3.3f %3.3f\n",x,y);
printf("%3.3f %3.3f\n",poarta[2].x,M.y);
printf("%i\n",1);
return 0;
}
printf("%i\n",0);
return 0;
}
printf("%3.3f %3.3f\n",M.x,M.y);
if(alfa > 0 && alfa < 90) {//poate lovi zidul 12 sau zidul 23
/*se intersecteaza traiectoria pucului cu zidul 12,
apoi verifica daca aceasta se afla intradevar in interiorul terenului
conditia implica si egalitate pentru cazul in care pucul loveste chiar
coltul terenului, in aceasta situatie mantinela se considera ca fiind atinsa
si este verificata si conditia pentru gol*/
intersectie(M,alfa,teren,12,&x,&y);
if(y <= teren[2].y)
{
printf("%3.3f %3.3f\n",x,y);
goal_check(poarta,2,3,x,y);
return 0;
}
/*in cazul in care pucul nu a lovit mantinela
se intersecteaza traiectoria cu al doilea zid posibil
si se retine locul de unde ricoseaza si se modifica unghiul alfa
*/
intersectie(M,alfa,teren,23,&x,&y);
if(x < teren[2].x)//pucul loveste zidul 23
{
M.x = x;
M.y = y;
alfa = 360 - alfa;
}
}
else if(alfa > 90 && alfa < 180)
{
intersectie(M,alfa,teren,30,&x,&y);
if(y <= teren[3].y)
{
printf("%3.3f %3.3f\n",x,y);
goal_check(poarta,0,1,x,y);
return 0;
}
intersectie(M,alfa,teren,23,&x,&y);
if(x > teren[3].x)
{
M.x = x;
M.y = y;
alfa = 180 + (180 - alfa);
}
}
else if(alfa > 180 && alfa < 270)
{
intersectie(M,alfa,teren,30,&x,&y);
if(y >= teren[0].y)
{
printf("%3.3f %3.3f\n",x,y);
goal_check(poarta,0,1,x,y);
return 0;
}
intersectie(M,alfa,teren,10,&x,&y);
if(x > teren[0].x)
{
M.x = x;
M.y = y;
alfa = 180 + (180 - alfa);
}
}
else if(alfa > 270 && alfa < 360)
{
intersectie(M,alfa,teren,12,&x,&y);
if(y >= teren[1].y)
{
printf("%3.3f %3.3f\n",x,y);
goal_check(poarta,2,3,x,y);
return 0;
}
intersectie(M,alfa,teren,10,&x,&y);
if(x < teren[1].x)
{
M.x = x;
M.y = y;
alfa = 360 - alfa;
}
}
}
return 0;
}
void TIMER_CALL()
{
TimeCount++;
if(LED_test==1)
calc_senc();
if(start0==1){
now_ENC_R+=(ENC_R-15000);
now_ENC_L+=(ENC_L-15000);
sum_ENC_R+=(ENC_R-15000);
sum_ENC_L+=(ENC_L-15000);
NowEnc=now_ENC_R-now_ENC_L;
now_vel_R=(double)(ENC_R-15000)*13.7375; // mm/s
now_vel_L=(double)(ENC_L-15000)*13.7375; // mm/s
now_vel=(now_vel_R+now_vel_L)/2.0;
if(goal_frag1==1 && (circl_count==1 || StCurveCheck2==curve)){
SPEED_ERR=now_vel-Desire_vel_nomal;
SPEED_SUM_ERR+=SPEED_ERR;
}
if(goal_frag1==1)
EncTimeCount++;
if(EncTimeCount==10){
EncTimeCount=0;
angl_vel=sum_ENC_R-sum_ENC_L;
if(angl_vel >= 0){
if(angl_vel == 0)
angl_vel=1;
curvature=(sum_ENC_R+sum_ENC_L)*Tred/(2*angl_vel);
if(curvature<0)
curvature*=(-1);
}
else if(angl_vel < 0){
angl_vel*=(-1);
curvature=(sum_ENC_R+sum_ENC_L)*Tred/(2*angl_vel);
if(curvature<0)
curvature*=(-1);
}
if(curvature/10 < 30){
SC_check=curve;
// LED1=ON;
K_corner*=0.9;
}
else if(curvature/10 > 30){
SC_check=ST;
// LED1=OFF;
}
for(u_count=0; u_count<5; u_count++){
if(SC_box[u_count]==curve)
SC_count++;
}
if(now_state==ST){
if(SC_count > 2)
LED1=ON;
}
else if(now_state==curve){
if(SC_count <= 2)
LED1=ON;
}
else if(now_state==unknown){
LED1=OFF;
LED2=OFF;
state_count++;
}
if(state_count==10){
state_count=0;
LED2=ON;
if(SC_count > 2){
now_state=curve;
}
else if(SC_count <= 2){
now_state=ST;
}
}
for(u_count=0; u_count<4; u_count++){
SC_box[u_count+1]=SC_box[u_count];
}
SC_box[0]=SC_check;
SC_count=0;
sum_ENC_L=0;
sum_ENC_R=0;
}
calc_senc();
StCurveCheck2=goal_check(now_ENC_R, now_ENC_L);
if(mot_frag==1){
if((circl_count==2 || circl_count==3) && StCurveCheck2==ST){
if(Accel > 0.0){
if(first_target_frag2==0){
first_target_frag2=1;
first_target_speed=(now_vel_R+now_vel_L)/2.0;
}
if(Desire_vel < Target_vel){
AccelTimeCount+=0.001;
Desire_vel=first_target_speed+(Accel*AccelTimeCount);
}
else if(Desire_vel >= Target_vel ){
Desire_vel=Target_vel;
}
}
else if(Accel < 0.0){
if(Desire_vel > Target_vel){
AccelTimeCount+=0.0017;
Desire_vel=Desire_vel+(Accel*AccelTimeCount);
}
else if(Desire_vel < Target_vel ){
Desire_vel=Target_vel;
}
}
SPEED_ERR_R=now_vel_R-Desire_vel;
SPEED_SUM_ERR_R+=SPEED_ERR_R;
SPEED_ERR_L=now_vel_L-Desire_vel;
SPEED_SUM_ERR_L+=SPEED_ERR_L;
}
mot_drive(ERR, sub_ERR, PRE_ERR, SUM_ERR, GYRO_ERR, SPEED_ERR_R, SPEED_SUM_ERR_R, SPEED_ERR_L, SPEED_SUM_ERR_L, SPEED_ERR, SPEED_SUM_ERR);
}
ENC_R=15000;
ENC_L=15000;
}
if(stop_frag==1)
stop_count++;
}
