int main(int argc, char *argv[]) { signal(SIGABRT, &sighandler); signal(SIGTERM, &sighandler); signal(SIGQUIT, &sighandler); signal(SIGINT, &sighandler); int ch; char filename[128]; change_current_dir(); if(argc < 2) strcpy(filename, MOTION_FILE_PATH); // Set default motion file path else strcpy(filename, argv[1]); minIni* ini = new minIni(INI_FILE_PATH); /////////////// Load/Create Action File ////////////////// if(Action::GetInstance()->LoadFile(filename) == false) { printf("Can not open %s\n", filename); printf("Do you want to make a new action file? (y/n) "); ch = _getch(); if(ch != 'y') { printf("\n"); return 0; } if(Action::GetInstance()->CreateFile(filename) == false) { printf("Fail to create %s\n", filename); return 0; } } //////////////////////////////////////////////////////////// //////////////////// Framework Initialize //////////////////////////// if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Initializing Motion Manager failed!\n"); return 0; } MotionManager::GetInstance()->SetEnable(false); MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Stop(); //MotionManager::GetInstance()->StopThread(); ///////////////////////////////////////////////////////////////////// MotionManager::GetInstance()->LoadINISettings(ini); DrawIntro(&cm730); while(1) { 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) // Right arrow key MoveRightCursor(); } } else if( ch == '[' ) UpDownValue(&cm730, -1); else if( ch == ']' ) UpDownValue(&cm730, 1); else if( ch == '{' ) UpDownValue(&cm730, -10); else if( ch == '}' ) UpDownValue(&cm730, 10); else if( ch == ' ' ) ToggleTorque(&cm730); else if( ch >= 'A' && ch <= 'z' ) { char input[128] = {0,}; char *token; int input_len; char cmd[80]; int num_param; int iparam[30]; 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' ) || ch == ' ' || ( ch >= '0' && ch <= '9')) { 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) { iparam[num_param++] = atoi(token); token = strtok( 0, " " ); } if(strcmp(cmd, "exit") == 0) { if(AskSave() == false) break; } else if(strcmp(cmd, "re") == 0) { ReadStep(&cm730); DrawPage(); } else if(strcmp(cmd, "help") == 0) HelpCmd(); else if(strcmp(cmd, "set") == 0) { if(num_param > 0) SetValue(&cm730, iparam[0]); else PrintCmd("Need parameter"); } else if(strcmp(cmd, "on") == 0) OnOffCmd(&cm730, true, num_param, iparam); else if(strcmp(cmd, "off") == 0) OnOffCmd(&cm730, false, num_param, iparam); else if(strcmp(cmd, "save") == 0) SaveCmd(ini); else if(strcmp(cmd, "pgain") == 0) { if(num_param > 0) GainCmd(&cm730, iparam[0], P_GAIN_COL); else PrintCmd("Need parameter"); } else if(strcmp(cmd, "igain") == 0) { if(num_param > 0) GainCmd(&cm730, iparam[0], I_GAIN_COL); else PrintCmd("Need parameter"); } else if(strcmp(cmd, "dgain") == 0) { if(num_param > 0) GainCmd(&cm730, iparam[0], D_GAIN_COL); else PrintCmd("Need parameter"); } else PrintCmd("Bad command! please input 'help'"); } } EndCommandMode(); } } DrawEnding(); return 0; }
int main(int argc, char *argv[]) { signal(SIGABRT, &sighandler); signal(SIGTERM, &sighandler); signal(SIGQUIT, &sighandler); signal(SIGINT, &sighandler); char filename[128]; change_current_dir(); if(argc < 2) strcpy(filename, MOTION_FILE_PATH); // Set default motion file path else strcpy(filename, argv[1]); //////////////////// Framework Initialize //////////////////////////// if(MotionManager::GetInstance()->Initialize(&cm730) == false) { printf("Initializing Motion Manager failed!\n"); exit(0); } MotionManager::GetInstance()->SetEnable(false); MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance()); linuxMotionTimer.Initialize(MotionManager::GetInstance()); linuxMotionTimer.Stop(); ///////////////////////////////////////////////////////////////////// head_ctrl.SetMode( EMOHead::COMMAND ); /* BLINKS */ for ( int i = 0; i < 10; ++i ) { blink_with_delay( 200000 ); } LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_chappie.mp3"); /* EYE DIRECTIONS */ head_ctrl.SetLookDirection( EMOHead::LOOK_LEFT ); sleep( 1 ); head_ctrl.SetLookDirection( EMOHead::LOOK_RIGHT ); sleep( 1 ); LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_chicken.mp3"); head_ctrl.SetLookDirection( EMOHead::LOOK_LEFT ); sleep( 1 ); head_ctrl.SetLookDirection( EMOHead::LOOK_RIGHT ); sleep( 1 ); head_ctrl.SetLookDirection( EMOHead::LOOK_FORWARD ); sleep( 1 ); /* ANTENNA POSITIONS */ head_ctrl.SetSpeeds( 64, 64, 64 ); head_ctrl.SetPositions( 255, 255, 255 ); LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_whats_that.mp3"); sleep( 1 ); head_ctrl.SetSpeeds( 8, 8, 8 ); head_ctrl.SetPositions( 0, 0, 0 ); sleep( 1 ); head_ctrl.SetSpeeds( 32, 32, 32 ); head_ctrl.SetPositions( 128, 128, 128 ); sleep( 1 ); head_ctrl.SetSpeeds( 8, 8, 8 ); head_ctrl.SetPositions( 255, 0, 255 ); sleep( 1 ); head_ctrl.SetSpeeds( 64, 64, 64 ); head_ctrl.SetPositions( 0, 255, 0 ); sleep( 1 ); head_ctrl.SetSpeeds( 32, 32, 32 ); head_ctrl.SetPositions( 128, 128, 128 ); sleep( 1 ); head_ctrl.SetSpeeds( 4, 4, 4 ); head_ctrl.SetPositions( 0, 0, 0 ); LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_no_he_can_talk_hes_smart.mp3"); /* EYE DOT PATTERN */ for ( int i = 0; i < 0xff; ++i ) { head_ctrl.SetEyeDotsRight( i ); head_ctrl.SetEyeDotsLeft( i ); usleep( 10000 ); } head_ctrl.SetEyeDots( EMOHead::DOTS_UP ); sleep( 1 ); head_ctrl.SetEyeDots( EMOHead::DOTS_DOWN ); sleep( 1 ); LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_i_cant_do_a_heist.mp3"); head_ctrl.SetEyeDotsRight( EMOHead::DOTS_WORRIED_RIGHT ); head_ctrl.SetEyeDotsLeft( EMOHead::DOTS_WORRIED_LEFT ); sleep( 3 ); head_ctrl.SetEyeDotsRight( EMOHead::DOTS_SAD_RIGHT ); head_ctrl.SetEyeDotsLeft( EMOHead::DOTS_SAD_LEFT ); sleep( 1 ); head_ctrl.SetEyeDots( EMOHead::DOTS_STAR ); sleep( 1 ); head_ctrl.SetEyeDots( EMOHead::DOTS_PLUS ); sleep( 1 ); LinuxActionScript::PlayMP3("../../../Data/mp3/chappie_clever_little_robot.mp3"); //ASCII art style spinner on high res OLED displays =P for ( int i=0; i < 5; ++i ) { head_ctrl.SetEyeDots( EMOHead::DOTS_BACKSLASH ); usleep( 100000 ); head_ctrl.SetEyeDots( EMOHead::DOTS_BAR ); usleep( 100000 ); head_ctrl.SetEyeDots( EMOHead::DOTS_FWDSLASH ); usleep( 100000 ); head_ctrl.SetEyeDots( EMOHead::DOTS_MINUS ); usleep( 100000 ); } head_ctrl.SetEyeDots( EMOHead::DOTS_SHIP ); sleep( 1 ); head_ctrl.SetEyeDots( EMOHead::DOTS_FULL ); //head_ctrl.SetMode( EMOHead::DEMO ); exit(0); }