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++; } } }