/* Attempt to pick up specified extension with context */
static int pickup_by_exten(struct ast_channel *chan, char *exten, char *context)
{
int res = -1;
struct ast_channel *target = NULL;
while ((target = ast_channel_walk_locked(target))) {
if ((!strcasecmp(target->macroexten, exten) || !strcasecmp(target->exten, exten)) &&
!strcasecmp(target->dialcontext, context) &&
(chan != target) && can_pickup(target)) {
res = pickup_do(chan, target);
ast_channel_unlock(target);
break;
}
ast_channel_unlock(target);
}
return res;
}
/* Attempt to pick up specified mark */
static int pickup_by_mark(struct ast_channel *chan, char *mark)
{
int res = -1;
const char *tmp = NULL;
struct ast_channel *target = NULL;
while ((target = ast_channel_walk_locked(target))) {
if ((tmp = pbx_builtin_getvar_helper(target, PICKUPMARK)) &&
!strcasecmp(tmp, mark) &&
can_pickup(target)) {
res = pickup_do(chan, target);
ast_channel_unlock(target);
break;
}
ast_channel_unlock(target);
}
return res;
}
/** Update the current poses of the robot
*/
static void conscious_thought(void) {
char *scmd;
int execute;
struct timeval currtime;
// get a speech command, and place that as
// the priority task
/*if ((scmd = get_speech_command())) {
if (strstr(scmd, "fetch")) {
task = "fetch";
} else if (strstr(scmd, "stop")) {
task = "stop";
}
}*/
// FSM (taskgraph)
execute = 0;
printf("Updating objects...\n");
update_object_positions();
gettimeofday(&currtime, NULL);
printf("Object positions:\n");
printf("<ball>\n");
for (int n = 0; n < (int)ballpos.size(); n++) {
printf("Ball: %f %f %f\n", ballpos[n].x, ballpos[n].y, ballpos[n].z);
}
printf("</ball>\n");
printf("<basket>\n");
for (int n = 0; n < (int)basketpos.size(); n++) {
printf("Basket: %f %f %f\n", basketpos[n].x, basketpos[n].y, basketpos[n].z);
}
printf("</basket>\n");
printf("Updating states...\n");
while (!execute) {
if (strcmp(task, "fetch") == 0) {
// transitions
// TODO: create a counter resolution for infinite loops
// TODO: pick up more than 1 ball
switch (subtask) {
case S_IDLE:
printf("State: IDLE\n");
if (num_balls_in_basket() == 0) {
subtask = S_FINDBALL;
} else {
subtask = S_FINDBASKET;
// execute = 1;
}
break;
case S_FINDBALL:
printf("State: FIND BALL\n");
if (num_balls_in_basket() != 0) {
subtask = S_IDLE;
} else if (!ballfound()) {
execute = 1;
} else {
subtask = S_GOTOBALL;
}
break;
case S_GOTOBALL:
printf("State: GOTO BALL\n");
if (num_balls_in_basket() != 0) {
subtask = S_IDLE;
} else if (!ballfound()) {
subtask = S_FINDBALL;
} else if (can_pickup(closest_object(ballpos))) {
subtask = S_PICKBALL;
} else {
execute = 1;
}
break;
case S_PICKBALL:
printf("State: PICK BALL\n");
if (num_balls_in_basket() != 0) {
subtask = S_IDLE;
} else if (!ballfound()) {
subtask = S_FINDBALL;
} else if (!can_pickup(closest_object(ballpos))) {
subtask = S_GOTOBALL;
} else {
execute = 1;
}
break;
case S_FINDBASKET:
printf("State: FIND BASKET\n");
if (!basketfound()) {
execute = 1;
} else {
subtask = S_GOTOBASKET;
}
break;
case S_GOTOBASKET:
printf("State: GOTO BASKET\n");
if (!basketfound()) {
subtask = S_FINDBASKET;
} else if (can_drop(closest_object(basketpos))) {
// have to do a strange thing here with timers
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE1;
} else {
execute = 1;
}
break;
case S_DROPBASKET_PHASE1:
printf("State: DROP BASKET P1\n");
if (difftime(currtime, actiontime) < 1.2) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE2;
}
break;
case S_DROPBASKET_PHASE2:
printf("State: DROP BASKET P2\n");
if (difftime(currtime, actiontime) < 0.5) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE3;
}
break;
case S_DROPBASKET_PHASE3:
printf("State: DROP BASKET P3\n");
if (difftime(currtime, actiontime) < 1.0) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE4;
}
break;
case S_DROPBASKET_PHASE4:
printf("State: DROP BASKET P4\n");
if (difftime(currtime, actiontime) < 1.0) {
execute = 1;
} else {
gettimeofday(&actiontime, NULL);
subtask = S_DROPBASKET_PHASE5;
}
break;
case S_DROPBASKET_PHASE5:
printf("State: DROP BASKET P5\n");
if (difftime(currtime, actiontime) < 1.2) {
execute = 1;
} else {
subtask = S_IDLE;
}
break;
default:
subtask = S_IDLE;
}
} else if (strcmp(task, "stop") == 0) {
subtask = S_IDLE;
printf("State: IDLE\n");
execute = 1;
}
}
printf("Executing...\n");
// move accordingly to the state
// TODO: do collision detection, stop the robot from moving
switch (subtask) {
case S_IDLE:
set_robot(0.0, 0.0, 0.0, 0.0);
break;
case S_FINDBALL:
set_robot(0.0, 0.30 * find_coeff, 0.0, 0.0);
break;
case S_GOTOBALL:
set_robot(0.32, -ballpos[0].x / 1000.0, -1.0, 0.0);
find_coeff = dsign(-ballpos[0].x);
break;
case S_PICKBALL:
set_robot(0.30, -ballpos[0].x / 1000.0, -1.0, 1.0);
find_coeff = dsign(-ballpos[0].x);
break;
case S_FINDBASKET:
set_robot(0.0, -0.30 * find_coeff, 0.0, 0.0);
break;
case S_GOTOBASKET:
set_robot(0.32, -basketpos[0].x / 1200.0, 0.0, 0.0);
find_coeff = dsign(-basketpos[0].x);
break;
case S_DROPBASKET_PHASE1:
set_robot(0.0, 0.0, 1.0, 0.0);
break;
case S_DROPBASKET_PHASE2:
set_robot(0.40, -basketpos[0].x / 1200.0, 0.0, 0.0);
find_coeff = dsign(-basketpos[0].x);
break;
case S_DROPBASKET_PHASE3:
set_robot(0.0, 0.0, 0.0, -1.0);
break;
case S_DROPBASKET_PHASE4:
set_robot(-0.40, 0.0, 0.0, 0.0);
break;
case S_DROPBASKET_PHASE5:
set_robot(0.0, 0.0, -1.0, 0.0);
break;
default:
set_robot(0.0, 0.0, 0.0, 0.0);
}
}
