int main(void)
{
SystemInit();
ControllerInit();
vTaskStartScheduler();
while(1);
}
int
main(void)
{
//initialise the graphics
GraphicsInit();
//initialise eventedness
EventInit();
//controller initialisation
ControllerInit();
TCCR1B |= (1 << WGM12);
TIMSK1 = (1 << OCIE1A);
sei();
OCR1A = 200;
TCCR1B |= (1 << CS10);
//initialise the users game code
setup();
//This is the event loop. It should be always spinning.
while(1) {
loop();
UpdateBtnState();
}
}
int main(int argc, char **argv) {
ControllerInit(argc, argv);
/*
Glut's initialization code. Set window's size and type of display.
Window size is put half the 800x600 resolution as defined by above
constants. The windows is positioned at the top leftmost area of
the screen.
*/
glutInit( &argc, argv );
glutInitDisplayMode( GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGBA | GLUT_MULTISAMPLE );
glutInitWindowPosition( GL_WIN_INITIAL_X, GL_WIN_INITIAL_Y );
glutInitWindowSize( GL_WIN_INITIAL_WIDTH, GL_WIN_INITIAL_HEIGHT );
int winNr= glutCreateWindow("OmniView");
//glutFullScreen();
/*
The function below are called when the respective event
is triggered.
*/
glutReshapeFunc(glutResize); // called every time the screen is resized
glutMouseFunc(glutMouse); // called when the application receives a input from the mouse
glutMotionFunc(glutMotion); // called when the mouse moves over the screen with one of this button pressed
glutPassiveMotionFunc(glutMotion); // callback for a window is called when the mouse
//moves within the window while no mouse buttons are pressed.
// glutSpecialFunc(glutSpecial); // called when a special key is pressed like SHIFT
glutDisplayFunc(display); // called when window needs to be redisplayed
glutKeyboardFunc(glutKeyboard); // called when the application receives a input from the keyboard
/*
the callback function: load the frame and detect the faces and generate the view
*/
//glutIdleFunc(cvcallback);
// glsceneinit();
glutMainLoop();
return 0;
}
int main(){
//class
KinectMangerThread kinectManager;
RobotArm arm;
InvalidMotionHandler motionHandler;
ColorBasedTracker tracker;
//variable
cv::Rect RobotROI((KINECT_DEPTH_WIDTH - 160) / 2 + 40, (KINECT_DEPTH_HEIGHT- 160) / 2, 160, 160);
bool saveCheck = false;
int sampleAngleBox[9], count = 0;
const int sampleAngleLimit[9] = {10000, 10000, 10000, 10000, 10000, 10000, 400, 400, 400};
char dirName[256];
float averBox[NUM_XEL] = {0};
//uniform random sampler
const int range_from = 0;
const int range_to = 10;
std::random_device rand_dev;
std::mt19937 generator(rand_dev());
std::uniform_int_distribution<int> distr_arm(-5000, 5000);
std::uniform_int_distribution<int> distr_finger(-200, 200);
//initialize
kinectManager.Initialize(RobotROI);
motionHandler.Initialize();
int presentSecond = time(NULL);
#ifdef RIGHT_ARM_USE
dxl_write_dword(arm.DXL_Get_Port(), 7, NX::P_HOMING_OFFSET_LL, 62750, 0);
#elif defined LEFT_ARM_USE
dxl_write_dword(arm.DXL_Get_Port(), 8, NX::P_HOMING_OFFSET_LL, -62750, 0);
#endif
ControllerInit(&arm);
if(!motionHandler.robotConnect(&arm)){
printf("Robot can not connect.\n");
motionHandler.Deinitialize();
return -1;
}
arm.TorqueOff();
printf("If u want to start program, press any key.\n");
getch();
//디렉토리 생성
itoa(presentSecond, dirName, 10);
CreateRGBDdir(dirName);
//배경취득
cv::Mat backRGB = kinectManager.getImg();
cv::Mat backDepth = kinectManager.getDepth();
cv::imshow("background", backRGB);
cv::waitKey(1);
char buf[256];
strcpy(buf, DEFAULT_PATH);
strcat(buf, dirName);
sprintf(buf, "%s\\%s", DEFAULT_PATH, dirName);
writeDepthData(backDepth, buf, "backDepth");
strcat(buf, "\\backRGB.bmp");
cv::imwrite(buf, backRGB);
tracker.InsertBackGround(backRGB, backDepth);
printf("\nIf u save background, press any key\n");
getch();
arm.TorqueOn();
while(!kinectManager.isThreadDead()){
//샘플링된 모션이 가능한 모션인지를 체크
int getAngle[9], tmpAngle[9], completePosition[9];
//motionHandler.ForwardEnd(&arm);
printf("Sampling start....");
while(1){
arm.GetPresPosition(getAngle);
//angle sampling - limit 이내의 각도 샘플링
memcpy(tmpAngle, getAngle, sizeof(int) * 9);
for(int i = 0; i < NUM_XEL; i++){
if(i < NUM_JOINT) sampleAngleBox[i] = distr_arm(generator);
else sampleAngleBox[i] = distr_finger(generator);
tmpAngle[i] += sampleAngleBox[i];
}
//motion check
if(motionHandler.InvalidCheck(tmpAngle, getAngle))
break;
}
printf("complete!\n");
printf("sampling result : \n");
for(int i = 0; i < NUM_XEL; i++) printf("%d ", sampleAngleBox[i]);
printf("\n");
//실제 움직임
printf("Move Robot.....");
arm.SetGoalPosition(tmpAngle);
WaitUntilMoveEnd(&arm);
arm.GetPresPosition(completePosition);
printf("complete!\n");
//write file
cv::Mat img = kinectManager.getImg();
cv::Mat depth = kinectManager.getDepth();
cv::Mat pointCloud = kinectManager.getPointCloud();
cv::imshow("cropImg", img);
cv::waitKey(1);
if(writeData(img, depth, pointCloud, &tracker, completePosition, dirName, count)){
count++;
printf("[%d] data saveComplete.\n", count);
printf("average : ");
for(int i = 0; i < NUM_XEL; i++){
averBox[i] = (averBox[i] * (count - 1) + sampleAngleBox[i]) / (float)count;
printf("%.1f ", averBox[i]);
}
printf("\n");
}else{
printf("Move previous position.....");
arm.SetGoalPosition(getAngle);
WaitUntilMoveEnd(&arm);
printf("complete!\n");
}
}
//Deallocation
cv::destroyAllWindows();
motionHandler.Deinitialize();
kinectManager.Deinitialize();
printf("Go End position.....");
int Initpos[] = {0, 0, 0, 0, 0, 0, 2622, 1534, 2100};
arm.SetGoalPosition(Initpos);
WaitUntilMoveEnd(&arm);
printf("complete!\n");
arm.TorqueOff();
return 0;
}
