face.cpp

课题研究用的直线型一级倒立摆的源程序

			break;

		case LEFT:  /*稳摆后左移*/
			if ((start == 5) && (pend == 1))
				offset += 0.1;
			break;

		case RIGHT: /*稳摆后右移*/
			if ((start == 5) && (pend == 1))
				offset -= 0.1;
			break;

		case UP_T:
		case LOW_T:
			if (safety == 0){
				safety = 1;
				start = 0;
				motion = 0;
				pend = 0;
				offset = 0;
				vel = 0;
				acc = 0;
				param_info[3].param_value[0] = 0;
			}
			break;

		case SPACE:     /* 急停按扭 */
			safety = ERR_1;
			break;

		default:
			break;

	}
	return key;
}

int handle_alarm(int key)
{
	switch (key)
	{
		case RETURN:
			act = PANEL;
			restore_screen();

			if (button == 0){
				first_swingup = 1;
				mutex_event = 1;
				delay(10);
				GT_AxisOn();
				offset = 0;
				acc = 0;
				vel = 0;
				pend = 0;
				pos = pos0 = 0;
				angle = angle0 = 0;
				posDot = angleDot = 0;
				GT_Axis(1);
				GT_Update();
				mutex_event = 0;
				start = 1;
			}
			else
				button = 0;

			break;

		case ESC:
			act = PANEL;
			restore_screen();

			button = 0;
			break;

		case TAB:
			if (button == 0)
				button = 1;
			else
				button = 0;

			display_button();

			break;

		default:
			break;

	}
	return key;
}


void display_alarm()
{
	setcolor(LIGHTGRAY);
	setfillstyle(SOLID_FILL, LIGHTGRAY);
	bar(rect.left, rect.top, rect.right, rect.bottom);

	setcolor(BLACK);
	setlinestyle(SOLID_LINE, 0, NORM_WIDTH);
	rectangle(rect.left+10, rect.top+10, rect.right-10, rect.bottom-10);

	WriteHz(rect.left+30, rect.top+20, 1, 1, 1, RED, "请您确认");
	WriteHz(rect.left+20, rect.top+40, 1, 1, 1, RED, "小车停在中间");
	WriteHz(rect.left+20, rect.top+60, 1, 1, 1, RED, "摆杆静止下垂");
	
	display_button();
}

void display_curve()
{
	int i;

	setcolor(LIGHTGRAY);
	setfillstyle(SOLID_FILL, LIGHTGRAY);
	bar(rect.left, rect.top, rect.right, rect.bottom);

	setcolor(BLACK);
	setfillstyle(SOLID_FILL, BLACK);
	bar(rect.left+10, rect.top+10, rect.right-10, rect.bottom - 90);
	rectangle(rect.left+10, rect.bottom - 80, rect.right - 10, rect.bottom - 10);

	setcolor(WHITE);
	line(rect.left + 30, rect.bottom - 110, rect.right - 10, rect.bottom - 110);
	line(rect.right - 10, rect.bottom - 110, rect.right - 15, rect.bottom - 112);
	line(rect.right - 10, rect.bottom - 110, rect.right - 15, rect.bottom - 108);

	line(rect.left + 30, rect.top + 10, rect.left + 30, rect.bottom - 110);
	line(rect.left + 30, rect.top + 10, rect.left + 28, rect.top + 15);
	line(rect.left + 30, rect.top + 10, rect.left + 32, rect.top + 15);

	settextstyle(DEFAULT_FONT, HORIZ_DIR, 1);
	outtextxy(rect.right - 20, rect.bottom - 105, "s");

	if (curve_id == 0){
		WriteHz(rect.right - 50, rect.top + 10, 1, 1, 1, WHITE, "小车");
		outtextxy(rect.left + 17, rect.top + 10, "m");

		for (i=0; i<CURVE_POINT; i++)
			putpixel(rect.left + 30 + i / 5, rect.top + 90 - respond[i].point_pos * 75.0 / respond_max_pos, WHITE);
	}
	else{
		WriteHz(rect.right - 50, rect.top + 10, 1, 1, 1, WHITE, "摆杆");
		outtextxy(rect.left + 17, rect.top + 10, "c");

		for (i=0; i<CURVE_POINT; i++)
			putpixel(rect.left + 30 + i / 5, rect.top + 90 - respond[i].point_the * 75.0 / respond_max_the, WHITE);
	}

	char unit_coor[3];

	for (i = 1; i< 10; i++){
		sprintf(unit_coor, "%d", abs(5 - i));
		if (i > 5)
			outtextxy(rect.left + 15, rect.top + 10 + i * 16, "-");
			
		outtextxy(rect.left + 20, rect.top + 10 + i * 16, unit_coor);
		line(rect.left + 30, rect.top + 10 + i * 16, rect.left + 32, rect.top + 10 + i * 16);

		sprintf(unit_coor, "%d", i);
		outtextxy(rect.left + 30 + i * 21 , rect.bottom - 105, unit_coor);
		line(rect.left + 30 + i * 21, rect.bottom - 110, rect.left + 30 + i * 21, rect.bottom -112);

	}

	setlinestyle(DOTTED_LINE, 0, NORM_WIDTH);
	line(rect.left + 30, rect.top + 90, rect.right - 10, rect.top + 90);
	setlinestyle(SOLID_LINE, 0, NORM_WIDTH);

	WriteHz(rect.left + 25, rect.bottom - 68, 1, 1, 1, BLACK, "文件名");

	setcolor(BLUE);
	setfillstyle(SOLID_FILL, BLUE);
	bar(rect.left + 80, rect.bottom - 70, rect.right - 20,  rect.bottom - 50);

	display_button();
}

