www.pudn.com > v0.9.rar > CM_main1.c
#define MWINCLUDECOLORS /******************************************************************************* * Debug switch Section *******************************************************************************/ /******************************************************************************* * Include File Section *******************************************************************************/ #include#include #include #include "QU_queue.h" #include "nano-X.h" #include "math.h" #include "Hardware.h" /******************************************************************************* * Macro Define Section *******************************************************************************/ #define Button_Width 50//一般按钮的宽度 #define Button_High 25//一般按钮的高度 #define Button_Borad_Color BLUE//一般按钮的边框颜色 #define Button_Borad_Width 2//一般按钮的边框宽度 #define Button_Back_Color_White WHITE//按钮的白背景色状态颜色 #define Button_Back_Color_Gray GRAY//按钮的灰背景色状态颜色 #define Button_Start_X 10//一般按钮在控制面板的第一个按钮的左边位置 #define Button_Start_Y 5//一般按钮在控制面板的高度位置 #define Button_Between 10//一般按钮的间距 //------------------------------------------------------------- #define Button_Pannel_Width 50//把手按钮的宽度 #define Button_Pannel_High 25//把手按钮的高度 #define Button_Pannel_Borad_Color GRAY//把手按钮的边框颜色 #define Button_Pannel_Back_Color WHITE//把手按钮的背景色 #define Button_Pannel_Borad_Width 2//把手按钮的边框宽度 #define Button_Pannel_Start_X 5//把手按钮在根窗口的第一个按钮的左边位置 #define Button_Pannel_Start_Y 207//把手按钮在根窗口的高度位置 #define Button_Pannel_Between 10////把手按钮的间距 #define SET (0) #define SAVE (1) #define NONE (0) #define LINE (1) #define ROUNTD (2) #define STOP (0) #define START (1) #define ARRIVED (2) #define Real_x(a) (a) #define Real_y(b) (200-b) #define STEP_LENTGTH (0.031416) #define FORWARD (1) #define BACKWARD (0) #define LEFT_FORWARD (0xfc) #define LEFT_BACKWARD (0xf8) #define RIGHT_FORWARD (0xf3) #define RIGHT_BACKWARD (0xf2) #define ALL_FORWARD (0xff) #define ALL_BACKWARD (0xfa) #define ALL_DOWN (0xf0) #define LF_RB (0xfe) #define LB_RF (0xfb) #define DELAY_TIME (100) /******************************************************************************* * Struct Define Section *******************************************************************************/ /******************************************************************************* * Prototype Declare Section *******************************************************************************/ void CM_gvGoTo(int X_Y_pos); void delay(); /******************************************************************************* * Global Variable Declare Section *******************************************************************************/ static GR_WINDOW_ID WIN_Root; static GR_WINDOW_ID WIN_Screen; static GR_WINDOW_ID WIN_Control; static GR_WINDOW_ID WIN_Massage; static GR_WINDOW_ID WIN_Status; static GR_WINDOW_ID WIN_But_Pass; static GR_WINDOW_ID WIN_But_SetPass; static GR_WINDOW_ID WIN_But_Start; static GR_WINDOW_ID WIN_But_Stop; static GR_WINDOW_ID WIN_But_Line; static GR_WINDOW_ID WIN_But_Round; static GR_GC_ID Gid_MainWin; static GR_GC_ID Gid_WangGe; static GR_GC_ID Gid_ButtonDown; static GR_GC_ID Gid_Button; static GR_GC_ID Gid_Guiji; static GR_GC_ID Gid_CurrentSite; static GR_GC_ID Gid_Message; static GR_GC_ID Gid_ButtonName; static GR_GC_ID Gid_ButtonDownName; static GR_GC_ID Gid_Green_Green; static GR_GC_ID Gid_Red_Red; static GR_GC_ID Gid_Yellow_Yellow; static GR_GC_ID Gid_Black_Green; static GR_GC_ID Gid_Black_Red; static GR_GC_ID Gid_Black_Yellow; static GR_FONT_ID Gid_Font; GR_EVENT Event;//装载事件的结构 unsigned short Mark_SetButStatus=0;//"设置"按钮状态 unsigned short Mark_DrawButStatus=0;//画图状态标志 unsigned short Mark_DrawButStaPause=0;//画图状态标志 unsigned short Mark_OBStatus=0; int X_sit = -1;//存储鼠标点 int Y_sit = -1; int Current_x = 0;//物体所在的坐标 int Current_y = 0; char arry_x[8]; char arry_y[8]; int numberOfPoint=0; int X_Y_pos; struct QU_linkQueue_st astQueue;//队列结构体 /******************************************************************************* * File Static Variable Define Section *******************************************************************************/ /******************************************************************************* * Function Define Section *******************************************************************************/ void initDraw() { //为显示轨迹窗口分配上下文 Gid_MainWin=GrNewGC(); GrSetGCForeground(Gid_MainWin,WHITE); GrSetGCBackground(Gid_MainWin,WHITE); /******************************************************/ //为显示窗口网格分配上下文 Gid_WangGe=GrNewGC(); GrSetGCForeground(Gid_WangGe,GRAY); GrSetGCBackground(Gid_WangGe,GRAY); /******************************************************/ //为按下的按钮分配上下文 Gid_ButtonDown=GrNewGC(); GrSetGCForeground(Gid_ButtonDown,GRAY); GrSetGCBackground(Gid_ButtonDown,GRAY); /******************************************************/ //为正常的按钮分配上下文 Gid_Button=GrNewGC(); GrSetGCForeground(Gid_Button,WHITE); GrSetGCBackground(Gid_Button,WHITE); /******************************************************/ //为轨迹分配上下文 Gid_Guiji=GrNewGC(); GrSetGCForeground(Gid_Guiji,GREEN); GrSetGCBackground(Gid_Guiji,BLACK); /******************************************************/ //为物体当前所处位置分配上下文 Gid_CurrentSite=GrNewGC(); GrSetGCForeground(Gid_CurrentSite,RED); GrSetGCBackground(Gid_CurrentSite,BLACK); /******************************************************/ //为坐标值显示分配上下文 Gid_Message=GrNewGC(); GrSetGCForeground(Gid_Message,GREEN); GrSetGCBackground(Gid_Message,WHITE); /******************************************************/ //为按钮名字分配上下文 Gid_ButtonName=GrNewGC(); GrSetGCForeground(Gid_ButtonName,BLACK); GrSetGCBackground(Gid_ButtonName,WHITE); /******************************************************/ //为按下按钮名字分配上下文 Gid_ButtonDownName=GrNewGC(); GrSetGCForeground(Gid_ButtonDownName,BLACK); GrSetGCBackground(Gid_ButtonDownName,GRAY); /******************************************************/ Gid_Green_Green=GrNewGC(); GrSetGCForeground(Gid_Green_Green,GREEN); GrSetGCBackground(Gid_Green_Green,GREEN); /******************************************************/ Gid_Red_Red=GrNewGC(); GrSetGCForeground(Gid_Red_Red,RED); GrSetGCBackground(Gid_Red_Red,RED); /******************************************************/ Gid_Yellow_Yellow=GrNewGC(); GrSetGCForeground(Gid_Yellow_Yellow,YELLOW); GrSetGCBackground(Gid_Yellow_Yellow,YELLOW); /******************************************************/ Gid_Black_Green=GrNewGC(); GrSetGCForeground(Gid_Black_Green,YELLOW); GrSetGCBackground(Gid_Black_Green,GREEN); /******************************************************/ Gid_Black_Red=GrNewGC(); GrSetGCForeground(Gid_Black_Red,WHITE); GrSetGCBackground(Gid_Black_Red,RED); /******************************************************/ Gid_Black_Yellow=GrNewGC(); GrSetGCForeground(Gid_Black_Yellow,RED); GrSetGCBackground(Gid_Black_Yellow,YELLOW); /******************************************************/ //创建字体 Gid_Font=GrCreateFont("HZKFONT",0,NULL); GrSetFontSize(Gid_Font,16); GrSetFontRotation(Gid_Font, 330); GrSetFontAttr(Gid_Font, GR_TFKERNING | GR_TFANTIALIAS, 0); /******************************************************/ //设置各上下文字体 GrSetGCFont(Gid_MainWin, Gid_Font); GrSetGCFont(Gid_WangGe, Gid_Font); GrSetGCFont(Gid_ButtonDown, Gid_Font); GrSetGCFont(Gid_Button, Gid_Font); GrSetGCFont(Gid_Guiji, Gid_Font); GrSetGCFont(Gid_CurrentSite, Gid_Font); GrSetGCFont(Gid_Message, Gid_Font); GrSetGCFont(Gid_ButtonName, Gid_Font); GrSetGCFont(Gid_ButtonDownName, Gid_Font); } int * CM_giDrawWindows() { int i; float X_cha; float Y_cha; float Cos; float Sin; float The_jiaodu; float R_long; if(GrOpen()<0)//连接图形服务器,失败返回-1 { printf("Nano-X Server Error!\n"); printf("Please make sure your Nano-X Server is running,then retry!\n"); exit(-1);//非正常退出 } initDraw(); //绘制各窗口 WIN_Root=GrNewWindow(GR_ROOT_WINDOW_ID,0,0,320,240,0,BLACK,WHITE);//绘制根窗口 GrMapWindow(WIN_Root);//显示根窗口 WIN_Screen=GrNewWindow(WIN_Root,80,0,161,201,1,WHITE,GRAY);//绘制波形显示窗口 GrMapWindow(WIN_Screen);//显示波形显示窗口 GrPoint(WIN_Screen,Gid_Red_Red,Real_x(Current_x),Real_y(Current_y)); WIN_Massage=GrNewWindow(WIN_Root,245,20,68,40,2,WHITE,GRAY);//坐标显示窗口 GrMapWindow(WIN_Massage); GrFillRect(WIN_Massage,Gid_Button,0,0,40,20);//画填充矩形为按钮 GrText(WIN_Massage,Gid_Message,4,18,"X:",-1,GR_TFASCII); GrText(WIN_Massage,Gid_Message,4,38,"Y:",-1,GR_TFASCII); WIN_Status=GrNewWindow(WIN_Root,10,20,50,25,2,WHITE,GRAY);//状态显示窗口 GrMapWindow(WIN_Status); GrFillRect(WIN_Status,Gid_Yellow_Yellow,0,0,50,25);//画填充矩形为按钮 GrText(WIN_Status,Gid_Black_Yellow,10,18,"停止",-1,GR_TFASCII); /*在这里加入画网格的程序*/ WIN_Control=GrNewWindow(WIN_Root,0,200,320,40,2,RED,GRAY);//绘制控制面板窗口//x0,y240,320*40 GrMapWindow(WIN_Control);//显示控制面板窗口 //绘制路径设置按钮 WIN_But_SetPass=GrNewWindow(WIN_Control,Button_Start_X,Button_Start_Y,Button_Width,Button_High,Button_Borad_Width,Button_Back_Color_White,Button_Borad_Color); GrMapWindow(WIN_But_SetPass); GrText(WIN_But_SetPass,Gid_ButtonName,9,18,"设定",-1,GR_TFASCII); //绘制路径设置引发的子按钮 //画线 WIN_But_Line=GrNewWindow(WIN_Control,Button_Start_X+1*(Button_Width+Button_Between),Button_Start_Y,Button_Width,Button_High,Button_Borad_Width,Button_Back_Color_White,Button_Borad_Color);//画线 //画圆 WIN_But_Round=GrNewWindow(WIN_Control,Button_Start_X+2*(Button_Width+Button_Between),Button_Start_Y,Button_Width,Button_High,Button_Borad_Width,Button_Back_Color_White,Button_Borad_Color);//画圆 //绘制开始/暂停按钮 WIN_But_Start=GrNewWindow(WIN_Control,Button_Start_X+1*(Button_Width+Button_Between),Button_Start_Y,Button_Width,Button_High,Button_Borad_Width,Button_Back_Color_White,Button_Borad_Color); GrMapWindow(WIN_But_Start); GrText(WIN_But_Start,Gid_ButtonName,9,18,"开始",-1,GR_TFASCII); //绘制停止按钮 WIN_But_Stop=GrNewWindow(WIN_Control,Button_Start_X+2*(Button_Width+Button_Between),Button_Start_Y,Button_Width,Button_High,Button_Borad_Width,Button_Back_Color_White,Button_Borad_Color); GrMapWindow(WIN_But_Stop); GrText(WIN_But_Stop,Gid_ButtonName,9,18,"停止",-1,GR_TFASCII); GrFillRect(WIN_But_Stop,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Stop,Gid_ButtonDownName,9,18,"停止",-1,GR_TFASCII); //绘制循迹按钮 WIN_But_Pass=GrNewWindow(WIN_Control,Button_Start_X+3*(Button_Width+Button_Between),Button_Start_Y,Button_Width,Button_High,Button_Borad_Width,Button_Back_Color_White,Button_Borad_Color); GrMapWindow(WIN_But_Pass); GrText(WIN_But_Pass,Gid_ButtonName,9,18,"循迹",-1,GR_TFASCII); //结束面板的绘制 /*****************************************************************/ //为个窗口(按钮)选择事件 //控制面板 GrSelectEvents(WIN_But_SetPass,GR_EVENT_MASK_EXPOSURE | GR_EVENT_MASK_BUTTON_UP | GR_EVENT_MASK_BUTTON_DOWN); GrSelectEvents(WIN_But_Start,GR_EVENT_MASK_EXPOSURE | GR_EVENT_MASK_BUTTON_UP | GR_EVENT_MASK_BUTTON_DOWN); GrSelectEvents(WIN_But_Stop,GR_EVENT_MASK_EXPOSURE | GR_EVENT_MASK_BUTTON_UP | GR_EVENT_MASK_BUTTON_DOWN); GrSelectEvents(WIN_But_Pass,GR_EVENT_MASK_EXPOSURE | GR_EVENT_MASK_BUTTON_UP | GR_EVENT_MASK_BUTTON_DOWN); //轨迹设置面板 GrSelectEvents(WIN_But_Line,GR_EVENT_MASK_EXPOSURE | GR_EVENT_MASK_BUTTON_UP | GR_EVENT_MASK_BUTTON_DOWN); GrSelectEvents(WIN_But_Round,GR_EVENT_MASK_EXPOSURE | GR_EVENT_MASK_BUTTON_UP | GR_EVENT_MASK_BUTTON_DOWN); //画图区 GrSelectEvents(WIN_Screen,GR_EVENT_MASK_EXPOSURE | GR_EVENT_MASK_BUTTON_DOWN | GR_EVENT_MASK_MOUSE_POSITION); //------------------------------------------------------------------------------------------------------------------------- //pthread_mutex_lock(&lock);//锁住互斥锁 //创建时钟 //timerId=GrCreateTimer(WIN_Screen,100,GR_TRUE); //开始捕获事件的循环 while(1) { GrGetNextEventTimeout(&Event,30);//获取下一个事件 if(Event.type == GR_EVENT_TYPE_BUTTON_DOWN)//控制面板各个按钮的按下事件 { if(Event.button.wid==WIN_But_SetPass) { if(Mark_SetButStatus == SAVE) { GrFillRect(WIN_But_SetPass,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_SetPass,Gid_ButtonDownName,9,18,"保存",-1,GR_TFASCII); } if(Mark_SetButStatus == SET) { GrFillRect(WIN_But_SetPass,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_SetPass,Gid_ButtonDownName,9,18,"设定",-1,GR_TFASCII); } } if(Event.button.wid==WIN_But_Pass) { GrFillRect(WIN_But_Pass,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Pass,Gid_ButtonDownName,9,18,"循迹",-1,GR_TFASCII); } if(Event.button.wid==WIN_But_Start) { GrFillRect(WIN_But_Start,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Start,Gid_ButtonDownName,9,18,"开始",-1,GR_TFASCII); } if(Event.button.wid==WIN_But_Stop) { GrFillRect(WIN_But_Stop,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Stop,Gid_ButtonDownName,9,18,"停止",-1,GR_TFASCII); } if(Event.button.wid==WIN_But_Line) { GrFillRect(WIN_But_Line,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Line,Gid_ButtonDownName,9,18,"画线",-1,GR_TFASCII); } if(Event.button.wid==WIN_But_Round) { GrFillRect(WIN_But_Round,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Round,Gid_ButtonDownName,9,18,"画圆",-1,GR_TFASCII); } if(Event.button.wid==WIN_Screen) { if(Mark_DrawButStatus == LINE) { arry_x[0]='X'; arry_x[1]=':'; arry_x[2]=((Event.button.x)/2)/100+'0'; arry_x[3]=(((Event.button.x)/2)%100)/10+'0'; arry_x[4]=((Event.button.x)/2)%10+'0'; arry_x[5]='C'; arry_x[6]='M'; arry_x[7]='\0'; arry_y[0]='Y'; arry_y[1]=':'; arry_y[2]=((200-Event.button.y)/2)/100+'0'; arry_y[3]=(((200-Event.button.y)/2)%100)/10+'0'; arry_y[4]=((200-Event.button.y)/2)%10+'0'; arry_y[5]='C'; arry_y[6]='M'; arry_y[7]='\0'; GrClearWindow(WIN_Massage,GR_FALSE); GrFillRect(WIN_Massage,Gid_Button,0,0,60,20);//画填充矩形为按钮 GrText(WIN_Massage,Gid_Message,4,18,arry_x,-1,GR_TFASCII); GrText(WIN_Massage,Gid_Message,4,38,arry_y,-1,GR_TFASCII); if(X_sit < 0) { X_sit = Event.button.x; Y_sit = Event.button.y; QU_giReInitQueue();//清空存储区 QU_giSetDataIntoQueue(X_sit+(200-Y_sit)*1000);//点存到缓冲区 } else { GrLine(WIN_Screen,Gid_Guiji,X_sit,Y_sit,Event.button.x,Event.button.y); numberOfPoint = (sqrt((double)((Event.button.x-X_sit)*(Event.button.x-X_sit)+((Event.button.y)-Y_sit)*((Event.button.y)-Y_sit))))/2; X_cha = Event.button.x-X_sit; Y_cha = -((Event.button.y)-Y_sit); if((X_cha == 0)&&(Y_cha > 0)) { The_jiaodu = 90*0.0174533; } if((X_cha == 0)&&(Y_cha < 0)) { The_jiaodu = 270*0.0174533; } if((X_cha > 0)&&(Y_cha == 0)) { The_jiaodu = 0*0.0174533; } if((X_cha < 0)&&(Y_cha == 0)) { The_jiaodu = 180*0.0174533; } if((X_cha > 0)&&(Y_cha > 0)) { The_jiaodu = atan(Y_cha/X_cha); } if((X_cha < 0)&&(Y_cha > 0)) { The_jiaodu = 180*0.0174533+atan(Y_cha/X_cha); } if((X_cha < 0)&&(Y_cha < 0)) { The_jiaodu = 180*0.0174533+atan(Y_cha/X_cha); } if((X_cha > 0)&&(Y_cha < 0)) { The_jiaodu = 360*0.0174533+atan(Y_cha/X_cha); } Cos = cos(The_jiaodu); Sin = sin(The_jiaodu); for(i=0;i = 0) { CM_gvGoTo(X_Y_pos); Current_x = X_Y_pos%1000; Current_y = X_Y_pos/1000; if(Mark_SetButStatus == SET) { arry_x[0]='X'; arry_x[1]=':'; arry_x[2]=((Current_x)/2)/100+'0'; arry_x[3]=(((Current_x)/2)%100)/10+'0'; arry_x[4]=((Current_x)/2)%10+'0'; arry_x[5]='C'; arry_x[6]='M'; arry_x[7]='\0'; arry_y[0]='Y'; arry_y[1]=':'; arry_y[2]=((Current_y)/2)/100+'0'; arry_y[3]=(((Current_y)/2)%100)/10+'0'; arry_y[4]=((Current_y)/2)%10+'0'; arry_y[5]='C'; arry_y[6]='M'; arry_y[7]='\0'; GrClearWindow(WIN_Massage,GR_FALSE); GrFillRect(WIN_Massage,Gid_Button,0,0,60,20);//画填充矩形为按钮 GrText(WIN_Massage,Gid_Message,4,18,arry_x,-1,GR_TFASCII); GrText(WIN_Massage,Gid_Message,4,38,arry_y,-1,GR_TFASCII); GrEllipse(WIN_Screen,Gid_Red_Red,Real_x(Current_x),Real_y(Current_y),1,1); } Mark_OBStatus = START; } } if(Mark_OBStatus == ARRIVED) { GrClearWindow(WIN_Status,GR_FALSE); GrFillRect(WIN_Status,Gid_Red_Red,0,0,50,25);//画填充矩形为按钮 GrText(WIN_Status,Gid_Black_Red,5,18,"已完成",-1,GR_TFASCII); GrClearWindow(WIN_But_Start,GR_FALSE); GrFillRect(WIN_But_Start,Gid_Button,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Start,Gid_ButtonName,9,18,"开始",-1,GR_TFASCII); GrClearWindow(WIN_But_Stop,GR_FALSE); GrFillRect(WIN_But_Stop,Gid_ButtonDown,0,0,Button_Width,Button_High);//画填充矩形为按钮 GrText(WIN_But_Stop,Gid_ButtonDownName,9,18,"停止",-1,GR_TFASCII); Mark_OBStatus = STOP; } } } void CM_gvGoTo(int X_Y_pos) { int New_x; int New_y; int Left_steps; int Right_steps; int Left_direction; int Right_direction; float Left_length; float Right_length; New_x = X_Y_pos%1000; New_y = X_Y_pos/1000; Left_length = sqrt((New_x/2+15)*(New_x/2+15)+(115-New_y/2)*(115-New_y/2))- sqrt((Current_x/2+15)*(Current_x/2+15)+(115-Current_y/2)*(115-Current_y/2)); Right_length = sqrt((95-New_x/2)*(95-New_x/2)+(115-New_y)*(115-New_y))- sqrt((95-Current_x/2)*(95-Current_x/2)+(115-Current_y/2)*(115-Current_y/2)); Left_steps = abs(Left_length/STEP_LENTGTH); Right_steps = abs(Right_length/STEP_LENTGTH); if(Left_length >= 0) { Left_direction =FORWARD; } else { Left_direction =BACKWARD; } if(Right_length >= 0) { Right_direction =FORWARD; } else { Right_direction =BACKWARD; } if(Left_steps>=Right_steps) { if((Left_direction == BACKWARD)&&(Right_direction == BACKWARD)) { while(Right_steps) { EXIO = ALL_BACKWARD; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; Right_steps--; } while(Left_steps) { EXIO = LEFT_BACKWARD; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; } } else if((Left_direction == FORWARD)&&(Right_direction == FORWARD)) { while(Right_steps) { EXIO = ALL_FORWARD; EXIO = ALL_DOWN; Left_steps--; Right_steps--; } while(Left_steps) { EXIO = LEFT_FORWARD; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; } } else if((Left_direction == BACKWARD)&&(Right_direction == FORWARD)) { while(Right_steps) { EXIO = LB_RF; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; Right_steps--; } while(Left_steps) { EXIO = LEFT_BACKWARD; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; } } else if((Left_direction == FORWARD)&&(Right_direction == BACKWARD)) { while(Right_steps) { EXIO = LF_RB; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; Right_steps--; } while(Left_steps) { EXIO = LEFT_FORWARD; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; } } } else { if((Left_direction == BACKWARD)&&(Right_direction == BACKWARD)) { while(Left_steps) { EXIO = ALL_BACKWARD; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; Right_steps--; } while(Right_steps) { EXIO = RIGHT_BACKWARD; delay(); EXIO = ALL_DOWN; delay(); Right_steps--; } } else if((Left_direction == FORWARD)&&(Right_direction == FORWARD)) { while(Left_steps) { EXIO = ALL_FORWARD; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; Right_steps--; } while(Right_steps) { EXIO = RIGHT_FORWARD; delay(); EXIO = ALL_DOWN; delay(); Right_steps--; } } else if((Left_direction == BACKWARD)&&(Right_direction == FORWARD)) { while(Left_steps) { EXIO = LB_RF; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; Right_steps--; } while(Right_steps) { EXIO = RIGHT_FORWARD; delay(); EXIO = ALL_DOWN; delay(); Right_steps--; } } else if((Left_direction == FORWARD)&&(Right_direction == BACKWARD)) { while(Left_steps) { EXIO = LF_RB; delay(); EXIO = ALL_DOWN; delay(); Left_steps--; Right_steps--; } while(Right_steps) { EXIO = RIGHT_BACKWARD; delay(); EXIO = ALL_DOWN; delay(); Right_steps--; } } } } void delay() { int i; for(i=0;i >>>>>>The queue is full!\n"); return(-1); } } int QU_niDelQueue(void) { int i; if(!QU_niIsEmptyQueue()) { if(1 == astQueue.rearSign) { astQueue.rearSign = 0; i = (*astQueue.rear).data; free(astQueue.rear); astQueue.rear = (*astQueue.rear).next; astQueue.length--; astQueue.rearSign = 1; return (i); } else { return(-1); } } else { printf("Error !>>>>>>>It is an Empty queue!\n"); return(-1); } } int QU_niIsFullQueue(void) { if(MAX_QUEUE_LENGTH == astQueue.length) return (1); else return (0); }