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);
}