void StatusCheck::Check(CM730 &cm730)
{
int value=0;
//////////////////////////////////////////////////////////////////////////////////////
// System Standby Toggle
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.PS != 0)
{
if(Walking::GetInstance()->IsRunning() == true)
{
Walking::GetInstance()->Stop();
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
}
if(ToggleRobotStandby()==1)
// wait for key release
while(PS3.key.PS != 0) usleep(8000);
}
if(robotInStandby == 1) return;
//////////////////////////////////////////////////////////////////////////////////////
// IMU AUTO GETUP ROUTINE
//////////////////////////////////////////////////////////////////////////////////////
/*
if(MotionStatus::FALLEN != STANDUP && (m_cur_mode == SOCCER) && m_is_started == 1)
{
Walking::GetInstance()->Stop();
resetLEDs(cm730);
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
Action::GetInstance()->m_Joint.SetEnableBody(true, true);
while(Action::GetInstance()->IsRunning() == 1) usleep(8000);
if(MotionStatus::FALLEN == FORWARD)
Action::GetInstance()->Start(1); // FORWARD GETUP 10
else if(MotionStatus::FALLEN == BACKWARD)
Action::GetInstance()->Start(1); // BACKWARD GETUP 11
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
}
*/
//////////////////////////////////////////////////////////////////////////////////////
// Shut it down, sit down.
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.Cross != 0)
{
resetLEDs(cm730);
Walking::GetInstance()->Stop();
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
m_is_started = 0;
m_cur_mode = SITTING;
LinuxActionScript::m_stop = 1;
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true);
while(Action::GetInstance()->Start(16) == false) usleep(8000);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true);
while(PS3.key.Cross != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// Start Walk Ready
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.Triangle != 0)
{
if(m_is_started == 0)
{
cm730.DXLPowerOn(true);
}
resetLEDs(cm730);
Walking::GetInstance()->Stop();
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
int lastMode = m_cur_mode;
m_cur_mode = SOCCER;
MotionManager::GetInstance()->Reinitialize();
MotionManager::GetInstance()->SetEnable(true);
m_is_started = 1;
bLJState = bRJState = false;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
if(lastMode == SITTING)
Action::GetInstance()->Start(8); //50
else
Action::GetInstance()->Start(9); //9
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true);
Action::GetInstance()->m_Joint.SetEnableBody(false);
usleep(5000);
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true);
while(PS3.key.Triangle != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// Action Script Button Assignment
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
// Select
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.Select != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_SELECT);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.Select != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// Square
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.Square != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_SQUARE);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.Square != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// Circle
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.Circle != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_CIRCLE);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.Circle != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// R1
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.R1 != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_R1);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.R1 != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// R2
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.R2 != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_R2);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.R2 != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// L1
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.L1 != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_L1);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.L1 != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// L2
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.L2 != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_L2);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.L2 != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// Start
//////////////////////////////////////////////////////////////////////////////////////
if(PS3.key.Start != 0)
{
if(LinuxActionScript::m_is_running == 0)
{
m_cur_mode = SOCCER;
LinuxActionScript::m_stop = 0;
Head::GetInstance()->m_Joint.SetEnableBody(false);
Walking::GetInstance()->m_Joint.SetEnableBody(false);
Action::GetInstance()->m_Joint.SetEnableBody(true);
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH_START);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
while(PS3.key.Select != 0) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
// PS3 R/C Control code
//////////////////////////////////////////////////////////////////////////////////////
if(Walking::GetInstance()->IsRunning() == true)
{
int rx,ry,dead_band=6;
double FBStep=0,RLTurn=0,RLStep=0,xd,yd;
static double speedAdjSum=0;
#ifdef Southpaw
rx = -(PS3.key.RJoyX-128);
ry = -(PS3.key.RJoyY-128);
#else
rx = -(PS3.key.LJoyX-128);
ry = -(PS3.key.LJoyY-128);
#endif
// fprintf(stderr, " (X:%d Y:%d)\n", rx, ry);
if(abs(rx) > dead_band || abs(ry) > dead_band)
{
xd = (double)(rx-dead_band)/256;
yd = (double)(ry-dead_band)/256;
RLTurn = 50*xd;
FBStep = 45*yd;
// fprintf(stderr, " (yd:%.1f)\n", yd);
Walking::GetInstance()->HIP_PITCH_OFFSET = Walking::GetInstance()->HIP_PITCH_OFFSET_START + yd/2;
if(FBStep < 0)
{
FBStep = 20*yd;
}
speedAdjSum += yd;
if(speedAdjSum > Walking::GetInstance()->UPPER_VELADJ_LIMIT)
{
speedAdjSum = Walking::GetInstance()->UPPER_VELADJ_LIMIT;
}
else if(speedAdjSum < Walking::GetInstance()->LOWER_VELADJ_LIMIT)
{
speedAdjSum = Walking::GetInstance()->LOWER_VELADJ_LIMIT;
}
else
{
speedAdjSum = 0;
}
}
Walking::GetInstance()->speedAdj = speedAdjSum;
Walking::GetInstance()->X_OFFSET = Walking::GetInstance()->X_OFFSET_START - speedAdjSum;
double hip_offset = Walking::GetInstance()->HIP_PITCH_OFFSET;
// fprintf(stderr, " (hip offset:%.1f)\n", hip_offset);
Walking::GetInstance()->X_MOVE_AMPLITUDE = FBStep;
Walking::GetInstance()->Y_MOVE_AMPLITUDE = RLStep;
Walking::GetInstance()->A_MOVE_AMPLITUDE = RLTurn;
// fprintf(stderr, " (FB:%.1f RL:%.1f)\n", FBStep, RLTurn);
}
else //things only done in auto mode
{
}
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
// STOP WALKING GAIT ENGINE
//////////////////////////////////////////////////////////////////////////////////////
if(Walking::GetInstance()->IsRunning() == true && PS3.key.Down != 0)
{
fprintf(stderr, "STOPPING WALKING GAIT\n");
resetLEDs(cm730);
Walking::GetInstance()->Stop();
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000);
}
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
// START WALKING GAIT ENGINE
//////////////////////////////////////////////////////////////////////////////////////
if(Walking::GetInstance()->IsRunning() == false && PS3.key.Up != 0)
{
if(m_cur_mode == SOCCER)
{
fprintf(stderr, "STARTING WALKING GAIT\n");
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();
}
}
//////////////////////////////////////////////////////////////////////////////////////
/*
//////////////////////////////////////////////////////////////////////////////////////
// toggle head from auto to not
if(PS3.key.LeftHat != 0)
{
if(bLJState == true)
{
bLJState = false;
if(m_cur_mode == SOCCER)
PS3BallFollower::GetInstance()->bHeadAuto = false;
else if(m_cur_mode == SITTING)
LineFollower::GetInstance()->bHeadAuto = false;
else if(m_cur_mode == LINE_FOLLOWING)
LineFollower::GetInstance()->bHeadAuto = false;
else if(m_cur_mode == ROBOT_FOLLOWING)
RobotFollower::GetInstance()->bHeadAuto = false;
//double pan,tilt;
//pan = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN);
//tilt = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_TILT);
//Head::GetInstance()->MoveByAngle(pan,tilt);
}
else
{
bLJState = true;
if(m_cur_mode == SOCCER)
{
if(PS3BallFollower::GetInstance()->bFullAuto == true)
PS3BallFollower::GetInstance()->bHeadAuto = true;
else
PS3BallFollower::GetInstance()->bHeadAuto = false;
}
else if(m_cur_mode == SITTING)
{
LineFollower::GetInstance()->bHeadAuto = true;
}
else if(m_cur_mode == LINE_FOLLOWING)
{
if(LineFollower::GetInstance()->bFullAuto == true)
LineFollower::GetInstance()->bHeadAuto = true;
else
LineFollower::GetInstance()->bHeadAuto = false;
}
else if(m_cur_mode == ROBOT_FOLLOWING)
{
if(RobotFollower::GetInstance()->bFullAuto == true)
RobotFollower::GetInstance()->bHeadAuto = true;
else
RobotFollower::GetInstance()->bHeadAuto = false;
}
}
PS3Vibrate();
// wait for key release
while(PS3.key.LeftHat != 0) usleep(8000);
}
*/
//////////////////////////////////////////////////////////////////////////////////////
// PS3 Head Control
//////////////////////////////////////////////////////////////////////////////////////
if((PS3BallFollower::GetInstance()->bHeadAuto == false && (m_cur_mode == SOCCER || m_cur_mode == SITTING)) )
{
int lx,ly,dead_band=6;
double pan,tilt;
pan = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN);
tilt = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_TILT);
Point2D pos = Point2D(pan,tilt);
#ifdef Southpaw
lx = -(PS3.key.LJoyX-128);
ly = -(PS3.key.LJoyY-128);
#else
lx = -(PS3.key.RJoyX-128);
ly = -(PS3.key.RJoyY-128);
#endif
if(abs(lx) > dead_band || abs(ly) > dead_band)
{
pos.X = pan + 0.2*Camera::VIEW_V_ANGLE*(lx-dead_band)/256;
pos.Y = tilt + 0.2*Camera::VIEW_H_ANGLE*(ly-dead_band)/256;
}
Head::GetInstance()->MoveByAngle(pos.X,pos.Y);
//Head::GetInstance()->MoveTracking(pos);
}
//////////////////////////////////////////////////////////////////////////////////////
}
// turn all servos off (make sure robot is sitting)
void ServoShutdown() {
WalkToggle(false); // turn off walking if is walking
PlayAction(15); // sit robot
// shutdown
cm730.DXLPowerOn(false);
}
void ServoStartup() {
cm730.DXLPowerOn(true);
}
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;
}