ロボットを創ろう

/* モータ制御実験Ａ／Ｄデータ収集用プログラムＪＫ５．Ｃ
　　　　　　　　　　　　　　　　　　1994.9.20.　A.Satoh Lab.
　　　　　　　　　　 By (Master_1) H.Wani */

#include <stdio.h>
#include <dos.h>
#include <graphics.h>
#include <conio.h>
#include <stdlib.h>
#include <pc98.h>
#include "\fads.h"

typedef struct{
int channel;
int color;
int *data;
int mode;
} ADdata_type;

/* prototype */
/* main */

void jikan();
void setch();
void soku();
void graph();
int ADsave();
int ADload();
void table();
void prg_end();

void cls_txtgraph();
int init_BGI();
void set_section();
ADdata_type *memtodata();
void datatomem();

/* variable */

int data,dataflag,alltime=20;
int chnum=2,dtnum=500;
char BUF[50];
ADdata_type *AD;

/* general use */

void cls_txtgraph()
{
clrscr();
cleardevice();
}

/* init BGI */

int init_BGI(int mode)
{
int gd=PC98,gm=PC98C8;

if(registerbgidriver(PC98_driver) < 0) return 0;
if(mode == 16) gm=PC98C16;
initgraph(&gd,&gm,"");
if(gd<0) return 0;
return 1;
}

/* menu */

void main()
{
init_BGI(8);

if ((AD = memtodata(chnum,dtnum)) == NULL) exit(1);

while(1)
{
cls_txtgraph();
textcolor(3);
gotoxy(28, 5); cprintf(" サーボ実験データ収集 ");
gotoxy(28, 6); printf(" ＭＥＮＵ ");
gotoxy(28, 8); printf(" １．測定時間 ");
gotoxy(28, 9); printf(" ２．チャンネル ");
gotoxy(28,10); cprintf(" ３．測定 ");
gotoxy(28,11); printf(" ４．データ確認 ");
gotoxy(28,12); printf(" ５．グラフ表示 ");
textcolor(2);
gotoxy(28,13); cprintf(" ６．データセーブ ");
gotoxy(28,14); cprintf(" ７．データロード ");
textcolor(4);
gotoxy(28,16); cprintf(" ESC．終了 ");
gotoxy(28,19); printf("番号を入力してください");

switch(getch())
{
case '1':jikan(); break;
case '2':setch(); break;
case '3':soku(); break;
case '4':table(); break;
case '5':graph(); break;
case '6':ADsave(); break;
case '7':ADload(); break;
case 0x1b:prg_end(); break;
}
}
}

void prg_end()
{
datatomem(AD);
cls_txtgraph();
closegraph();
exit(0);
}

/* jikan */

void jikan()
{
int i;

cls_txtgraph();
gotoxy(20, 8); printf("現在の測定時間 %5d sec",alltime);
gotoxy(20,12); printf("測定時間の入力(sec) ＝ ");
gets(BUF);
if (BUF[0] == NULL) return;
i = atoi(BUF);
if (i >= 5 && i <= 1000) { alltime = i; return; }
gotoxy(15,16); printf("時間の範囲は 5 ≦ time ≦ 1000");
getch();
}

/* set channel */

void setch()
{
int i;

cls_txtgraph();
gotoxy(15,5); printf("チャンネルの個数 = %3d\n\n",chnum);
for(i=0;i<chnum;i++)
{
textcolor(AD[i].color);
cprintf(" Data %2d channel %2d color %2d\r\n",
i,AD[i].channel,AD[i].color);
}
printf("\n\n チャンネル数の変更 > ");
gets(BUF);
if(BUF[0] == NULL) return;
i=atoi(BUF);
if(i < 1 || i>16) { printf("\n 範囲外です");
getch();
return; }

datatomem(AD,chnum);
chnum=i;
AD=memtodata(chnum,dtnum);
for(i=0;i<chnum;i++)
{
printf("\n Data %2d channel = ",i);
gets(BUF); AD[i].channel=atoi(BUF);
printf(" color = ");
gets(BUF); AD[i].color=atoi(BUF);
}
return;
}

/* sokutei */