void display_curve_char()
{
		setcolor(BLUE);
		setfillstyle(SOLID_FILL, BLUE);
		bar(rect.left + 80, rect.bottom - 70, rect.right - 20,  rect.bottom - 50);

		settextstyle(DEFAULT_FONT, HORIZ_DIR, 1);
		setcolor(WHITE);
		outtextxy(rect.left + 80,
				rect.bottom - 64, curve_info[curve_id].ch);
}

int handle_curve(int key)
{
	char char_key = (char) key;
	char c[20];

	int cur = curve_info[curve_id].cur;
	int num = curve_info[curve_id].num;

	if ((((char_key >= 'a') && (char_key <= 'z')) ||  ((char_key >= 'A') && (char_key <= 'Z'))) && (num < 8))
	{
		sprintf(c,"%c", char_key);
		strcpy(c+1, curve_info[curve_id].ch + cur);
		strcpy(curve_info[curve_id].ch + cur, c);

		display_curve_char();

		setcolor(BLUE);
		line(rect.left + 80 + cur * 8,
			rect.bottom - 52,
			rect.left + 80 + (cur + 1) * 8,
			rect.bottom - 52);
		curve_info[curve_id].num ++;
		curve_info[curve_id].cur ++;
	}
		gettime(&second);
		int t = second.ti_hund - first.ti_hund;
		if ((t >= 40) || ((t<0) && (t+100)>=40))
		{
			first.ti_hund = second.ti_hund;
			setwritemode(1);
			setcolor(WHITE);
			line(rect.left + 80 + cur * 8,
				rect.bottom - 52,
				rect.left + 80 + (cur + 1) * 8,
				rect.bottom - 52);
			setwritemode(0);
		}

	
	switch(key){

	case RETURN:
		act = PANEL;
		restore_screen();
		write_param_file();
		curve_info[curve_id].cur = 0;
		curve_info[curve_id].num = 0;
		curve_info[curve_id].ch[0] = '\0';
		break;

	case ESC:
		act = PANEL;
		restore_screen();
		curve_info[curve_id].cur = 0;
		curve_info[curve_id].num = 0;
		curve_info[curve_id].ch[0] = '\0';
		break;

	case TAB:
		if (button == 0)
			button = 1;
		else
			button = 0;
		display_button();
		break;

	case BACK:
		if (cur > 0){
			strcpy(curve_info[curve_id].ch + cur - 1,
				curve_info[curve_id].ch + cur);
			display_curve_char();
			curve_info[curve_id].num --;
			curve_info[curve_id].cur --;
		}
		break;

	case DEL:
		if (cur < num){
			strcpy(curve_info[curve_id].ch + cur,
				curve_info[curve_id].ch + cur + 1);
			curve_info[curve_id].num --;
			display_curve_char();
		}
		break;

	case LEFT:
		if (cur > 0){
			setcolor(BLUE);
			line(rect.left + 80 + cur * 8,
				rect.bottom - 52,
				rect.left + 80 + (cur + 1) * 8,
				rect.bottom - 52);
			curve_info[curve_id].cur --;
		}
		break;

	case RIGHT:
		if (cur < num){
			setcolor(BLUE);
			line(rect.left + 80 + cur * 8,
				rect.bottom - 52,
				rect.left + 80 + (cur + 1) * 8,
				rect.bottom - 52);
			curve_info[curve_id].cur ++;
		}
		break;
	}
	return 0;
}

