int autoMoveBack(Aris::Core::MSG &msg)
{
EGAIT cmd=GAIT_MOVE_BACK;
Aris::Core::MSG data;
data.SetLength(sizeof(cmd));
data.Copy(&cmd,sizeof(cmd));
cs.NRT_SendData(data);
return 0;
}
int autoEndDiscover(Aris::Core::MSG &msg)
{
EGAIT cmd=GAIT_END_DISCOVER;
Aris::Core::MSG data;
data.SetLength(sizeof(cmd));
data.Copy(&cmd,sizeof(cmd));
cs.NRT_SendData(data);
return 0;
}
int autoTurnRight(Aris::Core::MSG &msg)
{
EGAIT cmd=GAIT_TURN_RIGHT;
Aris::Core::MSG data;
data.SetLength(sizeof(cmd));
data.Copy(&cmd,sizeof(cmd));
cs.NRT_SendData(data);
return 0;
}
int On_VS_ConnectionReceived(Aris::Core::CONN *pConn, const char* addr,int port)
{
Aris::Core::MSG msg;
msg.SetMsgID(VS_Connected);
msg.SetLength(sizeof(port));
msg.Copy(&port,sizeof(port));
msg.CopyMore(addr,strlen(addr));
PostMsg(msg);
return 0;
}
int autoStart(Aris::Core::MSG &msg)
{
EGAIT cmd=GAIT_HOME2START;
Aris::Core::MSG data;
data.SetLength(sizeof(cmd));
data.Copy(&cmd,sizeof(cmd));
cs.NRT_SendData(data);
return 0;
}
int OnVisualSystemDataNeeded(Aris::Core::MSG &msg)
{
static int i = 0;
cout<<"Capture "<<i<<" begin!!!"<<endl;
visionsensor.capture(&i);
while(1)
{
if(i>40)
{
Kinect::ControlCommand = NoValidCommand;
Aris::Core::MSG data;
data.SetLength(1 * sizeof(int));
memset(data.GetDataAddress(), 0, data.GetLength());
data.Copy(&Kinect::ControlCommand, sizeof(i));
pVisualSystem->SendData(data);
break;
}
else
{
if(Kinect::IsCaptureEnd == true)
{
Kinect::IsCaptureEnd = false;
Aris::Core::MSG data;
data.SetLength(1 * sizeof(int));
memset(data.GetDataAddress(), 0, data.GetLength());
data.Copy(&Kinect::ControlCommand, sizeof(i));
pVisualSystem->SendData(data);
break;
}
}
}
return 0;
}
int On_CS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data)
{
int cmd_id=data.GetMsgID();
Aris::Core::MSG CMD;
CMD.SetMsgID(CS_CMD_Received);
CMD.SetLength(sizeof(int));
CMD.Copy(&cmd_id,sizeof(int));
// if the cmd has data, it should be repost together
CMD.CopyMore(data.GetDataAddress(), data.GetLength());
cout<<"received CMD is "<<cmd_id<<endl;
PostMsg(CMD);
return 0;
}
int tg(CMachineData& machineData,RT_MSG& msg)
{
const int MapAbsToPhy[18]=
{
10, 11, 9,
12, 14, 13,
17, 15, 16,
6, 8, 7,
3, 5, 4,
0, 2, 1
};
const int MapPhyToAbs[18]=
{
15, 17, 16,
12, 14, 13,
9, 11, 10,
2, 0, 1,
3, 5, 4,
7, 8, 6
};
int CommandID;
CommandID=msg.GetMsgID();
switch(CommandID)
{
case NOCMD:
for(int i=0;i<18;i++)
{
machineData.motorsCommands[i]=EMCMD_NONE;
}
rt_printf("NONE Command Get in NRT\n" );
break;
case ENABLE:
for(int i=0;i<18;i++)
{
machineData.motorsCommands[i]=EMCMD_ENABLE;
}
rt_printf("ENABLE Command Get in NRT\n" );
break;
case POWEROFF:
for(int i=0;i<18;i++)
{
machineData.motorsCommands[i]=EMCMD_POWEROFF;
gait.IfReadytoSetGait(false,i);
}
rt_printf("POWEROFF Command Get in NRT\n" );
break;
case STOP:
for(int i=0;i<18;i++)
{
machineData.motorsCommands[i]=EMCMD_STOP;
}
rt_printf("STOP Command Get in NRT\n" );
break;
case RUNNING:
for(int i=0;i<18;i++)
{
machineData.motorsCommands[i]=EMCMD_RUNNING;
gait.IfReadytoSetGait(true,i);
}
rt_printf("RUNNING Command Get in NRT\n" );
break;
case GOHOME_1:
machineData.motorsCommands[MapAbsToPhy[0]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[1]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[2]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[6]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[7]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[8]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[12]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[13]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[14]]=EMCMD_GOHOME;
gaitcmd[MapAbsToPhy[0]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[1]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[2]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[6]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[7]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[8]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[12]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[13]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[14]]=EGAIT::GAIT_HOME;
rt_printf("GOHOME_1 Command Get in NRT\n" );
break;
case GOHOME_2:
machineData.motorsCommands[MapAbsToPhy[3]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[4]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[5]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[9]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[10]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[11]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[15]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[16]]=EMCMD_GOHOME;
machineData.motorsCommands[MapAbsToPhy[17]]=EMCMD_GOHOME;
gaitcmd[MapAbsToPhy[3]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[4]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[5]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[9]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[10]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[11]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[15]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[16]]=EGAIT::GAIT_HOME;
gaitcmd[MapAbsToPhy[17]]=EGAIT::GAIT_HOME;
rt_printf("GOHOME_2 Command Get in NRT\n" );
break;
case HOME2START_1:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
}
gaitcmd[MapAbsToPhy[0]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[1]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[2]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[6]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[7]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[8]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[12]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[13]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[14]]=EGAIT::GAIT_HOME2START;
rt_printf("HOME2START_1 Command Get in NRT\n" );
}
break;
case HOME2START_2:
if(gait.m_gaitState[MapAbsToPhy[3]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
}
gaitcmd[MapAbsToPhy[3]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[4]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[5]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[9]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[10]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[11]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[15]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[16]]=EGAIT::GAIT_HOME2START;
gaitcmd[MapAbsToPhy[17]]=EGAIT::GAIT_HOME2START;
rt_printf("HOME2START_2 Command Get in NRT\n" );
}
break;
case FORWARD:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_MOVE;
rt_printf("FORWARD Command Get in NRT\n" );
}
}
break;
case BACKWARD:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_MOVE_BACK;
rt_printf("BACKWARD Command Get in NRT\n" );
}
}
break;
case TURNLEFT:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_TURN_LEFT;
rt_printf("TURNLEFT Command Get in NRT\n" );
}
}
break;
case TURNRIGHT:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_TURN_RIGHT;
rt_printf("TURNRIGHT Command Get in NRT\n" );
}
}
break;
case LEGUP:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_LEGUP;
rt_printf("LEGUP Command Get in NRT\n" );
}
}
break;
case MOVE:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_MOVE_MAP;
rt_printf("MOVE_MAP Command Get in NRT\n" );
}
}
break;
case TURN:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_TURN_MAP;
rt_printf("TURN_MAP Command Get in NRT\n" );
}
}
break;
case BEGINDISCOVER:
if(gait.m_gaitState[MapAbsToPhy[0]]==GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_BEGIN_DISCOVER;
rt_printf("BEGINDISCOVER Command Get in NRT\n" );
}
}
break;
case ENDDISCOVER:
if(gait.m_gaitState[MapAbsToPhy[0]]== GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_END_DISCOVER;
rt_printf("ENDDISCOVER Command Get in NRT\n" );
}
}
break;
case STEPUP:
if(gait.m_gaitState[MapAbsToPhy[0]]== GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_STEPUP_MAP;
rt_printf("STEPUP_MAP Command Get in NRT\n" );
}
}
break;
case STEPDOWN:
if(gait.m_gaitState[MapAbsToPhy[0]]== GAIT_STOP)
{
for(int i=0;i<18;i++)
{
machineData.motorsModes[i]=EOperationMode::OM_CYCLICVEL;
gaitcmd[MapAbsToPhy[i]]=EGAIT::GAIT_STEPDOWN_MAP;
rt_printf("STEPDOWN_MAP Command Get in NRT\n" );
}
}
break;
default:
break;
}
gait.RunGait(gaitcmd,machineData);
if(gait.IsGaitFinished())
{
if(CGait::IsMove == true&&CGait::IsMoveEnd == true)
{
CGait::IsMove = false;
CGait::IsMoveEnd = false;
cout<<"MOVE FINISHED!"<<endl;
Vision_Msg visioncmd = Vision_UpperControl;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
if(CGait::IsTurn == true&&CGait::IsTurnEnd == true)
{
CGait::IsTurn = false;
CGait::IsTurnEnd = false;
cout<<"TURN FINISHED!"<<endl;
Vision_Msg visioncmd = Vision_UpperControl;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
if(CGait::IsBeginDiscoverStart == true && CGait::IsBeginDiscoverEnd == true)
{
cout<<"BEGINDISCOVER FINISHED!"<<endl;
Vision_Msg visioncmd = Vision_StepUp;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
CGait::IsBeginDiscoverStart = false;
CGait::IsBeginDiscoverStart = false;
}
if(CGait::IsEndDiscoverStart == true && CGait::IsEndDiscoverEnd == true)
{
cout<<"ENDDISCOVER FINISHED!"<<endl;
Vision_Msg visioncmd = Vision_UpperControl;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
CGait::IsEndDiscoverStart = false;
CGait::IsEndDiscoverStart = false;
}
if(CGait::IsStepUp == true&&CGait::IsStepUpstep == true)
{
CGait::IsStepUpstep = false;
Vision_Msg visioncmd = Vision_StepUp;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
if(CGait::IsStepDown == true&&CGait::IsStepDownstep == true)
{
CGait::IsStepDownstep = false;
Vision_Msg visioncmd = Vision_StepDown;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
if(CGait::IsStepOver == true&&CGait::IsStepOverstep == true)
{
CGait::IsStepOverstep = false;
Vision_Msg visioncmd = Vision_StepOver;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
}
return 0;
}
int On_VS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data)
{
cout<<"Calibration Singnal Recieved"<<endl;
int num;
memcpy(&num,data.GetDataAddress(),data.GetLength());
//stop num >= 54
if (num >= 62 )
{
cout<<"Calibration finished"<<endl;
CGait::IsAutoCalibrateRegistered = false;
}
else
{
ifstream inf;
stringstream filenum;
string filename;
filenum<<(num-1);
filename = "./calibration_txt/"+filenum.str() + "F.txt";
inf.open(filename);
int Line_Num;
double temp;
for(int i=0;i<GAIT_AUTOCALIB_LEN/2;i++)
{
inf>>Line_Num;
for(int j=0;j<GAIT_WIDTH;j++)
{
inf>>temp;
CGait::GaitAutoCalib[i][j]=-(int)temp;
}
}
inf.close();
filenum.str("");
filename.clear();
filenum<<num;
filename = "./calibration_txt/"+filenum.str()+".txt";
inf.open(filename);
for(int i=GAIT_AUTOCALIB_LEN/2;i<GAIT_AUTOCALIB_LEN;i++)
{
inf>>Line_Num;
for(int j=0;j<GAIT_WIDTH;j++)
{
inf>>temp;
CGait::GaitAutoCalib[i][j]=-(int)temp;
}
}
inf.close();
cout<<"Calibration "<<num<<" begin"<<endl;
cout<<CGait::GaitAutoCalib[3999][0]<<endl;
EGAIT cmd=GAIT_AUTO_CALIBRATION;
Aris::Core::MSG data;
data.SetLength(sizeof(cmd));
data.Copy(&cmd,sizeof(cmd));
cout<<"Send Message to CS"<<endl;
cs.NRT_SendData(data);
}
return 0;
}
int OnGetControlCommand(MSG &msg)
{
int CommandID;
msg.Paste(&CommandID,sizeof(int));
Aris::Core::MSG data;
switch(CommandID)
{
case 1:
data.SetMsgID(POWEROFF);
cs.NRT_PostMsg(data);
break;
case 2:
data.SetMsgID(STOP);
cs.NRT_PostMsg(data);
break;
case 3:
data.SetMsgID(ENABLE);
cs.NRT_PostMsg(data);
break;
case 4:
data.SetMsgID(RUNNING);
cs.NRT_PostMsg(data);
break;
case 5:
data.SetMsgID(GOHOME_1);
cs.NRT_PostMsg(data);
break;
case 6:
data.SetMsgID(GOHOME_2);
cs.NRT_PostMsg(data);
break;
case 7:
data.SetMsgID(HOME2START_1);
cs.NRT_PostMsg(data);
break;
case 8:
data.SetMsgID(HOME2START_2);
cs.NRT_PostMsg(data);
break;
case 9:
data.SetMsgID(FORWARD);
cs.NRT_PostMsg(data);
break;
case 10:
data.SetMsgID(BACKWARD);
cs.NRT_PostMsg(data);
break;
case 11:
data.SetMsgID(TURNLEFT);
cs.NRT_PostMsg(data);
break;
case 12:
data.SetMsgID(TURNRIGHT);
cs.NRT_PostMsg(data);
break;
case 13:
data.SetMsgID(LEGUP);
cs.NRT_PostMsg(data);
break;
case 14:
{
CGait::IsVisionWalk = true;
Vision_Msg visioncmd = Vision_UpperControl;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
break;
case 15:
{
CGait::IsWalkAvoidRegistered = true;
Vision_Msg visioncmd = Walk_Avoid;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
break;
default:
cout<<"Do Not Get Validate CMD"<<endl;
break;
}
return CommandID;
}
int On_VS_DataReceived(Aris::Core::CONN *pConn, Aris::Core::MSG &data)
{
cout<<"receive data from visual system"<<endl;
switch (data.GetMsgID())
{
case 34:
{
cout<<"MOVE!"<<endl;
CGait::IsMove = true;
double *move_data = new double [data.GetLength()/sizeof(double)];
memcpy(move_data, data.GetDataAddress(), data.GetLength());
cout<<move_data[0]<<" "<<move_data[1]<<" "<<move_data[2]<<endl;
HexIII.RobotMove(move_data, *Gait_Move_Map);
for(int j=0;j<GAIT_MOVE_MAP_LEN;j++)
{
for(int i=0;i<GAIT_WIDTH;i++)
{
CGait::GaitMoveMap[j][i]=-(int)(Gait_Move_Map[j][i]);
}
}
Aris::Core::MSG controlcmd;
controlcmd.SetMsgID(MOVE);
cout<<"Send MOVE Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
break;
case 35:
{
cout<<"TURN!"<<endl;
CGait::IsTurn = true;
double *turn_data = new double [data.GetLength()/sizeof(double)];
memcpy(turn_data, data.GetDataAddress(), data.GetLength());
*turn_data = *turn_data*180/M_PI;
cout<<*turn_data<<endl;
HexIII.RobotTurn(turn_data, *Gait_Turn_Map);
for(int j=0;j<GAIT_TURN_MAP_LEN;j++)
{
for(int i=0;i<GAIT_WIDTH;i++)
{
CGait::GaitTurnMap[j][i]=-(int)(Gait_Turn_Map[j][i]);
}
}
Aris::Core::MSG controlcmd;
controlcmd.SetMsgID(TURN);
cout<<"Send TURN Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
break;
case 36:
{
cout<<"STEP UP!"<<endl;
CGait::IsStepUp = true;
double *map = new double [data.GetLength()/sizeof(double)];
memcpy(map, data.GetDataAddress(), data.GetLength());
static double StepUP_currentH[6] = {-1.05, -1.05, -1.05, -1.05, -1.05, -1.05};
static double StepUP_nextH[6];
memcpy(StepUP_nextH,map,sizeof(StepUP_nextH));
if(StepUP_currentH[0] == -0.85&&StepUP_currentH[1] == -0.85&&StepUP_currentH[2] == -0.85
&&StepUP_currentH[3] == -0.85&&StepUP_currentH[4] == -0.85&&StepUP_currentH[5] == -0.85
&&StepUP_nextH[0] == -0.85&&StepUP_nextH[1] == -0.85&&StepUP_nextH[2] == -0.85
&&StepUP_nextH[3] == -0.85&&StepUP_nextH[4] == -0.85&&StepUP_nextH[5] == -0.85)
{
cout<<"STEPUP FINISHED"<<endl;
CGait::IsStepUp = false;
delete[] map;
Vision_Msg visioncmd = Vision_UpperControl;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
else
{
double StepUpLen = 0.325;
HexIII.RobotStepUp(&StepUpLen,StepUP_currentH,StepUP_nextH,*Gait_StepUp__Map);
memcpy(StepUP_currentH,StepUP_nextH,sizeof(StepUP_nextH));
delete[] map;
for(int j=0;j<GAIT_STEPUPANDDOWN_LEN;j++)
{
for(int i=0;i<GAIT_WIDTH;i++)
{
CGait::GaitStepUpMap[j][i]=-(int)(Gait_StepUp__Map[j][i]);
}
}
Aris::Core::MSG controldata;
controldata.SetMsgID(STEPUP);
cs.NRT_PostMsg(controldata);
}
}
break;
case 37:
{
if(data.GetLength() == sizeof(int))
{
Vision_Msg visioncmd = Vision_StepDown;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
else
{
CGait::IsStepDown = true;
cout<<"STEP DOWN!"<<endl;
double *map = new double [data.GetLength()/sizeof(double)];
memcpy(map, data.GetDataAddress(), data.GetLength());
static double StepDown_currentH[6] = {-0.85, -0.85, -0.85, -0.85, -0.85, -0.85};
static double StepDown_nextH[6];
memcpy(StepDown_nextH,map,sizeof(StepDown_nextH));
if(StepDown_currentH[0] == -1.05&&StepDown_currentH[1] == -1.05&&StepDown_currentH[2] == -1.05
&&StepDown_currentH[3] == -1.05&&StepDown_currentH[4] == -1.05&&StepDown_currentH[5] == -1.05
&&StepDown_nextH[0] == -1.05&&StepDown_nextH[1] == -1.05&&StepDown_nextH[2] == -1.05
&&StepDown_nextH[3] == -1.05&&StepDown_nextH[4] == -1.05&&StepDown_nextH[5] == -1.05)
{
delete[] map;
CGait::IsStepDown = false;
cout<<"STEPDOWN FINISHED!"<<endl;
cout<<"ENDISCOVER!"<<endl;
CGait::IsEndDiscoverStart = true;
Aris::Core::MSG controlcmd;
controlcmd.SetMsgID(ENDDISCOVER);
cout<<"Send ENDDISCOVER Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
else
{
double StepDownLen = 0.325;
HexIII.RobotStepDown(&StepDownLen,StepDown_currentH,StepDown_nextH,*Gait_StepDown__Map);
memcpy(StepDown_currentH,StepDown_nextH,sizeof(StepDown_nextH));
delete[] map;
for(int j=0;j<GAIT_STEPUPANDDOWN_LEN;j++)
{
for(int i=0;i<GAIT_WIDTH;i++)
{
CGait::GaitStepDownMap[j][i]=-(int)(Gait_StepDown__Map[j][i]);
}
}
Aris::Core::MSG controldata;
controldata.SetMsgID(STEPDOWN);
cs.NRT_PostMsg(controldata);
}
}
}
break;
case 38:
{
if(data.GetLength() == sizeof(int))
{
Vision_Msg visioncmd = Vision_StepOver;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
else
{
cout<<"STEPOVER!"<<endl;
CGait::IsStepOver = true;
double *stepover_data = new double [data.GetLength()/sizeof(double)];
memcpy(stepover_data, data.GetDataAddress(), data.GetLength());
int count = int(stepover_data[0]);
if(count == 5)
{
CGait::IsStepOver = false;
cout<<"STEPOVER FINISHED!"<<endl;
Vision_Msg visioncmd = Vision_UpperControl;
Aris::Core::MSG visionmsg;
visionmsg.SetMsgID(VS_Capture);
visionmsg.SetLength(sizeof(visioncmd));
visionmsg.Copy(&visioncmd, sizeof(visioncmd));
PostMsg(visionmsg);
}
else
{
double move_data[3];
move_data[0] = stepover_data[1];
move_data[1] = stepover_data[2];
move_data[2] = stepover_data[3];
cout<<"Move Data From Sensor"<<endl;
cout<<"Move X: "<<move_data[0]<<endl;
cout<<"Move Y: "<<move_data[1]<<endl;
cout<<"Move Z: "<<move_data[2]<<endl;
HexIII.RobotMove(move_data, *Gait_Move_Map);
for(int j=0;j<GAIT_MOVE_MAP_LEN;j++)
{
for(int i=0;i<GAIT_WIDTH;i++)
{
CGait::GaitMoveMap[j][i]=-(int)(Gait_Move_Map[j][i]);
}
}
Aris::Core::MSG controlcmd;
controlcmd.SetMsgID(MOVE);
cout<<"Send MOVE Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
}
}
break;
case 39:
{
cout<<"BEGINDISCOVER!"<<endl;
CGait::IsBeginDiscoverStart = true;
Aris::Core::MSG controlcmd;
controlcmd.SetMsgID(BEGINDISCOVER);
cout<<"Send BEGINDISCOVER Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
break;
case 40:
{
cout<<"Vision Control Singnal Recieved, Beging Walk Avoid!"<<endl;
int command;
memcpy(&command, data.GetDataAddress(), data.GetLength());
Aris::Core::MSG controlcmd;
switch (command)
{
case 0:
CGait::IsWalkAvoidRegistered = false;
break;
case 1:
{
controlcmd.SetMsgID(FORWARD);
cout<<"Send GAIT_MOVE Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
break;
case 2:
{
controlcmd.SetMsgID(BACKWARD);
cout<<"Send GAIT_MOVE_BACK Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
break;
case 3:
{
controlcmd.SetMsgID(TURNLEFT);
cout<<"Send GAIT_TURN_LEFT Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
break;
case 4:
{
controlcmd.SetMsgID(TURNRIGHT);
cout<<"Send GAIT_TURN_RIGHT Message to CS"<<endl;
cs.NRT_PostMsg(controlcmd);
}
break;
default:
break;
}
}
break;
default:
break;
}
return 0;
}