void soku()
{
int i,j,x1=99,x2=100,y1,y2;
int data[20],channel[20];
int adr[1]={0xdc};

for(i=0;i<chnum;i++) channel[i]=AD[i].channel;
set_ADchannel(data,channel,chnum);
set_ADadr(adr);
alltime = set_ADtimer(alltime,dtnum);

set_section(0);

textcolor(4);
gotoxy(34,25); cprintf("Hit any key to start!");
getch();
textcolor(3);
gotoxy(34,25); cprintf(" Hit q key to stop. ");
gotoxy(1,1);

AD_start(100);
if(get_AD() > 0) { printf("ERROR"); }
for(j=0;j<chnum;j++) AD[j].data[0] = data[j];

for(i=1;i<dtnum;i++)
{
if(get_AD() > 0) { printf("ERROR"); }
x1++;
x2++;
for(j=0;j<chnum;j++)
{
AD[j].data[i] = data[j];

y1=(5608-AD[j].data[i-1]) >> 4;
y2=(5608-data[j]) >> 4;
setcolor(AD[j].color);
line(x1,y1,x2,y2);
}
if((char)pc98key(5)=='q') break;
}
AD_stop();
setcolor(15);
textcolor(6);
gotoxy(34,25); cprintf("Hit any key to return.");
getch();
}

/* graph */

void graph()
{
int i,j,x1=99,x2=100,y1,y2;

set_section(0);
for(i=1;i<dtnum;i++)
{
x1++;
x2++;
for(j=0;j<chnum;j++)
{
y1=350-((AD[j].data[i-1]+8) >> 4);
y2=350-((AD[j].data[i]+8) >> 4);
setcolor(AD[j].color);
line(x1,y1,x2,y2);
}
}
setcolor(15);
textcolor(6);
gotoxy(34,25); cprintf("Hit any key to return.");
i = getch();
gotoxy(34,25); printf(" ");
}

/* data save */

int ADsave()
{
FILE *file;
int i,j;

cls_txtgraph();
gotoxy(15,10); printf("input save file name -> ");
gets(BUF);
if ((file=fopen(BUF,"r")) != NULL)
{
fclose(file);
printf("\n\n over write ? (y/n)");
while((i=getch())!='y' && (i!='n'));
if (i=='n') return(0);
}
if ((file=fopen(BUF,"w")) == NULL)
{
printf("\n\n Error writing output file");
getch();
return(0);
}
fprintf(file," %5d, %5d, %5d,\n",alltime,chnum,dtnum);
fprintf(file," %5d,",0);
for(i=0;i<chnum;i++) fprintf(file," %5d,",AD[i].channel); fprintf(file,"\n");
fprintf(file," %5d,",0);
for(i=0;i<chnum;i++) fprintf(file," %5d,",AD[i].color); fprintf(file,"\n");
for(j=0;j<dtnum;j++)
{
fprintf(file," %5d,",j);
for(i=0;i<chnum;i++) fprintf(file," %5d,",AD[i].data[j]);
fprintf(file,"\n");
}
fclose(file);
return(1);
}

/* data load */

int ADload()
{
FILE *file;
int i,j,k;

cls_txtgraph();
gotoxy(15,10);
printf("input load file name -> ");
gets(BUF);
if ((file=fopen(BUF,"r")) == NULL)
{
printf("\n\n Could not find file \"%s\"",BUF);
getch();
return 0;
}

datatomem(AD,chnum);

fscanf(file," %5d, %5d, %5d,",&alltime,&chnum,&dtnum);
AD=memtodata(chnum,dtnum);
fscanf(file," %5d,",&k);
for(i=0;i<chnum;i++) fscanf(file," %5d,",&AD[i].channel);
fscanf(file," %5d,",&k);
for(i=0;i<chnum;i++) fscanf(file," %5d,",&AD[i].color);
for(j=0;j<dtnum;j++)
{
fscanf(file," %5d,",&k);
for(i=0;i<chnum;i++) fscanf(file," %5d,",&AD[i].data[j]);
}
fclose(file);
return 1;
}

/* table */

void table()
{
int i,j,n,top=0;

cls_txtgraph();
gotoxy(10,1); printf("サーボ実験データ測定時間＝ %4d sec",alltime);
gotoxy(4,3); printf("No. Data1 Data2 No. Data1 Data2 No. Data1 Data2 No. Data1 Data2");
setcolor(10);
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
for(i= 12;i<621;i+=152) line (i,31,i,368);
line(12,31,620,31); line(12,47,620,47); line(12,368,620,368);
setlinestyle(SOLID_LINE,0,NORM_WIDTH);
for(i=0;i<457;i+=152) {line (52+i,31,52+i,368); line(108+i,31,108+i,368);}
setcolor(15);
gotoxy(2,25); printf(" ↑:前ページ ↓:次ページｍ:表示切換(Real/Volt) ｑ:終了 ");

while(1)
{
for(i=0;i<20;i++)
{
gotoxy(3,4+i);
for(j=0;j<80;j+=20)
{
n = top+i+j;
if(n < dtnum)
{
printf("%4d ",n);
if(AD[0].mode == 0) printf("%5d %5d ",AD[0].data[n],AD[1].data[n]);
else printf("%5.2f %5.2f ",(AD[0].data[n])/409.6/2,(AD[1].data[n])/409.6/2);
}
}
}
switch(getch())
{
case 11:top=(top < 80)?top:top-80; break;
case 10:top=((top+80) > dtnum)?top:top+80; break;
case 'm':AD[0].mode = !AD[0].mode; break;
case 'q':return;
}
window(3,4,76,23); clrscr(); window(1,1,80,25);
}
}