void display_param_char()
{
	int x = rect.left+60, y = rect.top+39;
	int sel = param_info[param_id].param_sel;

	setcolor(BLUE);
	setfillstyle(SOLID_FILL,BLUE);
	bar(x, y - 15 + sel * 20, x + PARAM_LENGTH - 80,  y + 1 + sel * 20);

	settextstyle(DEFAULT_FONT, HORIZ_DIR, 1);
	setcolor(WHITE);
	outtextxy(x, y + sel * 20 - 10, param_info[param_id].param_str[sel]);

}

int handle_param(int key)
{
	char char_key;

	int sel = param_info[param_id].param_sel;
	int num = param_info[param_id].char_num[sel];
	int cur = param_info[param_id].cur_pos; 
	char c[10]="";

	int x = rect.left+60, y = rect.top+39;
	char_key = (char) key;

	if (((char_key >= '0') && (char_key <= '9') && (num < 7)) || (char_key == '.') || (char_key == '-')){
		sprintf(c,"%c", char_key);
		strcpy(c+1, param_info[param_id].param_str[sel] + cur);
		strcpy(param_info[param_id].param_str[sel] + cur, c);

		display_param_char();
		setcolor(BLUE);
		line(x + cur * 8, y + sel * 20, x + (cur + 1) * 8, y + sel * 20);
		param_info[param_id].char_num[sel] ++;
		param_info[param_id].cur_pos ++;
	}
		gettime(&second);
		int t = second.ti_hund - first.ti_hund;
		if ((t >= 40) || ((t<0) && (t+100)>=40)){
			first.ti_hund = second.ti_hund;
			setwritemode(1);
			setcolor(WHITE);
			line(x + cur * 8, y + sel * 20, x + cur * 8 + 6, y + sel * 20);
			setwritemode(0);
		}

	switch (key)
	{
		case RETURN:
			act = PANEL;
			restore_screen();

			if (button == 0){
				save_temp_param();
				if (param_id < 3)
					refresh_param();

			
				switch (param_id){
					case 0:
						ctrl_mode = 0;
						break;

					case 1:
						ctrl_mode = 1;
						break;

					case 2:
						ctrl_mode = 2;
						break;
					}
			
				if (respond_start == 0){
					respond_max_pos = 0.00001;
					respond_max_the = 0.00001;
					respond_start = 1;
				}
		        }
			button = 0;
			param_info[param_id].param_sel = 0;
			break;
			
		case ESC:
			act = 0;
			restore_screen();
			button = 0;
			param_info[param_id].param_sel = 0;
			break;

		case UP:
			setcolor(BLUE);
			line(x + cur * 8, y + sel * 20, x + cur * 8 + 6, y + sel * 20);
			if (sel > 0){
				param_info[param_id].param_sel --;
				param_info[param_id].cur_pos = 0;
			}
			break;

		case DOWN:
			setcolor(BLUE);
			line(x + cur * 8, y + sel * 20, x + cur * 8 + 6, y + sel * 20);

			if (sel < param_info[param_id].param_count - 1){
				param_info[param_id].param_sel ++;
				param_info[param_id].cur_pos = 0;
			}
			break;

		case TAB:
			if (button == 0)
				button = 1;
			else
				button = 0;
			display_button();
			break;

		case BACK:
			if (cur > 0){
				strcpy(param_info[param_id].param_str[sel] + cur - 1,
					param_info[param_id].param_str[sel] + cur);
				display_param_char();
				param_info[param_id].char_num[sel] --;
				param_info[param_id].cur_pos --;
			}
			break;

		case DEL:
			if (cur < num){
				strcpy(param_info[param_id].param_str[sel] + cur,
					param_info[param_id].param_str[sel] + cur + 1);
				param_info[param_id].char_num[sel] --;
				display_param_char();
			}
			break;

		case LEFT:
			if (cur > 0){
				setcolor(BLUE);
				line(x + cur * 8, y + sel * 20, x + (cur + 1) * 8, y + sel * 20);
				param_info[param_id].cur_pos --;
			}
			break;


		case RIGHT:
			if (cur < num){
				setcolor(BLUE);
				line(x + cur * 8, y + sel * 20, x + (cur + 1) * 8, y + sel * 20);
				param_info[param_id].cur_pos ++;
			}
			break;
	}
	return 0;
}

