yuya horie
RobocupSSLのメイン基板白mbedのプログラム
Rootsロボット mainプログラム
~ Robocup SSL（小型車輪リーグ）ロボット ~
Robocup SSLとは
●試合構成
Robocup小型ロボットリーグ（Small Size League)は，直径180[mm]，高さ150[mm]以内のサイズのロボット6台が1チームとなり，オレンジ色のゴルフボールを使ってサッカー競技を行う自立型ロボットコンテストである.
フィールドの上には2台のWebカメラが設置され，フィールド上のロボットとボールを撮影する．Visionサーバは，フィールドの画像データよりロボットとボールの座標データを算出し，LANを用い各チームのAI用PCに送信する．Webカメラの撮影速度は，60[fps]である．レフリーボックスは，ファウルやフリーキック，スローインなどの審判の判定を入力し，LANを通じて各チームのAI用PCに送信する.それぞれのチームのAI用PCは，ロボットとボールの座標，審判の判定を元にロボットの移動，キックなどの作戦を決定し，無線によってロボットに指令を送信する．
ロボット機能紹介
●オムニホイールによる方向転換不要の全方位移動
オムニホイールは，自由に回転可能なローラをホイールの外周上に配置した車輪である．ローラの回転により，車輪の回転と垂直の方向に駆動力を発することはできないが移動は可能となる．各車輪の角速度を調整することによって全方向への移動を可能にする．
●ドリブルバーのバックスピンによるボール保持
●電磁力を利用したキッカー
●キッカーの電磁力エネルギーを充電する充電回路
●ロボット情報が一目でわかるLCD
Diff: fail_safe/fail_safe.cpp
- Revision:
- 24:37a074e767c8
- Parent:
- 23:0bb032ef1880
diff -r 0bb032ef1880 -r 37a074e767c8 fail_safe/fail_safe.cpp
--- a/fail_safe/fail_safe.cpp Sat Apr 28 07:18:22 2018 +0000
+++ b/fail_safe/fail_safe.cpp Sat Apr 28 19:47:31 2018 +0000
@@ -5,6 +5,7 @@
#include "parameter_manager.h"
#include "status_manager.h"
#include "fail_safe.h"
+#include "filter.h"
#include "pid.h"
#include "math.h"
@@ -15,26 +16,46 @@
#elif STM32
Ticker JdgFailTimer;
-LPF DribMot_Current(0.8);
-PID DribMot_PID(1.0,0.1,0,40);
+//LPF DribMot_Current(0.8);
+//PID DribMot_PID(1.0,0.1,0,40);
#endif
/* **変数定義** */
#ifndef EXCHANGE_REQUEST
#else
-
+//static float output_filter[2] = {0};
+//static float filter_coefficient = 0.8;
+//static float output_pid[4] = {0};
+//static float err[3] = {0};
+//static float Kp=1.0,Ki=0.1,Kd=0.0;
+//static float Integral_limit = 40;
#endif
//static float dribbleMotor_current[5] = {0};
/* **ローカル関数定義** */
void jdgFail(void);
-void jdgOverCur_DribMot(void);
+
/* **ローカル関数** */
void jdgFail(void)
{
- jdgOverCur_DribMot();
+ StatusManager::alive_count++;
+
+
+// StatusManager::dribbleMotor_current = DribMot_Current.getLPF_Out( InterfaceManager::dribbleMotor_current.read() * (float)3.3 * (float)9.091 );
+// DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current);
+// if( StatusManager::dribbleMotor_current > (float)JDG_TH_OVERCUR_DIRBMOT )
+// {
+// StatusManager::dribbleMot_OvrCur = 1;
+// InterfaceManager::dribbleMotor.write((float)1.0 );
+// }
+// else
+// {
+// StatusManager::dribbleMot_OvrCur = 0;
+// InterfaceManager::dribbleMotor.write((float)1.0 - DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current) * (float)0.01 );
+// }
+ //jdgOverCur_DribMot();
}
/* ************************************************************************************************************************************************* */
@@ -42,31 +63,31 @@
/* ************************************************************************************************************************************************* */
void jdgOverCur_DribMot(void)
{
- StatusManager::dribbleMotor_current = DribMot_Current.getLPF_Out( InterfaceManager::dribbleMotor_current.read() * (float)3.3 * (float)9.091 );
- DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current);
- if( StatusManager::dribbleMotor_current > (float)JDG_TH_OVERCUR_DIRBMOT )
- {
- StatusManager::dribbleMot_OvrCur = 1;
- InterfaceManager::dribbleMotor.write((float)1.0 );
- }
- else
- {
- StatusManager::dribbleMot_OvrCur = 0;
- InterfaceManager::dribbleMotor.write((float)1.0 - DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current) * (float)0.01 );
- }
+// StatusManager::dribbleMotor_current = DribMot_Current.getLPF_Out( InterfaceManager::dribbleMotor_current.read() * (float)3.3 * (float)9.091 );
+// DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current);
+// if( StatusManager::dribbleMotor_current > (float)JDG_TH_OVERCUR_DIRBMOT )
+// {
+// StatusManager::dribbleMot_OvrCur = 1;
+// InterfaceManager::dribbleMotor.write((float)1.0 );
+// }
+// else
+// {
+// StatusManager::dribbleMot_OvrCur = 0;
+// InterfaceManager::dribbleMotor.write((float)1.0 - DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current) * (float)0.01 );
+// }
-#if 0
- dribbleMotor_current[0] = InterfaceManager::dribbleMotor_current.read();
-
- StatusManager::dribbleMotor_current = ( dribbleMotor_current[0] + dribbleMotor_current[1] + dribbleMotor_current[2] + dribbleMotor_current[3] + dribbleMotor_current[4]) / (float)5.0 ;
- StatusManager::dribbleMotor_current -= (float)0.481;/* 0[A]オフセット ここ自動取得にする */
- StatusManager::dribbleMotor_current *= 3.3 * 9.091;
-
- dribbleMotor_current[4] = dribbleMotor_current[3];
- dribbleMotor_current[3] = dribbleMotor_current[2];
- dribbleMotor_current[2] = dribbleMotor_current[1];
- dribbleMotor_current[1] = dribbleMotor_current[0];
-#endif
+//#if 0
+// dribbleMotor_current[0] = InterfaceManager::dribbleMotor_current.read();
+//
+// StatusManager::dribbleMotor_current = ( dribbleMotor_current[0] + dribbleMotor_current[1] + dribbleMotor_current[2] + dribbleMotor_current[3] + dribbleMotor_current[4]) / (float)5.0 ;
+// StatusManager::dribbleMotor_current -= (float)0.481;/* 0[A]オフセット ここ自動取得にする */
+// StatusManager::dribbleMotor_current *= 3.3 * 9.091;
+//
+// dribbleMotor_current[4] = dribbleMotor_current[3];
+// dribbleMotor_current[3] = dribbleMotor_current[2];
+// dribbleMotor_current[2] = dribbleMotor_current[1];
+// dribbleMotor_current[1] = dribbleMotor_current[0];
+//#endif
}
/* ************************************************************************************************************************************************* */
@@ -83,7 +104,7 @@
void FailSafe::init(void)
{
- JdgFailTimer.attach(&jdgFail,JDG_CYCLE_1MS);
+ JdgFailTimer.attach(&jdgFail , JDG_CYCLE_1MS);
}
\ No newline at end of file