二足歩行ロボットを歩かせるためのプログラムを考え試しているのですが、なかなか思い通りに 動作してくれません。
「基本的にどうなん…」ということを検証するために、足だけの部分を作りました。
サーボは、以前に使用電力が多すぎてすぐにバッテリーが消耗するものがあったのでそれを 利用しました。
上下２段に分けて、下の段にマイコン基盤とバッテリーを配置しました。
以下に、ＰＣ側で操作した通りに足が動作する最も基本的なプログラムを載せます。
この装置を使っていろいろな実験を試みるつもりです。
最初にＰＣで操作するvisual Basicで組んだプログラム（動作画面とソースプログラム）を、 ２番目にマイコン部のＣ言語プログラムを載せます。
左がＰＣ側のＶＢの実行画面です。
４つのチェックボックスがあります。
どれか一つをチェックして右の矢印をクリックするとチェックした個所の数字が増減します。
送信ボタンをクリックすると該当の足の膝関節が設定した数字の位置まで動きます。
次がそのためのＶＢのプログラムです。
Public Class Form1
'複数のイベントで使用する変数の定義********************
Dim servo_max, servo_min, targetServo, checkState(14) As Integer
Dim ar_checkbox(12) As CheckBox
Dim i As Integer
Private Sub Form1_Load(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles MyBase.Load
Dim response As MsgBoxResult
'シリアルポートをオープンする
Try
SerialPort1.Open()
Catch ex As IO.IOException
response = MsgBox("エラー！シリアルポートのオープンに失敗しました。
このまま起動しますか？", MsgBoxStyle.YesNo, "起動の確認")
End Try
'Noの場合はプログラム終了
If response = MsgBoxResult.No Then End
'変数の初期化
servo_max = 5565 'ＲＣサーボ出力最大値
servo_min = 0 'ＲＣサーボ出力最小値
'テキストボックスの配列初期化
ar_checkbox(0) = CheckBox1
ar_checkbox(1) = CheckBox2
ar_checkbox(2) = CheckBox3
ar_checkbox(3) = CheckBox4
ar_checkbox(4) = CheckBox5
ar_checkbox(5) = CheckBox6
ar_checkbox(6) = CheckBox7
ar_checkbox(7) = CheckBox8
ar_checkbox(8) = CheckBox9
ar_checkbox(9) = CheckBox10
ar_checkbox(10) = CheckBox11
ar_checkbox(11) = CheckBox12
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を25増やす********************
Private Sub Button4_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button4.Click
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
'サーボの出力値を250減らす
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) + 25)
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を25減らす********************
Private Sub Button5_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button5.Click '変数定義
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
'サーボの出力値を25減らす
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) - 25)
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を300増やす********************
Private Sub Button6_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button6.Click
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
'サーボの出力値を300増やす
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) + 300)
End Sub
'ラジオボタンで選択されたＲＣサーボの出力値を300減らす********************
Private Sub Button7_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button7.Click
'変数定義
Dim i, j, k As Integer
j = 0
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
j = j + 1
k = i
End If
Next
If j > 1 Then
MsgBox("複数のサーボにチェックがあります")
Return
End If
ar_checkbox(k).Text = Format(Val(ar_checkbox(k).Text) - 300)
End Sub
'マイコンにチェックされたＲＣサーボ信号出力値を送信する********************
Private Sub Button8_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button8.Click
'変数定義
Dim i, checkState(12) As Integer
Dim err_msg As String
Dim buff(64) As Byte
For i = 0 To 11
If ar_checkbox(i).Checked = True Then
checkState(i) = 1
Else
checkState(i) = 0
End If
Next
'テキストボックスの数値が最大／最小範囲にあるかチェックする
i = targetServo
If Val(ar_checkbox(i).Text) < servo_min Or Val(ar_checkbox(i).Text) > servo_max
Then
err_msg = Format(i) + "番サーボの出力値が制限範囲 ("
err_msg = err_msg + Format(servo_min) + "〜" + Format(servo_max) + ")
を超えています。
出力を見合わせます。"
MsgBox(err_msg, MsgBoxStyle.OkOnly, )
Exit Sub
End If
'マイコンにＲＣサーボ制御信号 状態変更コマンドを送信
write_sio_command("B")
'targetサーボに信号出力値を送信する
For i = 0 To 11
write_sio_int(ar_checkbox(i).Text)
Next
'ターゲットサーボのチェック状態を送る
For i = 0 To 11
write_sio_int(Str(checkState(i)))
Next
End Sub
'ロボット制御コマンドキャラクター送信********************
Private Sub write_sio_command(ByVal command As Char)
'変数定義
Dim command_char(64), buff(64) As Byte
'マイコンにコマンドを送信
command_char(0) = Asc(command)
SerialPort1.Write(command_char, 0, 1)
'受信確認待ちとエラー処理
Try
buff(0) = 0
SerialPort1.Read(buff, 0, 1)
' MsgBox(buff(0))
Catch ex As TimeoutException
MsgBox("通信エラーです。ロボットが規定時間以内に応答しません")
Exit Sub
End Try
If command_char(0) <> buff(0) Then
MsgBox("通信エラーです。ロボットがコマンドに正しく応答しません " )
Exit Sub
End If
End Sub
'２バイトの整数をシリアルポートから出力する（引数は文字列）********************
Private Sub write_sio_int(ByVal input_str As String)
'変数定義
Dim buff(64) As Byte
Dim output_val As Integer
'テキストボックスの文字列を整数に変換
output_val = Val(input_str)
'変換した整数の出力値を上位バイトと下位バイトに分割してバッファにセット
buff(0) = Int(output_val / 256)
buff(1) = output_val - Int(output_val / 256) * 256
'シリアルポートから出力
SerialPort1.Write(buff, 0, 2)
End Sub
'ＲＣサーボの制御信号出力開始********************
Private Sub Button2_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button2.Click
'変数定義
Dim command_char(64) As Byte
'マイコンにＲＣサーボ制御信号 状態変更コマンドを送信
write_sio_command("A")
'ＲＣサーボ制御信号 開始コマンドを送信
command_char(0) = Asc("1")
SerialPort1.Write(command_char, 0, 1)
End Sub
'ＲＣサーボの制御信号出力停止********************
Private Sub Button3_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button3.Click
'変数定義
Dim command_char(64) As Byte
'マイコンにＲＣサーボ制御信号 状態変更コマンドを送信
write_sio_command("A")
'ＲＣサーボ制御信号 停止コマンドを送信
command_char(0) = Asc("0")
SerialPort1.Write(command_char, 0, 1)
End Sub
Private Sub Button1_Click(ByVal sender As System.Object, ByVal e As System.EventArgs)
Handles Button1.Click
End
End Sub
End Class
#include "3048f.h"
/* 待ち時間発生 引数に、必要なミリ秒を指定する***********/
void init_io(void)
{
/*ITU０のクロックを８分週 3125：１ｍｓ、4687.5：１．５ｍｓ、6250：２ｍｓ、62500：２０ｍｓ*/
ITU0.TCR.BYTE = 0x03; /* ITU0、TCNTクリア禁止 clock 1/8 */
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
/* シリアル通信初期化*******************************/
SCI1.SCR.BYTE = 0x00; /* SCI1設定 stop,内部クロック */
SCI1.SMR.BYTE = 0x00; /* data8.stop1,pari non */
SCI1.BRR = 40; /* 19200bps */
ITU0.TCNT = 0; /* ITU0 TCNTｶｳﾝﾄを０にクリア */
while(ITU0.TCNT <= 3125);/*１ｍｓ待つ*/
SCI1.SCR.BYTE = 0x30; /* Tx,Rx有効 ,割込み無効 */
SCI1.SSR.BYTE &= 0x80; /* エラーフラグのクリア */
return;
}
/* SCI1から１文字受信する****************************/
char sci1_rx(void){
char data;
while((SCI1.SSR.BYTE & 0x78)==0); /* 受信とエラーのフラグが立つまで待つ*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return(0xff); /* エラー時はFFを返す */
}
}
/*unsigned int型変数（２バイト）をシリアルポートから入力する*/
/*備考：データが来るまで待つ */
unsigned int sci_read_uintA(void)
{
unsigned int data;
unsigned char data_L, data_H;
/*unsigned charのデータ２個を受け取る*/
data_H = sci1_rxWait();
data_L = sci1_rxWait();
/*unsigned intのデータに変換する*/
data = (unsigned int)data_H * 256 + (unsigned int)data_L;
return(data);
}
/* SCI1から１文字受信する データが来るまで待つ****************************/
unsigned char sci1_rxWait(void){
unsigned char data;
while((SCI1.SSR.BYTE & 0x78)==0) ; /* 受信とエラー以外は待つ*/
if(SCI1.SSR.BIT.RDRF == 1){ /* データ受信が正常 */
data = SCI1.RDR; /* データを受け取りdataに保存 */
SCI1.SSR.BIT.RDRF = 0; /* 受信フラグのクリア*/
return(data);
}
else{ /* データ受信にエラー発生 */
SCI1.SSR.BYTE &= 0xc7; /* エラーフラグをクリア */
return((unsigned char)0xff); /* エラー時はFFを返す */
}
}
/* SCI1に１文字送信する */
void sci1_tx(char data){
while(SCI1.SSR.BIT.TDRE == 0); /*未送信データが送られるまで待つ*/
SCI1.TDR = data; /*送信データのセット*/
SCI1.SSR.BIT.TDRE = 0; /*送信フラグのクリア*/
return;
}
/* SCI1に文字列を送信する 文字列は'\0'で締めくくっておく**************/
void sci1_strtx(char *str){
while(*str != '\0'){ /* 文字が\0になるまで繰り返す */
sci1_tx(*str); /* 1文字送信*/
str++; /* 次の文字に移る*/
} return;
}
/*サーボ番号をピン番号に割り当てる*/
void srvWari(int srvNo, int x)
{
switch(srvNo){
case 0: P3.DR.BIT.B0 = x;break;
case 1: P3.DR.BIT.B1 = x;break;
case 2: P3.DR.BIT.B2 = x;break;
case 3: P3.DR.BIT.B3 = x;break;
case 4: P3.DR.BIT.B4 = x;break;
case 5: P3.DR.BIT.B5 = x;break;
case 6: P3.DR.BIT.B6 = x;break;
case 7: P3.DR.BIT.B7 = x;break;
case 8: P1.DR.BIT.B0 = x;break;
case 9: P1.DR.BIT.B1 = x;break;
case 10:P1.DR.BIT.B2 = x;break;
case 11:P1.DR.BIT.B3 = x;break;
}
}
/*プログラムの始まり*/
/*ＰＣから3つの信号を受け取り該当のサーボを動作させる */
/*１：S（制御コマンド）２：サーボ番号０〜１１ ３：パルス幅相当数値を順番に受けて動作*/
main()
{
char signal;
int i, j,Width, pWidth[12],checkState[12],servoNo,switchSignal;
P3.DDR = 0xff; /*PORT3を出力に設定*/
P1.DDR = 0xff; /*PORT1を出力に設定*/
ITU.TSTR.BIT.STR0 = 1; /* ITU0 TCNTｶｳﾝﾄ開始 */
/*Ｈ８ハードの初期化を行う*/
init_io();
Width = 4687;/*パルス幅 初期値*/
/*無限ループ開始*/
switchSignal ='1';
while(1){
signal = sci1_rx();/*コマンドが受信されているか確認する*/
sci1_tx(signal);
if(signal == 'A'){/*ＲＣサーボの制御信号 出力ＯＮ／ＯＦＦ命令受信*/
/*スイッチデータ受信*/
switchSignal = sci1_rx();
}
if (switchSignal == '1'){
if( signal =='B') {
for(i=0;i<12;i++) pWidth[i] = sci_read_uintA();/*ﾊﾟﾙｽ幅12個分*/
for(i=0;i<12;i++) checkState[i] =sci_read_uintA();/*ｻｰﾎﾞﾁｪｯｸ状態*/
for(i=0;i<12;i++)
if(checkState[i]==1) {
servoNo = i;
Width = pWidth[i];
}
}
for(i=1;i<=20;i++){
ITU0.TCNT = 0; /* ITU0 TCNTｶｳﾝﾄを０にクリア */
srvWari(servoNo, 1);/*該当のピン出力を１にする*/
while(ITU0.TCNT< 7375 - Width) ;/* 設定値のパルス幅*/
srvWari(servoNo, 0);/*該当のピン出力を０にする*/
while(ITU0.TCNT < 40000);/*１３ｍｓ経過するまで待つ*/
}
}
}
}
