int
main(int argc, char * argv[]) {
struct cmdlineInfo cmdline;
FILE * ifP;
bool eof;
pbm_init(&argc, argv);
parseCommandLine(argc, argv, &cmdline);
ifP = pm_openr(cmdline.inputFilename);
eof = FALSE;
while (!eof) {
doPage(ifP, cmdline);
pbm_nextimage(ifP, &eof);
}
pm_close(ifP);
return 0;
}
void ContinueMotion_HeartBeat(void)
{
uint8_t MOVING = FALSE;
if(MHD.CurrentPage != -1){
// Check if Servos are still in Motion
/*
gbpParameter[0] = P_MOVING;
gbpParameter[1] = 1; //Read Length
for( ServoNo=1;ServoNo < NUM_OF_SERVOS_ATTACHED; ServoNo++){
TxPacket(ServoNo,INST_READ,2);
uint8_t bRxPacketLength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]);
if(bRxPacketLength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]){
if(gbpRxBuffer[4]==0){
//TxDString("M:");TxD8Hex(gbpRxBuffer[5]);
if(gbpRxBuffer[5]!=0){
// Servo is still moving
MOVING = TRUE;
}
}else{
TxDString("Error:");//gbpRxBuffer[4]
}
}
}
*/
if(MHD.MoveFinishTime>0){MHD.MoveFinishTime--;}
if(MHD.MoveFinishTime==0){
MOVING = FALSE;
//TxDString("M");
}else{
MOVING = TRUE;
}
if(MOVING == FALSE){
if(MHD.PosePause>0){MHD.PosePause--;}
if(MHD.PosePause==0){
//TxDString("+\n\r");
if(ROBOTSTATUS!=STOP_POSE){
if(MHD.CurrentPose+1<MHD.NoOfPoses){
// next pose
MHD.CurrentPose++;
//TxDString("---Next Pose---\n\r");
MHD.PosePause = doPose(MHD.CurrentPage,MHD.CurrentPose);
}else{
// finished
//TxDString("Done Page\n\r");
//printf("[pg%02X]\n",MHD.CurrentPage);
#ifdef ROBOT_DASH_MODE
if(MHD.CurrentPage==FORWARD_STEP_COMPLETED_PAGE_NO){
Forward_Dash_StepNo++;
}
if(MHD.CurrentPage==BACKWARD_STEP_COMPLETED_PAGE_NO){
Backward_Dash_StepNo++;
}
#endif
cli();
uint8_t NextPage= pgm_read_byte_far ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_NEXT_PAGE);
sei();
if(ROBOTSTATUS!=STOP_PAGE){
//TxDString("NEXT PAGE\n\r");//NextPage;
if(NextPage != 0){
doPage(NextPage);
}else{
MHD.CurrentPage=-1;
}
}else{
uint8_t ExitPage= pgm_read_byte_far ((uint32_t)POSE_BASE_ADR + (uint32_t)POSE_PAGE_SIZE*(uint32_t)MHD.CurrentPage + (uint32_t)POSE_PAGE_EXIT_PAGE);
if(ExitPage != 0){
doPage(ExitPage);
}else{
MHD.CurrentPage=-1;
}
}
}
}
}
}
}
}