void restore_temp_param()
{

	int i;
	for (i=0; i<param_info[param_id].param_count; i++){
		if ((param_id == MAX_MENU) && (i == 0))
			sprintf(param_info[param_id].param_str[i], "%d", int(param_info[param_id].param_value[i]));
		else
			sprintf(param_info[param_id].param_str[i], "%.2f", param_info[param_id].param_value[i]);
		param_info[param_id].char_num[i] = strlen(param_info[param_id].param_str[i]);
	}
	param_info[param_id].cur_pos = 0;
}

void save_temp_param()
{
	int i;
	for (i=0; i<param_info[param_id].param_count; i++)
	{
	  param_info[param_id].param_value[i]= atof(param_info[param_id].param_str[i]);
	}
}

void refresh_param()
{
	int i;
	setcolor(BLUE);
	setfillstyle(SOLID_FILL, BLUE);
	bar(20, 90, 117, 140);
	bar(8, 200, 117, 400);
	WriteHz(20, 90, 1, 1, 2, WHITE, menu_info[menu_id].menu_desc[param_id]);
	settextstyle(TRIPLEX_FONT, HORIZ_DIR, 1);
	setcolor(WHITE);
	for (i=0; i<param_info[param_id].param_count; i++){
		outtextxy(8, 200 + i * 20, param_info[param_id].param_desc[i]);
		outtextxy(40, 200 + i * 20, param_info[param_id].param_str[i]);
	}
}

void write_param_file()
{
	FILE* param;
	int i, j, data_length;
	char curve_data[20];

	if ((param=fopen(curve_info[curve_id].ch,"w+t")) != NULL){
		for(i=0; i<CURVE_POINT; i++){
			if (curve_id == 0)
				sprintf(curve_data, "%.2f", respond[i].point_pos);
			else
				sprintf(curve_data, "%.2f", respond[i].point_the);
			data_length = strlen(curve_data);
			for (j=0; j<data_length; j++)
				fputc(curve_data[j], param);
			fputc(' ', param);
		}
	fclose(param);
	}
}

void reset_car()
{
	safety = 1;
	start = 0;
	motion = 0;
	pend = 0;
	offset = 0;
	vel = 0;
	acc = 0;

	clearviewport();
	WriteHz(200, 200, 2, 2, 1, RED, "小车正在复位");
	WriteHz(250, 300, 1, 1, 1, WHITE, "请稍候．．．");

	while ( safety != 0);
	delay(10000);
	clearviewport();
	WriteHz(120, 200, 2, 2, 1, RED, "您可以安全的退出系统了");
	WriteHz(220, 400, 1, 1, 1, WHITE, "按任意键退出程序");
	getch();

}

void handle()
{
	int key;

	for (;;)
	{
		if (bioskey(1))
			key = bioskey(0);
		if (handle_pop[act](key) == ALT_X){
			if (draw_mode == 1)
				free(image);
			if (start > 0)
				reset_car();
			  	GT_AxisOff();
			return;
		}
		key = 0;

		if (safety == ERR_1){
			if (draw_mode == 1)
				free(image);

			clearviewport();
			WriteHz(60, 140, 4, 4, 2, RED, "急停按钮被触发");
			WriteHz(60, 240, 4, 4, 2, RED, "无法继续控制系统");
			WriteHz(250, 400, 1, 1, 1, WHITE, "按任意键退出程序");
			getch();
			return;
		}

		if (safety == ERR_2){
			if (draw_mode == 1)
				free(image);

			clearviewport();
			WriteHz(180, 140, 4, 4, 2, RED, "小车失速");
			WriteHz(160, 240, 4, 4, 2, RED, "系统被终止");
			WriteHz(250, 400, 1, 1, 1, WHITE, "按任意键退出程序");
			getch();
			return;
		}

		if (times == TIME_ONE){
			if ((draw_mode == 0) && (act == 0))
				draw_curve();


		if (respond_start == 1){
			respond[respond_index].point_the = (angle * 180 / M_PI  - 180);
			respond[respond_index].point_pos = pos;
			respond_max_pos = max(fabs(respond[respond_index].point_pos), respond_max_pos);
			respond_max_the = max(fabs(respond[respond_index].point_the), respond_max_the);

			respond_index ++;
			if (respond_index >= CURVE_POINT){
				respond_index = 0;
				respond_start = 0;
			}
		}
			times = 0;
		}
		if (times_draw == TIME_TWO){
			times_draw = 0;
			draw_text();
			if ((draw_mode == 1) && (act == 0))
				draw_pendulum();
		}
	}
}

float max(float value1, float value2)
{
   return ( (value1 > value2) ? value1 : value2);
}