void* keeperMotionThread(void* params)
{
void ** paramsList = (void**)params;
float &targetDist = *((float*) paramsList[0]);
float &moveDist = *((float*) paramsList[1]);
int &v_level= *((int*) paramsList[2]);
Robot &robot= *((Robot*) paramsList[3]);
bool &kmt_abort = *((bool*) paramsList[4]);
bool &rotating = *((bool*) paramsList[5]);
while(!robot.abort)
{
moveDist += v_level*DELTA_V*DELTA_T/float(1e6);
int acc = getAcc(v_level,targetDist-moveDist);
v_level += acc;
cv::Point2f tar_dir(robot.ownGoal_frontDir.y,-robot.ownGoal_frontDir.x);
if(v_level==0 && acc==0)
{
cv::Point2f ori_dir(sin(robot.ori),cos(robot.ori));
if(acos(ori_dir.dot(tar_dir))>ORI_TOL*M_PI/180)
{
rotating=true;
robot.rotateTo(tar_dir);
rotating=false;
}
}
sendAA(v_level*DELTA_V,v_level*DELTA_V);
//printf("dist = %f\n",targetDist-moveDist);
usleep(DELTA_T);
}
while(v_level)
{
v_level += v_level>0?-1:1;
sendAA(v_level*DELTA_V,v_level*DELTA_V);
//printf("dist = %f\n",targetDist-moveDist);
usleep(DELTA_T);
}
kmt_abort = true;
}
static void motor_thread_main()
{
motor_init();
while (1) {
pthread_mutex_lock(&task_mutex);
if (mutex_flag == 1) {
// sendSTOP();
// mylogfd (-1, "%s %d\n", "----", task_segNum);
if (task_segList != NULL) {
free(task_segList);
task_segList = NULL;
}
sliceTask(current);
mylogfd (1, "%s %d\n", "New Task", task_segNum);
mutex_flag = 0;
}
pthread_mutex_unlock(&task_mutex);
//mylogfd (1, "[moto] %d %d\n", task_segIdx, task_segNum);
if (task_segIdx >= task_segNum) {
mylogfd(MOTOFD,"[moto]main:job done\n");
pthread_mutex_lock(&task_mutex);
job_done = 1;
job_over = 1;
mylogfd (1, "[moto] job done\n");
pthread_mutex_unlock(&task_mutex);
// sendSTOP();
//sendAA(preleft,preright,0);
pthread_mutex_lock(&stop_mutex);
} else {
//mylogfd(1,"x");
if (task_segIdx == 0) {
if (((fabs(preleft - task_segList[task_segIdx].lspeed) > MINV)
||(fabs(preright - task_segList[task_segIdx].rspeed) > MINV))
&&(fabs(preleft > 10))) {
preleft = preleft/2;
preright = preright/2;
sendAA(preleft,preright,0);
usleep(1000000 * 0.1);
continue;
}
}
//mylogfd(-1,"[Control]/motor/main:deal with the task_seg\n");
if (task_segList[task_segIdx].type == TASK_ROTA) {
if (task_segList[task_segIdx].dur_time == 0) {
mylogfd(1,"time = 0\n");
task_segIdx ++;
continue;
}
sendA(task_segList[task_segIdx].lspeed,
task_segList[task_segIdx].rspeed,0,1);
usleep(1000000 * task_segList[task_segIdx].dur_time);
} else if (task_segList[task_segIdx].type == TASK_STOP) {
sendSTOP();
} else if (task_segList[task_segIdx].type == TASK_WITH_BALL) {
sendAA(task_segList[task_segIdx].lspeed,
task_segList[task_segIdx].rspeed,0);
usleep(1000000 * task_segList[task_segIdx].dur_time);
} else if (task_segList[task_segIdx].type == TASK_RUN) {
sendAA(task_segList[task_segIdx].lspeed+left_flag,
task_segList[task_segIdx].rspeed+right_flag,0);
usleep(1000000 * task_segList[task_segIdx].dur_time);
} else if (task_segList[task_segIdx].type == TASK_CATCH_BALL) {
sendAA(task_segList[task_segIdx].lspeed,
task_segList[task_segIdx].rspeed,0);
usleep(1000000 * task_segList[task_segIdx].dur_time);
}
preleft = task_segList[task_segIdx].lspeed;
preright = task_segList[task_segIdx].rspeed;
task_segIdx++;
}
}
}
