// Initializes the servos, gets the motion path // gets access to Motion Manger etc. bool Initialize() { minIni* ini = new minIni(INI_FILE_PATH); // check if on dev 0 // Motion manager initialize queries all servos if (MotionManager::GetInstance()->Initialize(&cm730) == false) { // if not try dev 1 linux_cm730.SetPortName(U2D_DEV_NAME1); if (MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("\n The Framework has encountered a connection error to the subcontroller!\n"); printf("Failed to initialize Motion Manager! Meaning the program is unable to communicate with Subcontroller.\n"); printf("Please check if subcontroller serial port is recognized in /dev/ as TTYUSB0 or TTYUSB1, or if port is currently in use by another program.\n"); return false; } } // load config ini for walking gait params Walking::GetInstance()->LoadINISettings(ini); usleep(100); // load config ini offset values Maybe to comment out if not used MotionManager::GetInstance()->LoadINISettings(ini); // instantiates each motion module to motion manager MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); // MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); Walking::GetInstance()->m_Joint.SetEnableBody(false); Action::GetInstance()->m_Joint.SetEnableBody(true); // instatiate sys timer and start linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Start(); // load motion binary to action module Action::GetInstance()->LoadFile(MOTION_FILE_PATH); return true; }
void Motion::StartEngines() { printf("===== Action script for DARwIn =====\n\n"); ChangeCurrentDir(); minIni* ini = new minIni(INI_FILE_PATH); Action::GetInstance()->LoadFile(MOTION_FILE_PATH); //////////////////// Framework Initialize //////////////////////////// if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false) { linux_arbotixpro.SetPortName(U2D_DEV_NAME1); if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false) { printf("Fail to initialize Motion Manager!\n"); } } Walking::GetInstance()->LoadINISettings(ini); usleep(100); MotionManager::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule( (MotionModule*) Action::GetInstance()); MotionManager::GetInstance()->AddModule( (MotionModule*) Head::GetInstance()); MotionManager::GetInstance()->AddModule( (MotionModule*) Walking::GetInstance()); linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Start(); ///////////////////////////////////////////////////////////////////// Walking::GetInstance()->m_Joint.SetEnableBody(false); Head::GetInstance()->m_Joint.SetEnableBody(false); Action::GetInstance()->m_Joint.SetEnableBody(true); MotionManager::GetInstance()->SetEnable(true); MotionManager::GetInstance()->ResetGyroCalibration(); Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); }
int main(int argc, char *argv[]) { signal(SIGABRT, &sighandler); signal(SIGTERM, &sighandler); signal(SIGQUIT, &sighandler); signal(SIGINT, &sighandler); ros::init(argc,argv, "WALK_TUNER"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("motiondirector", 1000, chatterCallback); change_current_dir(); LinuxArbotixPro linux_arbotixpro("/dev/ttyUSB0"); ArbotixPro arbotixpro(&linux_arbotixpro); minIni* ini; if (argc == 2) ini = new minIni(argv[1]); else ini = new minIni(INI_FILE_PATH); mjpg_streamer* streamer = new mjpg_streamer(0, 0); httpd::ini = ini; //////////////////// Framework Initialize //////////////////////////// if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } Walking::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); LinuxMotionTimer linuxMotionTimer; linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Start(); ///////////////////////////////////////////////////////////////////// DrawIntro(&arbotixpro); MotionManager::GetInstance()->SetEnable(true); MotionManager::GetInstance()->ResetGyroCalibration(); MotionManager::GetInstance()->Process(); ros::spin(); exit(0); /* while(1) { if(MotionStatus::FALLEN != STANDUP && (Walking::GetInstance()->IsRunning() == true) ) { Walking::GetInstance()->Stop(); while(Walking::GetInstance()->IsRunning() == 1) usleep(8000); Action::GetInstance()->m_Joint.SetEnableBody(true, true); if(MotionStatus::FALLEN == FORWARD) Action::GetInstance()->Start(1); // FORWARD GETUP 10 else if(MotionStatus::FALLEN == BACKWARD) Action::GetInstance()->Start(2); // BACKWARD GETUP 11 while(Action::GetInstance()->IsRunning() == 1) usleep(8000); Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true); } } */ /* while (1) { int ch = _getch(); if (ch == 0x1b) { ch = _getch(); if (ch == 0x5b) { ch = _getch(); if (ch == 0x41) // Up arrow key MoveUpCursor(); else if (ch == 0x42) // Down arrow key MoveDownCursor(); else if (ch == 0x44) // Left arrow key MoveLeftCursor(); else if (ch == 0x43) MoveRightCursor(); } } else if ( ch == '[' ) DecreaseValue(false); else if ( ch == ']' ) IncreaseValue(false); else if ( ch == '{' ) DecreaseValue(true); else if ( ch == '}' ) IncreaseValue(true); else if ( ch >= 'A' && ch <= 'z' ) { char input[128] = {0,}; char *token; int input_len; char cmd[80]; char strParam[20][30]; int num_param; int idx = 0; BeginCommandMode(); printf("%c", ch); input[idx++] = (char)ch; while (1) { ch = _getch(); if ( ch == 0x0A ) break; else if ( ch == 0x7F ) { if (idx > 0) { ch = 0x08; printf("%c", ch); ch = ' '; printf("%c", ch); ch = 0x08; printf("%c", ch); input[--idx] = 0; } } else if ( ch >= 'A' && ch <= 'z' ) { if (idx < 127) { printf("%c", ch); input[idx++] = (char)ch; } } } fflush(stdin); input_len = strlen(input); if (input_len > 0) { token = strtok( input, " " ); if (token != 0) { strcpy( cmd, token ); token = strtok( 0, " " ); num_param = 0; while (token != 0) { strcpy(strParam[num_param++], token); token = strtok( 0, " " ); } if (strcmp(cmd, "exit") == 0) { if (AskSave() == false) break; } if (strcmp(cmd, "re") == 0) DrawScreen(); else if (strcmp(cmd, "save") == 0) { Walking::GetInstance()->SaveINISettings(ini); SaveCmd(); } else if (strcmp(cmd, "mon") == 0) { MonitorCmd(); } else if (strcmp(cmd, "help") == 0) HelpCmd(); else PrintCmd("Bad command! please input 'help'"); } } EndCommandMode(); } } DrawEnding();*/ // //ros::spin(); }
int main(void) { time_t rawtime; struct tm * timeinfo; time ( &rawtime ); timeinfo = localtime ( &rawtime ); stringstream p; p<<"[" << timeinfo->tm_hour<< ":"<< timeinfo->tm_min << ":" << timeinfo->tm_sec <<"]"; std::string logname = "/home/robot/Desktop/LOG"; logname+=p.str(); startLog(logFile,logname); for (int i=0; i<BrainBall.x_detail ;i++) { BrainBall.x_place[i] = 0; } printf( "\n===== Action script for DARwIn =====\n\n"); change_current_dir(); minIni* ini = new minIni(INI_FILE_PATH); Action::GetInstance()->LoadFile(MOTION_FILE_PATH); //////////////////// Framework Initialize //////////////////////////// if(MotionManager::GetInstance()->Initialize(&cm730) == false) { linux_cm730.SetPortName("/dev/ttyUSB1"); if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } } Walking::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); LinuxMotionTimer linuxMotionTimer; linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Start(); ///////////////////////////////////////////////////////////////////// Walking::GetInstance()->m_Joint.SetEnableBody(false); Head::GetInstance()->m_Joint.SetEnableBody(false); Action::GetInstance()->m_Joint.SetEnableBody(true); MotionManager::GetInstance()->SetEnable(true); Action::GetInstance()->Start(1); //Init(stand up) pose Action::GetInstance()->Brake(); while(Action::GetInstance()->IsRunning()) usleep(8*1000); cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL); MotionManager::GetInstance()->ResetGyroCalibration(); printf("Press the ENTER key to begin!\n"); getchar(); ////// writeToLog(logFile,"started"); printf("0-Play\n"); printf("1-Kick\n"); printf("2-stand\n"); printf("3-Straight walking\n"); printf("4-Turn right\n"); printf("5-Turn left\n"); printf("6-stop walking\n"); printf("7-reset\n"); printf("8-GetTilt\n"); printf("9-SetTilt\n"); printf("q-Exit\n"); Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); while(1) { char c =getchar(); if (c == '0') { play(); } else if (c == '1') { writeToLog(logFile,"kick"); Action::GetInstance()->Start(2); while(Action::GetInstance()->IsRunning()) usleep(8*1000); } else if(c == '2') { writeToLog(logFile,"stand"); Action::GetInstance()->Start(1); while(Action::GetInstance()->IsRunning()) usleep(8*1000); } else if(c == '3') { if(Walking::GetInstance()->IsRunning() == false ) { fprintf(stderr, "STARTING\n"); Action::GetInstance()->Start(9); while(Action::GetInstance()->IsRunning()) usleep(8*1000); //Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true,true); Walking::GetInstance()->X_MOVE_AMPLITUDE = 0; Walking::GetInstance()->Y_MOVE_AMPLITUDE = 0; Walking::GetInstance()->A_MOVE_AMPLITUDE = 0; Walking::GetInstance()->Start(); } else Walking::GetInstance()->A_MOVE_AMPLITUDE = 0; } else if(c == '4') { if(Walking::GetInstance()->IsRunning() == true ) { fprintf(stderr, "TURNING RIGHT\n"); Walking::GetInstance()->A_MOVE_AMPLITUDE = -5; } } else if(c == '5') { if(Walking::GetInstance()->IsRunning() == true ) { fprintf(stderr, "TURNING LEFT\n"); Walking::GetInstance()->A_MOVE_AMPLITUDE = 5; } } else if(c == '6') { if(Walking::GetInstance()->IsRunning() == true ) { fprintf(stderr, "STOPPING\n"); Walking::GetInstance()->Stop(); while(Walking::GetInstance()->IsRunning() == 1) usleep(8000); Walking::GetInstance()->m_Joint.SetEnableBody(false); Head::GetInstance()->m_Joint.SetEnableBody(false); Action::GetInstance()->m_Joint.SetEnableBody(true); MotionManager::GetInstance()->SetEnable(true); } } else if(c == '7') { for(int i=JointData::ID_R_SHOULDER_PITCH; i<JointData::NUMBER_OF_JOINTS; i++) Reset(&cm730, i); Reset(&cm730, CM730::ID_CM); } else if(c == '8') { float tilt; tilt = GetTilt(); printf("%f\n", tilt); } else if(c == '9') { float tilt,pan; cout<<"enter tilt\n"; cin>>tilt; cout<<"enter pan\n"; cin>>pan; SetTilt(pan,tilt); } else if(c == 'q')
int main(int argc, char *argv[]) { signal(SIGABRT, &sighandler); signal(SIGTERM, &sighandler); signal(SIGQUIT, &sighandler); signal(SIGINT, &sighandler); change_current_dir(); LinuxCM730 linux_cm730("/dev/ttyUSB0"); CM730 cm730(&linux_cm730); minIni* ini; if(argc==2) ini = new minIni(argv[1]); else ini = new minIni(INI_FILE_PATH); //////////////////// Framework Initialize //////////////////////////// if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } Walking::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); LinuxMotionTimer linuxMotionTimer; linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Start(); ///////////////////////////////////////////////////////////////////// DrawIntro(&cm730); MotionManager::GetInstance()->SetEnable(true); MotionManager::GetInstance()->ResetGyroCalibration(); while(1) { int ch = _getch(); if(ch == 0x1b) { ch = _getch(); if(ch == 0x5b) { ch = _getch(); if(ch == 0x41) // Up arrow key MoveUpCursor(); else if(ch == 0x42) // Down arrow key MoveDownCursor(); else if(ch == 0x44) // Left arrow key MoveLeftCursor(); else if(ch == 0x43) MoveRightCursor(); } } else if( ch == '[' ) DecreaseValue(false); else if( ch == ']' ) IncreaseValue(false); else if( ch == '{' ) DecreaseValue(true); else if( ch == '}' ) IncreaseValue(true); else if( ch >= 'A' && ch <= 'z' ) { char input[128] = {0,}; char *token; int input_len; char cmd[80]; char strParam[20][30]; int num_param; int idx = 0; BeginCommandMode(); printf("%c", ch); input[idx++] = (char)ch; while(1) { ch = _getch(); if( ch == 0x0A ) break; else if( ch == 0x7F ) { if(idx > 0) { ch = 0x08; printf("%c", ch); ch = ' '; printf("%c", ch); ch = 0x08; printf("%c", ch); input[--idx] = 0; } } else if( ch >= 'A' && ch <= 'z' ) { if(idx < 127) { printf("%c", ch); input[idx++] = (char)ch; } } } fflush(stdin); input_len = strlen(input); if(input_len > 0) { token = strtok( input, " " ); if(token != 0) { strcpy( cmd, token ); token = strtok( 0, " " ); num_param = 0; while(token != 0) { strcpy(strParam[num_param++], token); token = strtok( 0, " " ); } if(strcmp(cmd, "exit") == 0) { if(AskSave() == false) break; } if(strcmp(cmd, "re") == 0) DrawScreen(); else if(strcmp(cmd, "save") == 0) { Walking::GetInstance()->SaveINISettings(ini); SaveCmd(); } else if(strcmp(cmd, "mon") == 0) { MonitorCmd(); } else if(strcmp(cmd, "help") == 0) HelpCmd(); else PrintCmd("Bad command! please input 'help'"); } } EndCommandMode(); } } DrawEnding(); exit(0); }
int main(void) { //Register signal and signal handler signal(SIGINT, signal_callback_handler); printf( "\n===== Head tracking Tutorial for DARwIn =====\n\n"); change_current_dir(); minIni* ini = new minIni(INI_FILE_PATH); Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE); LinuxCamera::GetInstance()->Initialize(0); LinuxCamera::GetInstance()->LoadINISettings(ini); mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT); ColorFinder* ball_finder = new ColorFinder(); ball_finder->LoadINISettings(ini); httpd::ball_finder = ball_finder; BallTracker tracker = BallTracker(); //////////////////// Framework Initialize //////////////////////////// if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false) { linux_arbotixpro.SetPortName(U2D_DEV_NAME1); if (MotionManager::GetInstance()->Initialize(&arbotixpro) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } } Walking::GetInstance()->LoadINISettings(ini); usleep(100); MotionManager::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); //MotionManager::GetInstance()->StartThread(); //LinuxMotionTimer::Initialize(MotionManager::GetInstance()); LinuxMotionTimer linuxMotionTimer; linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Start(); MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false); MotionManager::GetInstance()->SetEnable(true); ///////////////////////////////////////////////////////////////////// Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); //Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8); //Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8); while (isRunning) { //usleep(10000); Point2D pos; LinuxCamera::GetInstance()->CaptureFrame(); tracker.Process(ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame)); rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame; for (int i = 0; i < rgb_ball->m_NumberOfPixels; i++) { if (ball_finder->m_result->m_ImageData[i] == 1) { rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 0] = 255; rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 1] = 0; rgb_ball->m_ImageData[i * rgb_ball->m_PixelSize + 2] = 0; } } streamer->send_image(rgb_ball); } return 0; }
int main(int argc, char *argv[]) { int trackerSel; change_current_dir(); minIni* ini = new minIni(INI_FILE_PATH); minIni* ini1 = new minIni(M_INI); StatusCheck::m_ini = ini; StatusCheck::m_ini1 = ini1; //////////////////// Framework Initialize //////////////////////////// if(MotionManager::GetInstance()->Initialize(&cm730) == false) { linux_cm730.SetPortName(U2D_DEV_NAME1); if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Fail to initialize Motion Manager!\n"); return 0; } } Walking::GetInstance()->LoadINISettings(ini); usleep(100); MotionManager::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance()); MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance()); //MotionManager::GetInstance()->StartThread(); //LinuxMotionTimer::Initialize(MotionManager::GetInstance()); LinuxMotionTimer linuxMotionTimer; linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Start(); ///////////////////////////////////////////////////////////////////// // MotionManager::GetInstance()->LoadINISettings(ini); int firm_ver = 0,retry=0; //important but allow a few retries while(cm730.ReadByte(JointData::ID_HEAD_PAN, MX28::P_VERSION, &firm_ver, 0) != CM730::SUCCESS) { fprintf(stderr, "Can't read firmware version from Dynamixel ID %d!! \n\n", JointData::ID_HEAD_PAN); retry++; if(retry >=3) exit(1);// if we can't do it after 3 attempts its not going to work. } if(0 < firm_ver && firm_ver < 40) { Action::GetInstance()->LoadFile(MOTION_FILE_PATH); } else { fprintf(stderr, "Wrong firmware version %d!! \n\n", JointData::ID_HEAD_PAN); exit(0); } //conversion! //////////////// /* Action::GetInstance()->LoadFile("../../../Data/motion.bin"); int j,k,p,a; double f; for(k=0;k<Action::MAXNUM_PAGE;k++) { Action::GetInstance()->LoadPage(k, &Page); for(j=0;j<Action::MAXNUM_STEP;j++) { for(p=0;p<31;p++) { a = Page.step[j].position[p]; if(a < 1024) { f = ((a-512)*10)/3+2048; a = (int)f; if(a<0) a =0; if(a>4095) a = 4095; Page.step[j].position[p] = a; } } } Action::GetInstance()->SavePage(k, &Page); } exit(0); */ //copy page //////////////// if(argc>1 && strcmp(argv[1],"-copy")==0) { printf("Page copy -- uses files motion_src.bin and motion_dest.bin\n"); if(Action::GetInstance()->LoadFile((char *)"../../../Data/motion_src.bin") == false) { printf("Unable to open source file\n"); exit(1); } int k; void *page1; page1 = malloc(sizeof(Robot::Action::PAGE)); printf("Page to load:"); if(scanf("%d",&k) != EOF) { if(Action::GetInstance()->LoadPage(k, (Robot::Action::PAGE *)page1) == false) { printf("Unable to load page %d\n",k); exit(1); } if(Action::GetInstance()->LoadFile((char *)"../../../Data/motion_dest.bin") == false) { printf("Unable to open destination file\n"); exit(1); } if(Action::GetInstance()->SavePage(k, (Robot::Action::PAGE *)page1) == false) { printf("Unable to save page %d\n",k); exit(1); } printf("Completed successfully.\n"); exit(0); } } ///////////////////////////// /* Walking::GetInstance()->m_Joint.SetEnableBody(true,true); MotionManager::GetInstance()->SetEnable(true); Walking::GetInstance()->LoadINISettings(m_ini); cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL); PS3Controller_Start(); LinuxActionScript::PlayMP3("../../../Data/mp3/ready.mp3"); Action::GetInstance()->Start(15); while(Action::GetInstance()->IsRunning()) usleep(8*1000); */ Walking::GetInstance()->LoadINISettings(ini); MotionManager::GetInstance()->LoadINISettings(ini); Walking::GetInstance()->m_Joint.SetEnableBody(false); Head::GetInstance()->m_Joint.SetEnableBody(false); Action::GetInstance()->m_Joint.SetEnableBody(true); MotionManager::GetInstance()->SetEnable(true); cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL); if(PS3Controller_Start() == 0) printf("PS3 controller not installed.\n"); cm730.WriteWord(CM730::P_LED_HEAD_L, cm730.MakeColor(1,1,1),0); //determine current position StatusCheck::m_cur_mode = GetCurrentPosition(cm730); //LinuxActionScript::PlayMP3("../../../Data/mp3/ready.mp3"); if((argc>1 && strcmp(argv[1],"-off")==0) || (StatusCheck::m_cur_mode == SITTING)) { cm730.DXLPowerOn(false); //for(int id=JointData::ID_R_SHOULDER_PITCH; id<JointData::NUMBER_OF_JOINTS; id++) // cm730.WriteByte(id, MX28::P_TORQUE_ENABLE, 0, 0); } else { Action::GetInstance()->Start(15); while(Action::GetInstance()->IsRunning()) usleep(8*1000); } while(1) { StatusCheck::Check(cm730); if(StatusCheck::m_is_started == 0) continue; } return 0; }