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')
/**
Controls the angle of the X and Y axis. Only one axis will be tilted from the
zero position at a time.
@param xyCurPos_px The current position of the ball in (x, y) (in pixels)
@return cv::Point A vector containing the desired angle for each axis
*/
cv::Point DualAxisController::AngleControl(cv::Point xyCurPos_px)
{
//NOTE: AngleControl is called once per frame
this->xyCurPos_px = xyCurPos_px;
ComputeError();
int xTiltAngle = outputZero;
int yTiltAngle = outputZero;
/* Like the single axis tilt controller, we want to tilt and wait for
specified periods. However, we also only want to control a single axis
at a time. */
// If we're already controlling X, keep controlling it.
if (controllingX)
{
// If X is within the minimum position error, stop controlling it,
// reset the X timers and do nothing else this frame.
if (abs(error.x) <= minimumPositionError)
{
controllingX = false;
this->tiltStartTick = 0;
}
else
{
// We're controlling X, it's outside of the minimum position error,
// let's move it.
xTiltAngle = GetTilt();
}
}
// else if we're controlling Y, keep controlling it.
else if (controllingY) {
// If Y is within min pos, stop controlling and reset Y timers.
if (abs(error.y) <= minimumPositionError)
{
controllingY = false;
this->tiltStartTick = 0;
}
else
{
// Move Y!
yTiltAngle = GetTilt();
}
}
// else if we're controlling neither, see which one (if any) is above the
// minimum position error, and start controlling it.
else if (abs(error.x) > minimumPositionError)
{
controllingX = true;
xTiltAngle = GetTilt();
}
else if (abs(error.y) > minimumPositionError)
{
controllingY = true;
yTiltAngle = GetTilt();
}
// else, we're at our desired position within +- the minimum error
xTiltAngle = NormalizeAngle(xTiltAngle);
yTiltAngle = NormalizeAngle(yTiltAngle);
return cv::Point(xTiltAngle, yTiltAngle);
}
