void Motion_controller::move(const uint16_t &mm,const uint8_t &dir) { uint16_t segment = 1500; Message msg; uint16_t dis = 0; uint8_t count = mm / segment; uint8_t count_msg = 0; if(mm < CAR_MOVEMENT_THRESHOLD) { if(glo_debug_msg) printf("Motion_controller::move() -> the distance is too small\n"); return ; } switch(dir) { case CAR_FORWARD://forward if(mm > segment) { for(count_msg = 0;count_msg < count;count_msg++) { msg.CarMoveUpMM(segment); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } dis = mm - count * segment; msg.CarMoveUpMM(dis); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } else { msg.CarMoveUpMM(mm); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } break; case CAR_BACKWARD://backward if(mm > segment) { for(count_msg = 0;count_msg < count;count_msg++) { msg.CarMoveDownMM(segment); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } dis = mm - count * segment; msg.CarMoveDownMM(dis); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } else { msg.CarMoveDownMM(mm); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } break; case CAR_LEFT://left if(mm > segment) { for(count_msg = 0;count_msg < count;count_msg++) { msg.CarMoveLeftMM(segment); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } dis = mm - count * segment; msg.CarMoveLeftMM(dis); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } else { msg.CarMoveLeftMM(mm); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } break; case CAR_RIGHT://right if(mm > segment) { for(count_msg = 0;count_msg < count;count_msg++) { msg.CarMoveRightMM(segment); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } dis = mm - count * segment; msg.CarMoveRightMM(dis); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } else { msg.CarMoveRightMM(mm); if(glo_motion_enable) msg.safe_sendMessage(this->Com->fd); } break; default: break; } return ; }