/* memory */

ADdata_type * memtodata(int chnum,int dtnum)
{
ADdata_type *ADdata;
int i,j;

if ((ADdata = (ADdata_type *)calloc(chnum,sizeof(ADdata_type))) == NULL)
{
printf("\n\n memory over");
printf("\n\n Hit any key.");
getch();
return NULL;
}

for(i=0;i<chnum;i++)
{
if ((ADdata[i].data = (int *)calloc(dtnum,sizeof(int))) == NULL)
{
for(j=0;j<i;j++) free(ADdata[i].data);
datatomem(ADdata,i-1);
printf("\n\n memory over %d",i);
printf("\n\n Hit any key.");
getch();
return NULL;
}

ADdata[i].channel = (i*8) % 16;
ADdata[i].color = (1+i) % 8;
ADdata[i].mode = 0;
}
return ADdata;
}

void datatomem(ADdata_type *ADdata,int chnum)
{
int i;

for(i=0;i<chnum;i++) free(ADdata[i].data);
free(ADdata);
}

/* set section */

void set_section(int oy)
{
int i,dec,sign;
char *str;

cls_txtgraph();
setlinestyle(SOLID_LINE,0,THICK_WIDTH);
for(i=0;i<11;i+=10) { line(99+i*50,93,99+i*50,351);
line(100,94+25.6*i,600,94+25.6*i);}
setlinestyle(SOLID_LINE,0,NORM_WIDTH);
for(i=1;i<10;i++) { line(99+i*50,95,99+i*50,350);
line(100,94+25.6*i,600,94+25.6*i);}
settextjustify(CENTER_TEXT,CENTER_TEXT);
settextstyle(DEFAULT_FONT,VERT_DIR,1);
outtextxy( 70,222,"Ｖｏｌｔａｇｅ [Volt]");
for(i=0;i<11;i+=2) outtextxy(90,350-25.6*i,itoa(oy+i/2,BUF,10));
settextstyle(DEFAULT_FONT,HORIZ_DIR,1);
outtextxy(350,374,"Ｔｉｍｅ [sec]");
for(i=0;i<11;i++)
{
str = fcvt((double)alltime/10*i,3,&dec,&sign);
str[dec+1]=str[dec]; str[dec]='.'; str[dec+2]=0;
outtextxy(100+i*50,358,str);
}
}

//==============================
// ステップ応答のサンプル
//==============================

#include "mmss.h"

void main()
{
float moku; // 目標値
float pot; // ポテンショメータの電圧
float motor_v; // コンピュータからモータへの電圧
// (実際はこの電圧の約3倍の電圧がモータにかかる)

Init_ss(); // いろいろな前処理

while (1) {
Wait_ss(); // ウェイト

pot = GetVolt(); // ポテンショメータの電圧読み取り(モータの現在位置取得)

moku = 2.5; // 目標値一定
// ポテンショメータが2.5[V]を出力する場所を目標値とする

motor_v = moku - pot; // 計算

OutputVolt(motor_v); // 電圧出力(モータへの出力電圧変更)

Draw(moku, pot); // グラフに点を描画する
}
}

//==============================
// サイン波入力のサンプル
//==============================

#include "mmss.h"

void main()
{
float moku; // 目標値
float pot; // ポテンショメータの電圧
float motor_v; // コンピュータからモータへの電圧
// (実際はこの電圧の約3倍の電圧がモータにかかる)
float now_time = 0; // 現在の時間、単位は秒

Init_ss(); // いろいろな前処理

while (1) {
Wait_ss(); // ウェイト

pot = GetVolt(); // ポテンショメータの電圧読み取り(モータの現在位置取得)

moku = 2.5 * sin(2 * M_PI / 10 * now_time); // サイン波の計算

motor_v = moku - pot; // 計算

OutputVolt(motor_v); // 電圧出力(モータへの出力電圧変更)

Draw(moku, pot); // グラフに点を描画する

now_time = now_time + 0.04; // 時間を進める
}
}

このページで紹介している写真は無断で転載することを禁止します。ただし、記事を模倣してロボットを創ることは大歓迎です。