tf-2md3-devel@t-frog.com
差動二輪駆動(PWS)型移動ロボット以外に利用した例
Date:
Mon, 08 Dec 2014 13:02:46 +0900
From:
長田大輝 <
a211066 at ns dot kogakuin dot ac dot jp
>To:
<
tf-2md3-devel at t-frog dot com
>Subject:
[00077] 差動二輪駆動(PWS)型移動ロボット以外に利用した例
工学院大学機械システム工学科システムインテグレーション研究室(羽田研究室)
の長田と申します。
お世話になっております。
私は火山調査ロボットに関する卒業研究を行っているのですが、
ヘリ追尾用の2軸アンテナ台のハードウェアを自作し、
T-FrogモータドライバとYP-Spurを用いて制御を行いました。
基本的には提供されているマニュアル通りなのですが、
差動二輪駆動(PWS)型移動ロボット以外に利用した例ということで、
他の方々のご参考までにご報告いたします。
行ったことは以下の通りです。
１）ハードウェアに合わせたパラメタファイルの用意
２）アンテナ制御台の制御角度の確認・調整
３）関数の作成
４）メイン関数の作成
それぞれ以下でご説明いたします。
１）ハードウェアに合わせたパラメタファイルの用意
まず自作のハードウェアに合わせてパラメタファイルを作成しました。
仕様モータはiCartで使われているツジ電子社製パルスエンコーダ付きギヤード
ブラシレスモータ、TF-M30-24-3500-G15LとTF-M30-24-3500-G15Rです。
モータドライバとモータの接続は下記になります。
-----
接続
Motor 1 OutputにTF-M30-24-3500-G15Lのモータ端子
Motor 1 Encoder InputにTF-M30-24-3500-G15Lのエンコーダ端子
Motor 2 OutputにTF-M30-24-3500-G15Rモータ端子
Motor 2 Encoder InputにTF-M30-24-3500-G15Rのエンコーダ端子
-----
今回はアンテナ制御台は
縦方向チルト軸のギア比が1500:1で、仕様上の目標角速度が0.017 rad/s角加速
度が0.017 rad/s^2
横方向パン軸のギア比が15:1で仕様上の目標角速度が0.175 rad/s角加速度が0.
175 rad/s^2でした。
それぞれのエンコーダは500 pulse/revです。
そこでYP-Spur付属？の標準パラメータファイルicart-mini.paramを元に
各パラメタを下記のように設定しました。
この中で、GAIN_KIとTORQUE_VISCOS、TORQUE_NEWTONのパラメタだけを変更しま
した。
また、パラメタではギア比を変更せず、角度の命令をする際にギア比分の差を与
えて調整しています。
-----
paramファイルの中身
VERSION 4
COUNT_REV 400
VOLT 24
CYCLE 0.001
GEAR 75
MOTOR_R 0.75
MOTOR_PHASE 3
TORQUE_FINENESS 0.000001
RADIUS[0] 0.07455
RADIUS[1] -0.07455
TREAD 0.30737
CONTROL_CYCLE 0.015
TORQUE_MAX 0.05
TORQUE_LIMIT 0.4
MAX_VEL 0.9
MAX_W 3.14
MAX_ACC_V 1.5
MAX_ACC_W 6.28
MAX_CENTRI_ACC 2.45
L_C1 0.01
L_K1 800
L_K2 300
L_K3 200
L_DIST 0.6
INTEGRAL_MAX 0.05
MOTOR_VC 630.0
MOTOR_TC 0.01515
MOTOR_M_INERTIA 0
TIRE_M_INERTIA 0.02
MASS 10
MOMENT_INERTIA 0.1
GAIN_KP 120
GAIN_KI 5000
TORQUE_VISCOS 0
TORQUE_NEWTON 0
-----
２）アンテナ制御台の制御角度の確認・調整
次に設定したパラメタファイルが正しく動作しているかを
ypspur-interpreterを用いて確認します。
? set_wheel_velおよびset_wheel_accelで角速度と角加速度を設定します。
? wheel_angで目標角度の値分モータを動かします。
※私はwheel_angの目標角度を与える際にギア比を考慮しました。
? get_wheel_angで現在のモータの角度を調べます。
?、?を繰り返し、調整します。
これでアンテナを正しく任意の角度に向けることができるようになりました。
使用したコマンドは下記になります。
-----
使用したコマンド
set_wheel_vel アンテナのPan角方向の角速度 アンテナのTilt角方向の角速度
set_wheel_accel アンテナのPan角方向の角加速度 アンテナのTilt角方向の角加
速度
wheel_ang アンテナのPan角方向の目標角度(単位radian) アンテナのTilt角方向
の目標角度(単位radian)
get_wheel_ang
-----
コマンドを使用した例が下記になります。
-----
例
set_wheel_vel 10 0.5
set_wheel_accel 1 0.1
wheel_ang 0.3 30
get_wheel_ang
-----
３）関数の作成
YP-Spurには大変すばらしい関数群があるのですが、
今回はPWS型移動ロボットではないので、これらの関数は使えません。
そこで、それよりも低いレベルの関数
YP_wheel_ang()、YP_get_wheel_ang()を使って、vとwを直接制御することにしま
した。
YP_wheel_ang()はエンドエフェクタのPI制御を行う関数で
また、YP_get_wheel_ang()は2つのエンドエフェクタの現在角度を知ることがで
きる関数です。
今回のエンドエフェクタはアンテナの向いている方向です。
これらを用いて、アンテナの目標方位(Pan,Tilt)を与えるとそちらを向ける
関数を作成しました。関数の中身は以下になります。
------------------------------------------------------------------------
-------------------------------------
関数の中身
//======================================================================
========
//関数名：my_wheel_ang.c
//ユーザーは何の考えなしに移動をしてほしい角度を代入すると、(0,0)を基準
に
//指定した角度まで移動する
//右回りが＋、左回りが−
//gcc my_wheel_ang.c my_get_wheel_ang.c -o my_wheel_ang.exe -I /usr/
local/include -L /usr/local/lib -lypspur -lws2_32 -lm
//======================================================================
========
#include <stdio.h>
#include <ypspur.h>
#include <windows.h>
#include "my_get_wheel_ang.h" //座標取得のため
#define vel_P 5
#define vel_T 10
#define accel_P 0.5
#define accel_T 1
#define GEAR 100 //ギア比
//引数(パン、チルト、誤差)
double my_wheel_ang(double my_tar_P,double my_tar_T,double err_P,double
err_T,double *my_set_P,double *my_set_T){
//引数my_tar_P…パン
//引数my_tar_T…チルト
//引数err_P…パンの誤差範囲
//引数err_T…チルトの誤差範囲
double con_P; //目標値パン
double con_T; //目標値チルト
double log_P; //現在のパン
double log_T; //現在のチルト
double my_set_P_in; //残差を格納
double my_set_T_in; //残差を格納
int flag_P; //パンの停止用フラグa
int flag_T; //チルトの停止用フラグ
my_set_P_in = *my_set_P; //こっちにもmy_set_Pを格納
my_set_T_in = *my_set_T; //こっちにもmy_set_Tを格納
//計算式
con_P = my_set_P_in + my_tar_P; //目標値パン
con_T = my_set_T_in + my_tar_T; //目標値チルト
//------ギア比倍・正負入れ替え--------//
double con_T2 = (-1) *con_T * GEAR;
//角度指定を出す
YP_wheel_ang( con_P, con_T2 );
return 0;
}
/*----------------------------------------------------------------------
--------
int main(void)
{
double my_tar_P;
double my_tar_T;
double my_set_P;
double my_set_T;
double err_P = 0.01;
double err_T = 1;
//------PTUの初期設定--------------------------------------------------
--//
//------Spur初期化------//
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
//-------角速度の設定-------//
YP_set_wheel_vel( vel_P , vel_T );
//------角加速度の設定------//
YP_set_wheel_accel( accel_P , accel_T );
//---------------------------------------------------------------------
--------//
my_wheel_ang(Pan,Tilt,err_P,err_T,&my_set_P,&my_set_T);
}
*/
------------------------------------------------------------------------
--------------------------------------
//======================================================================
==========
//関数名:my_get_wheel_ang.c
//ユーザーがmy_get_wheel_angを行った場合、その条件での角度を取得する
//gcc my_get_wheel_ang.c -o my_get_wheel_ang.exe -I /usr/local/include -
L /usr/local/lib -lypspur -lws2_32 -lm
//======================================================================
==========
#include <stdio.h>
#include <ypspur.h>
#define vel_P 5
#define vel_T 10
#define accel_P 0.5
#define accel_T 1
#define GEAR 100
double my_get_wheel_ang( double *my_get_ang_P, double *my_get_ang_T,
double *my_set_P, double *my_set_T ){
//引数my_get_ang_P…パンの計算結果
//引数my_get_ang_T…チルト計算結果
//引数my_set_P…my_set_wheel_angで取得した差
//引数my_set_T…my_set_wheel_angで取得した差
double Spur_P ;
double Spur_T ; //起動した時からの角度を取得するための変数
double Spur_T_GEARwari;
//起動した時からの角度を取得する
YP_get_wheel_ang(&Spur_P,&Spur_T);
Spur_T_GEARwari = ((-1) * Spur_T / GEAR);
*my_get_ang_P = Spur_P - *my_set_P; //パン
*my_get_ang_T = Spur_T_GEARwari - *my_set_T; //チルト
return 0;
}
/*----------------------------------------------------------------------
--------
int main(void)
{
double my_get_ang_P;
double my_get_ang_T;
double my_set_P;//大域変数
double my_set_T;//大域変数
//------PTUの初期設定--------------------------------------------------
---//
//------Spur初期化----------------------//
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
//-------角速度の設定-------------------//
YP_set_wheel_vel( vel_P , vel_T );
//------角加速度の設定------------------//
YP_set_wheel_accel( accel_P , accel_T );
//---------------------------------------------------------------------
---//
my_get_wheel_ang( &my_get_ang_P, &my_get_ang_T, &my_set_P, &my_set_T );
}
*/
------------------------------------------------------------------------
-------------------------------------
//======================================================================
========
//関数名:my_set_wheel_ang.c
//ユーザーがwheelの角度を指定することができる
//gcc my_set_wheel_ang.c -o my_set_wheel_ang.exe -I /usr/local/include -
L /usr/local/lib -lypspur -lws2_32 -lm
//======================================================================
========
#include <stdio.h>
#include <ypspur.h>
#define vel_P 5
#define vel_T 10
#define accel_P 0.5
#define accel_T 1
#define GEAR 100
double my_set_wheel_ang( double my_P, double my_T, double *my_set_P,
double *my_set_T ){
//引数my_P…ユーザー指定のパン
//引数my_T…ユーザー指定のチルト
//引数&my_set_P…Spurのパンをユーザー指定の経度にした際の差
//引数&my_set_T…Spurのチルトをユーザー指定の緯度にした際の差
double Spur_P; //Spurのパン
double Spur_T; //Spurのチルト
double Spur_T_GEARwari;//Spurのチルトをギア比で割ったもの
//Spurが持っているパンチルトを取得する
YP_get_wheel_ang(&Spur_P,&Spur_T);
Spur_T_GEARwari = ((-1) * Spur_T / GEAR);
//差を計算する、この値をこの後も保存する
*my_set_P = Spur_P - my_P;
*my_set_T = Spur_T_GEARwari - my_T;
return 0;
}
/*----------------------------------------------------------------------
--------
int main(void)
{
double my_P;
double my_T;
double my_set_P;//大域変数
double my_set_T;//大域変数
//------PTUの初期設定--------------------------------------------------
---//
//------Spur初期化----------------------//
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
//-------角速度の設定-------------------//
YP_set_wheel_vel( vel_P , vel_T );
//------角加速度の設定------------------//
YP_set_wheel_accel( accel_P , accel_T );
//---------------------------------------------------------------------
---//
my_set_wheel_ang(my_P, my_T, &my_set_P, &my_set_T );
}
*/
------------------------------------------------------------------------
--------------------------------------
４）メイン関数の作成
３で作成した関数を使い、全体の制御プログラムを作成しました。
今回は目標角度を1秒おきに与えてそちらを向けるようにしました。
目標角度はヘリのGPS座標を通信で受け取り算出するのですが、
この報告とは関係ないので割愛します。
基本的にはYP-Spurの考え方と同じで、目標値を与えて、
制御はYP-Spurに任せつつも、常時それを監視する、という流れになります。
しかし、実際には1秒おきに目標値を勝手に更新しているので
あまりYP-Spurの考え方とは合致していないような気もします。
-----
メイン関数の初期化部分と制御部分
制御台の初期化部分
/*------Spur初期化------*/
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
/*-------角速度の設定-------*/
YP_set_wheel_vel( vel_P , vel_T );
/*------角加速度の設定------*/
YP_set_wheel_accel( accel_P , accel_T );
-----
制御部分
my_wheel_ang(Pan,Tilt,err_P,err_T,&my_set_P,&my_set_T);
Sleep(1000);//秒スリープ
-----
以上のシステムを作成したうえで、小浅間山で行ったデモビデオを
下記で公開しておきました。
https://www.youtube.com/watch?v=NaE3Uu-_t4k&feature=youtu.be
また、デモビデオではアンテナ台の動きが分かりにくいので、アンテナ台がアッ
プのものを別途に下記で公開しました。
https://www.youtube.com/watch?v=nBQFdLfKM6A&feature=youtu.be
私の勉強不足のため、まだ制御パラメタが調整しきれておらずガタついておりま
すが、
T-FrogドライバとYP-Spurのおかげで、非常に短い時間で作成することができま
した。
(機械部分開発が60日、回路部分開発が14日、ソフト50日ぐらい)
有用なソフトウェアをご提供いただき、誠にありがとうございました。
以上です。
長田大輝（ながただいき）
工学院大学 工学部 機械システム工学科
の長田と申します。
お世話になっております。
私は火山調査ロボットに関する卒業研究を行っているのですが、
ヘリ追尾用の2軸アンテナ台のハードウェアを自作し、
T-FrogモータドライバとYP-Spurを用いて制御を行いました。
基本的には提供されているマニュアル通りなのですが、
差動二輪駆動(PWS)型移動ロボット以外に利用した例ということで、
他の方々のご参考までにご報告いたします。
行ったことは以下の通りです。
１）ハードウェアに合わせたパラメタファイルの用意
２）アンテナ制御台の制御角度の確認・調整
３）関数の作成
４）メイン関数の作成
それぞれ以下でご説明いたします。
１）ハードウェアに合わせたパラメタファイルの用意
まず自作のハードウェアに合わせてパラメタファイルを作成しました。
仕様モータはiCartで使われているツジ電子社製パルスエンコーダ付きギヤード
ブラシレスモータ、TF-M30-24-3500-G15LとTF-M30-24-3500-G15Rです。
モータドライバとモータの接続は下記になります。
-----
接続
Motor 1 OutputにTF-M30-24-3500-G15Lのモータ端子
Motor 1 Encoder InputにTF-M30-24-3500-G15Lのエンコーダ端子
Motor 2 OutputにTF-M30-24-3500-G15Rモータ端子
Motor 2 Encoder InputにTF-M30-24-3500-G15Rのエンコーダ端子
-----
今回はアンテナ制御台は
縦方向チルト軸のギア比が1500:1で、仕様上の目標角速度が0.017 rad/s角加速
度が0.017 rad/s^2
横方向パン軸のギア比が15:1で仕様上の目標角速度が0.175 rad/s角加速度が0.
175 rad/s^2でした。
それぞれのエンコーダは500 pulse/revです。
そこでYP-Spur付属？の標準パラメータファイルicart-mini.paramを元に
各パラメタを下記のように設定しました。
この中で、GAIN_KIとTORQUE_VISCOS、TORQUE_NEWTONのパラメタだけを変更しま
した。
また、パラメタではギア比を変更せず、角度の命令をする際にギア比分の差を与
えて調整しています。
-----
paramファイルの中身
VERSION 4
COUNT_REV 400
VOLT 24
CYCLE 0.001
GEAR 75
MOTOR_R 0.75
MOTOR_PHASE 3
TORQUE_FINENESS 0.000001
RADIUS[0] 0.07455
RADIUS[1] -0.07455
TREAD 0.30737
CONTROL_CYCLE 0.015
TORQUE_MAX 0.05
TORQUE_LIMIT 0.4
MAX_VEL 0.9
MAX_W 3.14
MAX_ACC_V 1.5
MAX_ACC_W 6.28
MAX_CENTRI_ACC 2.45
L_C1 0.01
L_K1 800
L_K2 300
L_K3 200
L_DIST 0.6
INTEGRAL_MAX 0.05
MOTOR_VC 630.0
MOTOR_TC 0.01515
MOTOR_M_INERTIA 0
TIRE_M_INERTIA 0.02
MASS 10
MOMENT_INERTIA 0.1
GAIN_KP 120
GAIN_KI 5000
TORQUE_VISCOS 0
TORQUE_NEWTON 0
-----
２）アンテナ制御台の制御角度の確認・調整
次に設定したパラメタファイルが正しく動作しているかを
ypspur-interpreterを用いて確認します。
? set_wheel_velおよびset_wheel_accelで角速度と角加速度を設定します。
? wheel_angで目標角度の値分モータを動かします。
※私はwheel_angの目標角度を与える際にギア比を考慮しました。
? get_wheel_angで現在のモータの角度を調べます。
?、?を繰り返し、調整します。
これでアンテナを正しく任意の角度に向けることができるようになりました。
使用したコマンドは下記になります。
-----
使用したコマンド
set_wheel_vel アンテナのPan角方向の角速度 アンテナのTilt角方向の角速度
set_wheel_accel アンテナのPan角方向の角加速度 アンテナのTilt角方向の角加
速度
wheel_ang アンテナのPan角方向の目標角度(単位radian) アンテナのTilt角方向
の目標角度(単位radian)
get_wheel_ang
-----
コマンドを使用した例が下記になります。
-----
例
set_wheel_vel 10 0.5
set_wheel_accel 1 0.1
wheel_ang 0.3 30
get_wheel_ang
-----
３）関数の作成
YP-Spurには大変すばらしい関数群があるのですが、
今回はPWS型移動ロボットではないので、これらの関数は使えません。
そこで、それよりも低いレベルの関数
YP_wheel_ang()、YP_get_wheel_ang()を使って、vとwを直接制御することにしま
した。
YP_wheel_ang()はエンドエフェクタのPI制御を行う関数で
また、YP_get_wheel_ang()は2つのエンドエフェクタの現在角度を知ることがで
きる関数です。
今回のエンドエフェクタはアンテナの向いている方向です。
これらを用いて、アンテナの目標方位(Pan,Tilt)を与えるとそちらを向ける
関数を作成しました。関数の中身は以下になります。
------------------------------------------------------------------------
-------------------------------------
関数の中身
//======================================================================
========
//関数名：my_wheel_ang.c
//ユーザーは何の考えなしに移動をしてほしい角度を代入すると、(0,0)を基準
に
//指定した角度まで移動する
//右回りが＋、左回りが−
//gcc my_wheel_ang.c my_get_wheel_ang.c -o my_wheel_ang.exe -I /usr/
local/include -L /usr/local/lib -lypspur -lws2_32 -lm
//======================================================================
========
#include <stdio.h>
#include <ypspur.h>
#include <windows.h>
#include "my_get_wheel_ang.h" //座標取得のため
#define vel_P 5
#define vel_T 10
#define accel_P 0.5
#define accel_T 1
#define GEAR 100 //ギア比
//引数(パン、チルト、誤差)
double my_wheel_ang(double my_tar_P,double my_tar_T,double err_P,double
err_T,double *my_set_P,double *my_set_T){
//引数my_tar_P…パン
//引数my_tar_T…チルト
//引数err_P…パンの誤差範囲
//引数err_T…チルトの誤差範囲
double con_P; //目標値パン
double con_T; //目標値チルト
double log_P; //現在のパン
double log_T; //現在のチルト
double my_set_P_in; //残差を格納
double my_set_T_in; //残差を格納
int flag_P; //パンの停止用フラグa
int flag_T; //チルトの停止用フラグ
my_set_P_in = *my_set_P; //こっちにもmy_set_Pを格納
my_set_T_in = *my_set_T; //こっちにもmy_set_Tを格納
//計算式
con_P = my_set_P_in + my_tar_P; //目標値パン
con_T = my_set_T_in + my_tar_T; //目標値チルト
//------ギア比倍・正負入れ替え--------//
double con_T2 = (-1) *con_T * GEAR;
//角度指定を出す
YP_wheel_ang( con_P, con_T2 );
return 0;
}
/*----------------------------------------------------------------------
--------
int main(void)
{
double my_tar_P;
double my_tar_T;
double my_set_P;
double my_set_T;
double err_P = 0.01;
double err_T = 1;
//------PTUの初期設定--------------------------------------------------
--//
//------Spur初期化------//
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
//-------角速度の設定-------//
YP_set_wheel_vel( vel_P , vel_T );
//------角加速度の設定------//
YP_set_wheel_accel( accel_P , accel_T );
//---------------------------------------------------------------------
--------//
my_wheel_ang(Pan,Tilt,err_P,err_T,&my_set_P,&my_set_T);
}
*/
------------------------------------------------------------------------
--------------------------------------
//======================================================================
==========
//関数名:my_get_wheel_ang.c
//ユーザーがmy_get_wheel_angを行った場合、その条件での角度を取得する
//gcc my_get_wheel_ang.c -o my_get_wheel_ang.exe -I /usr/local/include -
L /usr/local/lib -lypspur -lws2_32 -lm
//======================================================================
==========
#include <stdio.h>
#include <ypspur.h>
#define vel_P 5
#define vel_T 10
#define accel_P 0.5
#define accel_T 1
#define GEAR 100
double my_get_wheel_ang( double *my_get_ang_P, double *my_get_ang_T,
double *my_set_P, double *my_set_T ){
//引数my_get_ang_P…パンの計算結果
//引数my_get_ang_T…チルト計算結果
//引数my_set_P…my_set_wheel_angで取得した差
//引数my_set_T…my_set_wheel_angで取得した差
double Spur_P ;
double Spur_T ; //起動した時からの角度を取得するための変数
double Spur_T_GEARwari;
//起動した時からの角度を取得する
YP_get_wheel_ang(&Spur_P,&Spur_T);
Spur_T_GEARwari = ((-1) * Spur_T / GEAR);
*my_get_ang_P = Spur_P - *my_set_P; //パン
*my_get_ang_T = Spur_T_GEARwari - *my_set_T; //チルト
return 0;
}
/*----------------------------------------------------------------------
--------
int main(void)
{
double my_get_ang_P;
double my_get_ang_T;
double my_set_P;//大域変数
double my_set_T;//大域変数
//------PTUの初期設定--------------------------------------------------
---//
//------Spur初期化----------------------//
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
//-------角速度の設定-------------------//
YP_set_wheel_vel( vel_P , vel_T );
//------角加速度の設定------------------//
YP_set_wheel_accel( accel_P , accel_T );
//---------------------------------------------------------------------
---//
my_get_wheel_ang( &my_get_ang_P, &my_get_ang_T, &my_set_P, &my_set_T );
}
*/
------------------------------------------------------------------------
-------------------------------------
//======================================================================
========
//関数名:my_set_wheel_ang.c
//ユーザーがwheelの角度を指定することができる
//gcc my_set_wheel_ang.c -o my_set_wheel_ang.exe -I /usr/local/include -
L /usr/local/lib -lypspur -lws2_32 -lm
//======================================================================
========
#include <stdio.h>
#include <ypspur.h>
#define vel_P 5
#define vel_T 10
#define accel_P 0.5
#define accel_T 1
#define GEAR 100
double my_set_wheel_ang( double my_P, double my_T, double *my_set_P,
double *my_set_T ){
//引数my_P…ユーザー指定のパン
//引数my_T…ユーザー指定のチルト
//引数&my_set_P…Spurのパンをユーザー指定の経度にした際の差
//引数&my_set_T…Spurのチルトをユーザー指定の緯度にした際の差
double Spur_P; //Spurのパン
double Spur_T; //Spurのチルト
double Spur_T_GEARwari;//Spurのチルトをギア比で割ったもの
//Spurが持っているパンチルトを取得する
YP_get_wheel_ang(&Spur_P,&Spur_T);
Spur_T_GEARwari = ((-1) * Spur_T / GEAR);
//差を計算する、この値をこの後も保存する
*my_set_P = Spur_P - my_P;
*my_set_T = Spur_T_GEARwari - my_T;
return 0;
}
/*----------------------------------------------------------------------
--------
int main(void)
{
double my_P;
double my_T;
double my_set_P;//大域変数
double my_set_T;//大域変数
//------PTUの初期設定--------------------------------------------------
---//
//------Spur初期化----------------------//
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
//-------角速度の設定-------------------//
YP_set_wheel_vel( vel_P , vel_T );
//------角加速度の設定------------------//
YP_set_wheel_accel( accel_P , accel_T );
//---------------------------------------------------------------------
---//
my_set_wheel_ang(my_P, my_T, &my_set_P, &my_set_T );
}
*/
------------------------------------------------------------------------
--------------------------------------
４）メイン関数の作成
３で作成した関数を使い、全体の制御プログラムを作成しました。
今回は目標角度を1秒おきに与えてそちらを向けるようにしました。
目標角度はヘリのGPS座標を通信で受け取り算出するのですが、
この報告とは関係ないので割愛します。
基本的にはYP-Spurの考え方と同じで、目標値を与えて、
制御はYP-Spurに任せつつも、常時それを監視する、という流れになります。
しかし、実際には1秒おきに目標値を勝手に更新しているので
あまりYP-Spurの考え方とは合致していないような気もします。
-----
メイン関数の初期化部分と制御部分
制御台の初期化部分
/*------Spur初期化------*/
if( Spur_init() < 0 ){
fprintf(stderr, "ERROR : cannot open spur.\n");
return -1;
}
/*-------角速度の設定-------*/
YP_set_wheel_vel( vel_P , vel_T );
/*------角加速度の設定------*/
YP_set_wheel_accel( accel_P , accel_T );
-----
制御部分
my_wheel_ang(Pan,Tilt,err_P,err_T,&my_set_P,&my_set_T);
Sleep(1000);//秒スリープ
-----
以上のシステムを作成したうえで、小浅間山で行ったデモビデオを
下記で公開しておきました。
https://www.youtube.com/watch?v=NaE3Uu-_t4k&feature=youtu.be
また、デモビデオではアンテナ台の動きが分かりにくいので、アンテナ台がアッ
プのものを別途に下記で公開しました。
https://www.youtube.com/watch?v=nBQFdLfKM6A&feature=youtu.be
私の勉強不足のため、まだ制御パラメタが調整しきれておらずガタついておりま
すが、
T-FrogドライバとYP-Spurのおかげで、非常に短い時間で作成することができま
した。
(機械部分開発が60日、回路部分開発が14日、ソフト50日ぐらい)
有用なソフトウェアをご提供いただき、誠にありがとうございました。
以上です。
長田大輝（ながただいき）
工学院大学 工学部 機械システム工学科
a211066 at ns dot kogakuin dot ac dot jp
References
-
from 長田大輝 <a211066 at ns dot kogakuin dot ac dot jp>Mon, 08 Dec 2014 13:02:46 +0900
2013 copyright (c) T-frog Project All Rights Reserved.