void ConfigWindow::update_interactions()
{
if(!getSetting<bool>("general/interactions-enabled")) return;
update_distances();
std::mt19937 gen(QDateTime::currentMSecsSinceEpoch());
std::uniform_real_distribution<> real_dis(0, 1);
// For each interaction
for(auto &i: interactions) {
// check probability
// For each pony that starts this interaction
for(auto &p: ponies) {
if(p->name.toLower() != i.pony) continue;
if(p->in_interaction) continue;
if((p->interaction_delays.find(i.name) != p->interaction_delays.end()) && // Check if there is an active delay for this interaction in this pony
(p->interaction_delays.at(i.name) > QDateTime::currentMSecsSinceEpoch())) continue;
// TODO: add it to interaction instance, and when cancelling, cancel interaction for every pony in interaction
std::vector<std::shared_ptr<Pony>> interaction_targets;
// For each target of that interaction
for(const QVariant &p_target: i.targets) {
// For each found target
for(auto &pp: ponies) {
if(pp == p) continue; // Do not interact with self
if(pp->name.toLower() != p_target.toString()) continue;
if(pp->in_interaction) {
continue; // The pony is already in an interaction
} else if(pp->sleeping) {
continue; // Sleeping ponies do not interact
} else if(distances.at(std::make_pair(p->name.toLower(), p_target.toString())) > i.distance) {
continue; // The pony is too far, check the rest
} else if((pp->interaction_delays.find(i.name) != pp->interaction_delays.end()) && // Check if there is an active delay for this interaction in this pony
(pp->interaction_delays.at(i.name) > QDateTime::currentMSecsSinceEpoch())) {
continue; // The pony has an active delay for this interaction
} else {
// We found a suitable pony, we can do the interaction
if(real_dis(gen) <= i.probability) {
// Only interact with specified probability
interaction_targets.push_back(pp);
}
}
}
}
if(interaction_targets.empty()) {
// We didn't find anypony to interact with, check the next initiating pony for this interaction
continue;
}
QString selected_behavior = i.select_behavior();
p->current_interaction = i.name;
p->current_interaction_delay = i.reactivation_delay;
p->in_interaction = true;
p->change_behavior_to(selected_behavior);
if(i.select_every_taget == false) { // Select random pony to interact with
std::uniform_int_distribution<> dis(0, interaction_targets.size()-1);
uint32_t rnd = dis(gen);
interaction_targets[rnd]->current_interaction = i.name;
interaction_targets[rnd]->current_interaction_delay = i.reactivation_delay;
interaction_targets[rnd]->in_interaction = true;
interaction_targets[rnd]->change_behavior_to(selected_behavior);
if(getSetting<bool>("general/debug")) {
qDebug() << p->name << "interacting with" << interaction_targets[rnd]->name << "using behavior" << selected_behavior << "for interaction" << i.name;
}
} else {
// Interact with all suitable ponies
for(auto &t: interaction_targets) {
t->current_interaction = i.name;
t->current_interaction_delay = i.reactivation_delay;
t->in_interaction = true;
t->change_behavior_to(selected_behavior);
if(getSetting<bool>("general/debug")) {
qDebug() << p->name << " interacting with " << t->name << " using " << selected_behavior << " for interaction " << i.name;
}
}
}
}
}
}
uint8_t behaviour_obstacle_avoidance() {
copro_ir_startMeasure();
update_distances();
dist_l = (dist_l<250)? (dist_l+5):255;
dist_r = (dist_r<250)? (dist_r+5):255;
dist_f = (dist_f>5)? (dist_f-5):0;
dist_max_ls = (dist_l>dist_fl)?dist_l:dist_fl;
dist_max_rs = (dist_r>dist_fr)?dist_r:dist_fr;
dist_max_fs = (dist_fr>dist_fl)?dist_fr:dist_fl;
if (dist_f>dist_max_fs) dist_max_fs = dist_f;
uint8_t emergency = 0;
int16_t speed_l = 0;
int16_t speed_r = 0;
int16_t speed = 0;
uint8_t ledr = 0x00;
uint8_t ledg = 0x00;
uint16_t sum = dist_r + dist_fr
+ dist_f + dist_fl
+ dist_l;
sum /= 5;
uint8_t dmax = (sum<0xe0)?(sum+0x20):0xff;
uint8_t dmin = (sum>0x20)?(sum-0x20):0x00;
ledr=0;
ledg=0;
stress = (stress>10)?(stress-10):(0);
stress += (copro_current_l>20)?(copro_current_l-5):0;
stress += (copro_current_r>20)?(copro_current_r-5):0;
uint8_t ori_corr = (32+ori-ori_opt)%32;
//gfx_set_proportional(0);
//gfx_move(100, 40);
//print_hex(ori_corr);
uint8_t proposed_mode = mode;
static uint8_t candidate_mode = 0;
static uint8_t candidate_cnt = 0;
switch (mode) {
case MODE_STOP:
if (check_le(dist_l, 0, DIST_ALL))
proposed_mode = MODE_ROTATE_LEFT;
if (check_le(dist_r, 0, DIST_ALL))
proposed_mode = MODE_ROTATE_RIGHT;
if (check_le(dist_fl, 0, DIST_ALL))
proposed_mode = MODE_TURN_LEFT;
if (check_le(dist_fr, 0, DIST_ALL))
proposed_mode = MODE_TURN_RIGHT;
if (check_le(dist_f, 0, DIST_ALL))
proposed_mode = MODE_STRAIGHTON;
break;
case MODE_STRAIGHTON:
if (check_le(dist_fl, 0, DIST_FL|DIST_F|DIST_FR))
proposed_mode = MODE_TURN_LEFT;
if (check_le(dist_fr, 0, DIST_FL|DIST_F|DIST_FR))
proposed_mode = MODE_TURN_RIGHT;
if (check_le(dist_f, -2, DIST_FL|DIST_F|DIST_FR))
proposed_mode = MODE_STRAIGHTON;
if ((3<=ori_corr) && (ori_corr<=15))
proposed_mode = MODE_TURN_LEFT;
if ((29>=ori_corr) && (ori_corr>=17))
proposed_mode = MODE_TURN_RIGHT;
if (!check_le(dist_max_fs, -40, DIST_ALL)) {
if (dist_max_rs>dist_max_ls)
proposed_mode = MODE_ROTATE_LEFT;
else if (dist_max_ls>dist_max_rs)
proposed_mode = MODE_ROTATE_RIGHT;
else
proposed_mode = MODE_STOP;
}
if (dist_max_fs>0xa0)
proposed_mode = MODE_BACK;
break;
case MODE_TURN_LEFT:
if ((ori_corr>29) || (ori_corr<3))
proposed_mode = MODE_STRAIGHTON;
if (check_le(dist_l, 0, DIST_L|DIST_FL|DIST_F))
proposed_mode = MODE_ROTATE_LEFT;
if (check_le(dist_fl, -3, DIST_L|DIST_FL|DIST_F))
proposed_mode = MODE_TURN_LEFT;
if (check_le(dist_f, 0, DIST_L|DIST_FL|DIST_F))
proposed_mode = MODE_STRAIGHTON;
//if (!check_le(dist_max_fs, -40, DIST_ALL))
// proposed_mode = MODE_STOP;
if (dist_max_fs>0xa0)
proposed_mode = MODE_BACK;
break;
case MODE_TURN_RIGHT:
if ((ori_corr>29) || (ori_corr<3))
proposed_mode = MODE_STRAIGHTON;
if (check_le(dist_r, 0, DIST_R|DIST_FR|DIST_F))
proposed_mode = MODE_ROTATE_RIGHT;
if (check_le(dist_fr, -3, DIST_R|DIST_FR|DIST_F))
proposed_mode = MODE_TURN_RIGHT;
if (check_le(dist_f, 0, DIST_R|DIST_FR|DIST_F))
proposed_mode = MODE_STRAIGHTON;
//if (!check_le(dist_max_fs, -40, DIST_ALL))
// proposed_mode = MODE_STOP;
if (dist_max_fs>0xa0)
proposed_mode = MODE_BACK;
break;
case MODE_ROTATE_LEFT:
if (check_le(dist_l, 0, DIST_L|DIST_FL|DIST_F))
proposed_mode = MODE_ROTATE_LEFT;
if (check_le(dist_fl, -1, DIST_L|DIST_FL|DIST_F))
proposed_mode = MODE_TURN_LEFT;
if (check_le(dist_f, -1, DIST_L|DIST_FL|DIST_F))
proposed_mode = MODE_TURN_LEFT;
if (!check_le(dist_max_ls, -40, DIST_ALL))
proposed_mode = MODE_BACK;
break;
case MODE_ROTATE_RIGHT:
if (check_le(dist_r, 0, DIST_R|DIST_FR|DIST_F))
proposed_mode = MODE_ROTATE_RIGHT;
if (check_le(dist_fr, -1, DIST_R|DIST_FR|DIST_F))
proposed_mode = MODE_TURN_RIGHT;
if (check_le(dist_f, -1, DIST_R|DIST_FR|DIST_F))
proposed_mode = MODE_TURN_RIGHT;
if (!check_le(dist_max_rs, -40, DIST_ALL))
proposed_mode = MODE_BACK;
break;
case MODE_BACK:
proposed_mode = MODE_STOP;
break;
}
if (mode_time<255) {
mode_time++;
if (mode_time>=minmodetime[mode]) {
mode_time=255;
}
}
if (mode_time==255) {
if (mode!=proposed_mode) {
if (proposed_mode==candidate_mode) {
candidate_cnt++;
if (candidate_cnt>5) {
mode = proposed_mode;
mode_time = 0;
}
} else {
candidate_cnt = 0;
candidate_mode = proposed_mode;
}
}
}
if (stress>200) {
if (mode != MODE_BACK) {
mode = MODE_BACK;
distances[(32-3+ori)%32]=255;
distances[(32-2+ori)%32]=255;
distances[(32-1+ori)%32]=255;
distances[(32+0+ori)%32]=255;
distances[(32+1+ori)%32]=255;
distances[(32+2+ori)%32]=255;
distances[(32+3+ori)%32]=255;
} else {
mode = MODE_STOP;
distances[(16-3+ori)%32]=255;
distances[(16-2+ori)%32]=255;
distances[(16-1+ori)%32]=255;
distances[(16+0+ori)%32]=255;
distances[(16+1+ori)%32]=255;
distances[(16+2+ori)%32]=255;
distances[(16+3+ori)%32]=255;
}
emergency = 1;
stress = 0;
}
if (mode!=last_mode) {
mode_cnt+=2;
if (mode_cnt>80) {
mode = MODE_BACK;
emergency = 1;
}
last_mode=mode;
} else {
if (mode_cnt>0) {
mode_cnt--;
}
}
//gfx_move(100, 0);
//print_hex(mode);
uint8_t turn_factor;
switch (mode) {
case MODE_STOP:
speed_l=0, speed_r=0, speed=0, ledg=0x00, ledr=0x84;
turn_factor = 0;
emergency = 1;
break;
case MODE_STRAIGHTON:
headlight_target=10;
// speed_l=100, speed_r=100, speed=50, ledg= 0x30;
speed_l=100, speed_r=100, speed=80, ledg= 0x30;
turn_factor = 60;
break;
case MODE_TURN_LEFT:
headlight_target=10;
// speed_l=70, speed_r=100, speed=40, ledg= 0x08;
speed_l=70, speed_r=100, speed=60, ledg= 0x08;
turn_factor = 60;
break;
case MODE_TURN_RIGHT:
headlight_target=10;
// speed_l=100, speed_r=70, speed=40, ledg= 0x40;
speed_l=100, speed_r=70, speed=60, ledg= 0x40;
turn_factor = 60;
break;
case MODE_ROTATE_LEFT:
headlight_target=0;
speed_l=-100, speed_r=100, speed=20, ledg= 0x04;
turn_factor = 30;
break;
case MODE_ROTATE_RIGHT:
headlight_target=0;
speed_l=100, speed_r=-100, speed=20, ledg= 0x80;
turn_factor = 30;
break;
case MODE_BACK:
headlight_target=0;
speed_l=-100, speed_r=-100, speed=20, ledg= 0x03, ledr=0x00;
turn_factor = 20;
break;
}
IO_LEDS_RED_PORT = ledr;
IO_LEDS_GREEN_PORT = ledg;
if (headlight_act<headlight_target)
headlight_act++;
if (headlight_act>headlight_target)
headlight_act--;
if (headlight_act)
leds_set_headlights(1<<(headlight_act-1));
else
leds_set_headlights(0);
if (mode==MODE_STRAIGHTON) {
int16_t lval = 2*dist_fl+dist_l;
int16_t rval = 2*dist_fr+dist_r;
int16_t delta = lval-rval;
if (abs(delta)>5) {
if (lval>rval) speed_r-=10; // rechts mehr Platz
if (rval>lval) speed_l-=10; // links mehr Platz
} else {
if (lval>rval) speed_r-=5; // rechts mehr Platz
if (rval>lval) speed_l-=5; // links mehr Platz
}
} else if (mode==MODE_BACK) {
uint16_t lval = dist_fl+2*dist_l;
uint16_t rval = dist_fr+2*dist_r;
if (lval>rval) speed_r-=15; // rechts mehr Platz
if (rval>lval) speed_l-=15; // links mehr Platz
} else {
uint16_t lval = dist_fl+dist_l;
uint16_t rval = dist_fr+dist_r;
if (lval>rval) speed_r-=5; // rechts mehr Platz
if (rval>lval) speed_l-=5; // links mehr Platz
}
if (ori_corr == 0) {
// BESTER WEG LIEGT GERADEAUS
} else if (ori_corr < 16) {
// BESTER WEG LIEGT LINKS
// ori_corr [1 ... 8]
speed_l -= (turn_factor * (uint16_t)ori_corr)/8;
speed_r += (turn_factor * (uint16_t)ori_corr)/8;
speed = (speed * (16 - ori_corr))/16;
} else {
// BESTER WEG LIEGT RECHTS
// ori_corr [24 ...31]
ori_corr = 32-ori_corr;
speed_l += (turn_factor * (uint16_t)ori_corr)/8;
speed_r -= (turn_factor * (uint16_t)ori_corr)/8;
speed = (speed * (16 - ori_corr))/16;
}
speed_l = (speed_l*speed)/128;
speed_r = (speed_r*speed)/128;
#if 0
if (abyss_counter<500) { // 5 sec
if ((acquisition_getFloorState(AN_FLOOR_L)==FLOOR_LINE) ||
(acquisition_getFloorState(AN_FLOOR_R)==FLOOR_LINE) ||
(acquisition_getFloorState(AN_LINE_L)==FLOOR_LINE) ||
(acquisition_getFloorState(AN_LINE_R)==FLOOR_LINE) ){
// still in area
abyss_counter=0;
} else {
abyss_counter++;
}
} else {
if ((acquisition_getDiff(AN_FLOOR_L)<0x02)||(acquisition_getDiff(AN_FLOOR_R)<0x02)) {
speed_l=0;
speed_r=0;
emergency = 1;
}
// if ((acquisition_getFloorState(AN_FLOOR_R)!=FLOOR_ABYSS) || (acquisition_getFloorState(AN_FLOOR_L)!=FLOOR_ABYSS)) {
// speed_l=0;
// speed_r=0;
// emergency = 1;
// }
if (line_detect_counter_l) {
line_detect_counter_l--;
} else if (acquisition_getFloorState(AN_FLOOR_L)!=FLOOR_ABYSS) {
line_detect_counter_l = 300;
if (line_detect_counter_r==0) {
line_detect_pos_l = copro_ticks_l;
line_detect_pos_r = copro_ticks_r;
}
}
if (line_detect_counter_r) {
line_detect_counter_r--;
} else if (acquisition_getFloorState(AN_FLOOR_R)!=FLOOR_ABYSS) {
line_detect_counter_r = 300;
if (line_detect_counter_l==0) {
line_detect_pos_l = copro_ticks_l;
line_detect_pos_r = copro_ticks_r;
}
}
if (line_detect_counter_l && line_detect_counter_r) {
speed_l=0;
speed_r=0;
emergency = 1;
line_detect_dir = line_detect_counter_l-line_detect_counter_r;
line_detect_pos_l = copro_ticks_l-line_detect_pos_l;
line_detect_pos_r = copro_ticks_r-line_detect_pos_r;
return BEHAVIOUR_LINE;
}
}
#endif
if (((tspeed_l<0) && (speed_l>=0)) ||
((tspeed_l>0) && (speed_l<=0)) ||
((tspeed_r<0) && (speed_r>=0)) ||
((tspeed_r>0) && (speed_r<=0)) ||
emergency) {
tspeed_l = 0;
tspeed_r = 0;
copro_stop();
} else {
tspeed_l = (3*tspeed_l+speed_l)/4;
tspeed_r = (3*tspeed_r+speed_r)/4;
if ((speed_l>0)&&(speed_l<tspeed_l)) {
tspeed_l=speed_l;
}
if ((speed_r>0)&&(speed_r<tspeed_r)) {
tspeed_r=speed_r;
}
copro_setSpeed(tspeed_l, tspeed_r);
}
return BEHAVIOUR_OBSTACLE;
}
