📄 face.cpp
字号:
/*************************************************
直线一级倒立摆
Linear Single Inverted Pendulum
**************************************************/
#include <graphics.h>
#include <stdio.h>
#include <conio.h>
#include <stdlib.h>
#include <bios.h>
#include <string.h>
#include <dos.h>
#include "userlib.h"
#include "sv_pci.h"
#include "ctrl.h"
#include "chinese.h"
#include "draw.h"
#include "face.h"
extern int times, times_draw;
extern double pos;
extern double angle;
extern double pos0;
extern double angle0;
extern double angleDot;
extern double posDot;
extern double vel;
extern double acc;
extern int safety;
extern int pend, start, motion;
extern float offset;
extern double v;
extern double ang_2pi;
float respond_max_pos = 0.00001;
float respond_max_the = 0.00001;
int respond_start = 0; // 记录响应曲线开始标志
int respond_index = 0;
RESPOND_CURVE respond[CURVE_POINT];
extern DRAW_POINT dp[];
PopFunc handle_pop[] = {handle_panel, handle_menu, handle_param, handle_curve, handle_alarm}; //, handle_mode, handle_param};
char *menu_file[] = {"ＬＱＲ", "ＰＩＤ", "传递函数"};
char *menu_sign[] = {"扰动信号", "小车响应", "摆杆响应"};
char *param_lqr_desc[] = {"Kx", "Kx'", "Ka", "Ka'"};
char *param_pid_desc[] = {"KP", "KI", "KD"};
char *param_root_desc[] = {"b0", "b1", "b2", "b3", "b4", "a1", "a2", "a3", "a4"};
char *param_sign_desc[] = {"波形", "频率", "幅值"};
char *param_lqr_str[] = {" ", " ", " ", " "};
char *param_pid_str[] = {" ", " ", " "};
char *param_root_str[] = {" ", " ", " ", " ", " "," ", " ", " ", " "};
char *param_sign_str[] = {" ", " ", " "};
int param_lqr_num[] = {0, 0, 0, 0};
int param_pid_num[] = {0, 0, 0};
int param_root_num[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int param_sign_num[] = {0, 0, 0};
////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////控制器参数///////////////////////////////////////////////
float param_lqr_value[] = {-31.62, -20.95, 77.8, 11.76}; //LQR控制参数
float param_pid_value[] = {100,8,40}; //PID控制参数
float param_root_value[] = {270.15, -53.1, 260.85, 0, 0, 1, -1.7788, 0.7788, 0};//传递函数参数
////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////
float param_sign_value[] = {0, 0, 0};
MENU_INFO menu_info[2];
PARAM_INFO param_info[4];
CUR curve_info[2];
void *image;
void *buf;
unsigned size;
int fre_time = 0;
int mutex_event = 0; //稳摆标志位
int first_swingup = 0;
int act = 0;
int button = 0;
int menu_id = 0, param_id = 0, curve_id = 0;
int draw_mode = 0, ctrl_mode = 0;
double zoom_the = 1.0/128, zoom_thedot = 1.0/256, zoom_pos = 16.0, zoom_posdot = 8.0;
RECT rect;
struct time first, second;
void init_graph() /* 初始化图形库模式 */
{
int gdriver = DETECT, gmode,errorcode;
initgraph(&gdriver, &gmode, "");
errorcode = graphresult();
if(errorcode != grOk)
{
printf("Graphics error: %s\n", grapherrormsg(graphresult()));
printf("Press any key to halt.");
getch();
exit(1);
}
}
void init_bar() /*初始化工具条*/
{
setcolor(CYAN);
setfillstyle(SOLID_FILL, CYAN);
bar(0, 0, TITLE_LENGTH, TITLE_HEIGHT);
WriteHz(200, 0, 1, 1, 1, BLACK, "固高科技--直线一级倒立摆");
WriteHz(500, 0, 1, 1, 1, BLACK, "版本：１. １０");
setcolor(7);
setfillstyle(SOLID_FILL, 7);
bar(0, TITLE_HEIGHT, TITLE_LENGTH, TITLE_HEIGHT * 2);
WriteHz(10, 20, 1, 1, 2, BLACK, "控制器");
WriteHz(110, 20, 1, 1, 2, BLACK, "扰动");
WriteHz(190, 20, 1, 1, 2, BLACK, "退出");
setcolor(BLACK);
settextstyle(SANS_SERIF_FONT, HORIZ_DIR, 0);
setusercharsize(1, 2, 1, 2);
outtextxy(64, 17, "(F)");
outtextxy(148, 17, "(D)");
outtextxy(228, 17, "(X)");
setcolor(BLUE);
setfillstyle(SOLID_FILL, BLUE);
bar(0, 40, 639, 479);
}
void display_panel() //显示界面面板
{
int i;
setcolor(WHITE);
settextstyle(TRIPLEX_FONT, HORIZ_DIR, 1);
WriteHz(20, 60, 1, 1, 2, WHITE, "控制算法：");
WriteHz(10, 180, 1, 1, 2, WHITE, "控制参数：");
outtextxy(18, 200, "K1 =");
outtextxy(18, 220, "K2 =");
outtextxy(18, 240, "K3 =");
outtextxy(18, 260, "K4 =");
WriteHz(20, 425, 1, 1, 4, LIGHTRED, "急停按");
WriteHz(20, 445, 1, 1, 4, LIGHTRED, "空格键");
setcolor(LIGHTGRAY);
setlinestyle(SOLID_LINE, 0, NORM_WIDTH);
line(0, 160, 120, 160);
line(0, 405, 120, 405);
restore_temp_param();
refresh_param();
WriteHz(135, 60, 1, 1, 2, WHITE, "摆杆偏角：");
WriteHz(135, 90, 1, 1, 2, WHITE, "小车偏移：");
WriteHz(300, 60, 1, 1, 2, WHITE, "度");
WriteHz(300, 90, 1, 1, 2, WHITE, "毫米");
WriteHz(365, 60, 1, 1, 2, WHITE, "摆杆角速度：");
WriteHz(365, 90, 1, 1, 2, WHITE, "小车线速度：");
WriteHz(560, 60, 1, 1, 2, WHITE, "度／秒");
WriteHz(560, 90, 1, 1, 2, WHITE, "毫米／秒");
setcolor(YELLOW);
outtextxy(232, 56, "0");
outtextxy(232, 86, "0");
outtextxy(482, 56, "0");
outtextxy(482, 86, "0");
WriteHz(160, 440, 1, 1, 2, WHITE, "启动Ｓ 停止Ｔ 起摆↑ 左移← 右移→ 模式切换Ｃ");
setcolor(LIGHTGRAY);
setlinestyle(SOLID_LINE, 0, THICK_WIDTH);
line(120, 40, 120, 479);
line(120, 120, 639, 120);
line(120, 420, 639, 420);
draw_curve_panel();/*画曲线面板*/
}
int handle_menu(int key) /*程序菜单人机交互命令处理函数*/
{
int param_count;
param_id = menu_id * MAX_MENU + menu_info[menu_id].menu_sel;
switch (key)
{
case RETURN:
restore_screen();
menu_info[menu_id].menu_sel = 0;
switch (param_id)
{
case 0:
case 1:
case 2:
case 3:
act = PARAM;
param_count = param_info[param_id].param_count;
rect.left = (MAXX - PARAM_LENGTH) / 2;
rect.right = rect.left + PARAM_LENGTH;
rect.top = (MAXY - param_count * PARAM_HEIGHT) / 2;
rect.bottom = rect.top + param_count * PARAM_HEIGHT + 80;
save_screen();
display_param();
break;
case 4:
if (respond_start == 0){
act = CURVE;
curve_id = 0;
rect.left = (MAXX - CURVE_LENGTH) / 2;
rect.right = rect.left + CURVE_LENGTH;
rect.top = (MAXY - CURVE_HEIGHT) / 2;
rect.bottom = rect.top + CURVE_HEIGHT + 80;
save_screen();
display_curve();
}
else{
act = PANEL;
menu_info[menu_id].menu_sel = 0;
}
break;
case 5:
if (respond_start == 0)
{
act = CURVE;
curve_id = 1;
rect.left = (MAXX - CURVE_LENGTH) / 2;
rect.right = rect.left + CURVE_LENGTH;
rect.top = (MAXY - CURVE_HEIGHT) / 2;
rect.bottom = rect.top + CURVE_HEIGHT + 80;
save_screen();
display_curve();
}
else
{
act = PANEL;
menu_info[menu_id].menu_sel = 0;
}
break;
}
break;
case ESC:
act = PANEL;
restore_screen();
menu_info[menu_id].menu_sel = 0;
break;
case UP:
menu_info[menu_id].menu_sel --;
display_menu();
break;
case DOWN:
menu_info[menu_id].menu_sel ++;
display_menu();
break;
case RIGHT:
case LEFT:
restore_screen();
menu_info[menu_id].menu_sel = 0;
if (menu_id == 0)
menu_id = 1;
else
menu_id = 0;
rect.left = menu_id * MENU_LENGTH;
rect.right = menu_id * MENU_LENGTH + MENU_LENGTH;
rect.top = TITLE_HEIGHT * 2;
rect.bottom = TITLE_HEIGHT * 2 + menu_info[menu_id].menu_count * MENU_HEIGHT + 10;
save_screen();
display_menu();
break;
}
return 0;
}
int save_screen()
{
size = imagesize(rect.left, rect.top, rect.right, rect.bottom);
buf = malloc(size);
if (buf == NULL)
return 0;
getimage(rect.left, rect.top, rect.right, rect.bottom, buf);
return 1;
}
void display_menu() /*显示图形用户菜单*/
{
int i;
int menu_count = menu_info[menu_id].menu_count;
if (menu_info[menu_id].menu_sel >= menu_count)
menu_info[menu_id].menu_sel = 0;
else if (menu_info[menu_id].menu_sel < 0)
menu_info[menu_id].menu_sel = menu_count - 1;
setcolor(LIGHTGRAY);
setfillstyle(SOLID_FILL, LIGHTGRAY);
bar(menu_id * MENU_LENGTH,
TITLE_HEIGHT * 2,
(menu_id + 1) * MENU_LENGTH,
TITLE_HEIGHT * 2 + menu_count * MENU_HEIGHT + 10);
setcolor(BLACK);
setlinestyle(SOLID_LINE, 0, NORM_WIDTH);
rectangle(menu_id * MENU_LENGTH + 5,
TITLE_HEIGHT * 2 + 5,
(menu_id + 1) * MENU_LENGTH - 5,
TITLE_HEIGHT * 2 + menu_count * MENU_HEIGHT + 5);
for (i=1; i < menu_count; i++)
line(menu_id * MENU_LENGTH + 5,
TITLE_HEIGHT * 2 + i * MENU_HEIGHT + 5,
(menu_id + 1) * MENU_LENGTH - 5,
TITLE_HEIGHT * 2 + i * MENU_HEIGHT + 5);
setcolor(GREEN);
setfillstyle(SOLID_FILL, GREEN);
bar(menu_id * MENU_LENGTH + 8,
TITLE_HEIGHT * 2 + menu_info[menu_id].menu_sel * MENU_HEIGHT + 10,
(menu_id + 1) * MENU_LENGTH - 8,
TITLE_HEIGHT * 2 + menu_info[menu_id].menu_sel * MENU_HEIGHT + 30);
for (i=0; i<menu_count; i++)
WriteHz(menu_id * MENU_LENGTH + 10,
TITLE_HEIGHT * 2 + i * MENU_HEIGHT + 10,
1, 1, 3, BLACK,
menu_info[menu_id].menu_desc[i]);
}
void restore_screen()
{
putimage(rect.left, rect.top, buf, COPY_PUT);
free(buf);
}
void init_rect()
{
int i;
menu_info[0].menu_desc = menu_file;
menu_info[0].menu_count = 3;
menu_info[0].menu_sel = 0;
menu_info[1].menu_desc = menu_sign;
menu_info[1].menu_count = 3;
menu_info[1].menu_sel = 0;
*(param_lqr_str[0]) = '\0';
*(param_lqr_str[1]) = '\0';
*(param_lqr_str[2]) = '\0';
*(param_lqr_str[3]) = '\0';
*(param_pid_str[0]) = '\0';
*(param_pid_str[1]) = '\0';
*(param_pid_str[2]) = '\0';
*(param_sign_str[0]) = '\0';
*(param_sign_str[1]) = '\0';
*(param_sign_str[2]) = '\0';
param_info[0].param_desc = param_lqr_desc;
param_info[0].param_str = param_lqr_str;
param_info[0].param_count = 4;
param_info[0].param_sel = 0;
param_info[0].char_num = param_lqr_num;
param_info[0].param_value = param_lqr_value; /*数组重命名（LQR控制器参数数组）*/
param_info[0].cur_pos = 0;
param_info[1].param_desc = param_pid_desc;
param_info[1].param_str = param_pid_str;
param_info[1].param_count = 3;
param_info[1].param_sel = 0;
param_info[1].char_num = param_pid_num;
param_info[1].param_value = param_pid_value; /*数组重命名（PID控制器参数数组）*/
param_info[1].cur_pos = 0;
param_info[2].param_desc = param_root_desc;
param_info[2].param_str = param_root_str;
param_info[2].param_count = 9;
param_info[2].param_sel = 0;
param_info[2].char_num = param_root_num;
param_info[2].param_value = param_root_value; /*数组重命名（传递函数参数数组）*/
param_info[2].cur_pos = 0;
param_info[3].param_desc = param_sign_desc;
param_info[3].param_str = param_sign_str;
param_info[3].param_count = 3;
param_info[3].param_sel = 0;
param_info[3].char_num = param_sign_num;
param_info[3].param_value = param_sign_value;
param_info[3].cur_pos = 0;
for (i=0; i<MAX_POINT; i++)
{
dp[i].point_the = BASE_Y;
dp[i].point_thedot = BASE_Y;
dp[i].point_pos = BASE_Y;
dp[i].point_posdot = BASE_Y;
}
}
void display_param()
{
int i;
restore_temp_param();
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);
setcolor(BLACK);
settextstyle(TRIPLEX_FONT, HORIZ_DIR, 1);
for (i=0; i<param_info[param_id].param_count; i++){
if (param_id == MAX_MENU)
WriteHz(rect.left+20, rect.top+(i+1)*20 + 4, 1, 1, 1, BLACK, param_info[param_id].param_desc[i]);
else
outtextxy(rect.left+20, rect.top+(i+1)*20, param_info[param_id].param_desc[i]);
}
setcolor(BLUE);
setfillstyle(SOLID_FILL, BLUE);
for (i=0; i<param_info[param_id].param_count; i++)
bar(rect.left + 60,
rect.top + (i + 1) * 20 + 4,
rect.left + PARAM_LENGTH - 20,
rect.top + 16 +(i + 1) * 20 + 4);
setcolor(WHITE);
settextstyle(DEFAULT_FONT, HORIZ_DIR, 1);
for (i=0; i<param_info[param_id].param_count; i++)
outtextxy(rect.left+60, rect.top+(i+1)*20+8,
param_info[param_id].param_str[i]);
display_button();
}
void display_button()
{
setcolor(LIGHTGRAY);
setfillstyle(SOLID_FILL, LIGHTGRAY);
bar(rect.left+20, rect.bottom - 42, rect.right - 20, rect.bottom - 20);
setcolor(GREEN);
setfillstyle(SOLID_FILL, GREEN);
if (button == 0)
bar(rect.left + 20, rect.bottom - 42,
rect.left + 65, rect.bottom - 20);
else
bar(rect.right - 65, rect.bottom - 42,
rect.right - 20, rect.bottom - 20);
WriteHz(rect.left + 25, rect.bottom - 40, 1, 1, 3, BLACK, "确认");
WriteHz(rect.right - 60, rect.bottom - 40, 1, 1, 3, BLACK, "取消");
}
int handle_panel(int key)
{
switch(key)
{
case ALT_F:
act = MENU;
menu_id = 0;
rect.left = 0;
rect.right = MENU_LENGTH;
rect.top = TITLE_HEIGHT * 2;
rect.bottom = TITLE_HEIGHT * 2 + menu_info[0].menu_count * MENU_HEIGHT + 10;
save_screen();
display_menu();
break;
case ALT_D:
act = MENU;
menu_id = 1;
rect.left = MENU_LENGTH;
rect.right = MENU_LENGTH * 2;
rect.top = TITLE_HEIGHT * 2;
rect.bottom = TITLE_HEIGHT * 2 + menu_info[1].menu_count * MENU_HEIGHT + 10;
save_screen();
display_menu();
break;
case ALT_X:
return ALT_X;
case UP_S:
case LOW_S:
if ((safety == 0) && (start == 0)){
if (first_swingup == 0){
act = ALARM;
// mutex_event = 1;
mutex_event = 0;
rect.left = 220;
rect.top = 200;
rect.right = 360;
rect.bottom = 330;
save_screen();
button = 1;
display_alarm();
}
else
start = 1;
}
break;
case F1:
if (zoom_the < 512){
zoom_the *= 2;
draw_zoom_unit(zoom_the, 0);
}
break;
case F2:
if (zoom_thedot < 512){
zoom_thedot *= 2;
draw_zoom_unit(zoom_thedot, 1);
}
break;
case F3:
if (zoom_pos < 512){
zoom_pos *= 2;
draw_zoom_unit(zoom_pos, 2);
}
break;
case F4:
if (zoom_posdot < 512){
zoom_posdot *= 2;
draw_zoom_unit(zoom_posdot, 3);
}
break;
case F5:
if (zoom_the > (1.0/512)){
zoom_the /= 2;
draw_zoom_unit(zoom_the, 0);
}
break;
case F6:
if (zoom_thedot > (1.0/512)){
zoom_thedot /= 2;
draw_zoom_unit(zoom_thedot, 1);
}
break;
case F7:
if (zoom_pos > (1.0/512)){
zoom_pos /= 2;
draw_zoom_unit(zoom_pos, 2);
}
break;
case F8:
if (zoom_posdot > (1.0/512)){
zoom_posdot /= 2;
draw_zoom_unit(zoom_posdot, 3);
}
break;
case UP_C:
case LOW_C:
if (draw_mode == 0){
draw_mode = 1;
draw_pendulum_panel();
}
else{
draw_mode = 0;
free(image);
draw_curve_panel();
}
break;
case UP:
if ((start == 2) && (pend == 0) && (motion == 0) && (safety == 0)){
start = 3;
motion = 1; /*小车起摆运动标志位*/
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -