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(void)
{
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
stringstream p;
p<<"[" << timeinfo->tm_hour<< ":"<< timeinfo->tm_min << ":" << timeinfo->tm_sec <<"]";//defines log files name
std::string logname = "/home/robot/Logs/log";
logname+=p.str();
startLog(logFile,logname);
cvflann::Logger::setDestination("/home/robot/Logs/log_rami_and_lihen");
cvflann::Logger::setLevel(4);
char* test_text = "works";
cvflann::Logger::log(1,test_text);
if (sem_init(&ball_sem, 0, 1) != 0)
{
printf("Couldn't sem_init\n");
}
for (int i=0; i<BrainBall.x_detail ;i++)
{
BrainBall.x_place[i] = 0;
}
printf( "\n===== 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(&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);
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 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();
//pthread_create(&VisionT, NULL, VisionThread,(void *) NULL);
pthread_create(&gyroT, NULL, gyroThread,(void *) NULL);
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(4);
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')