yuya horie / Mbed 2 deprecated Roots

RobocupSSLのメイン基板白mbedのプログラム

Dependencies:   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は，ロボットとボールの座標，審判の判定を元にロボットの移動，キックなどの作戦を決定し，無線によってロボットに指令を送信する． 700

ロボット機能紹介

●オムニホイールによる方向転換不要の全方位移動

オムニホイールは，自由に回転可能なローラをホイールの外周上に配置した車輪である．ローラの回転により，車輪の回転と垂直の方向に駆動力を発することはできないが移動は可能となる．各車輪の角速度を調整することによって全方向への移動を可能にする．
400

●ドリブルバーのバックスピンによるボール保持

●電磁力を利用したキッカー

●キッカーの電磁力エネルギーを充電する充電回路

●ロボット情報が一目でわかるLCD

fail_safe/fail_safe.cpp@23:0bb032ef1880, 2018-04-28 (annotated)

Committer:
alt0710
Date:
Sat Apr 28 07:18:22 2018 +0000
Revision:
23:0bb032ef1880
Child:
24:37a074e767c8
???????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alt0710 23:0bb032ef1880 1
alt0710 23:0bb032ef1880 2 #include "mbed.h"
alt0710 23:0bb032ef1880 3 #include "comm.h"
alt0710 23:0bb032ef1880 4 #include "interface_manager.h"
alt0710 23:0bb032ef1880 5 #include "parameter_manager.h"
alt0710 23:0bb032ef1880 6 #include "status_manager.h"
alt0710 23:0bb032ef1880 7 #include "fail_safe.h"
alt0710 23:0bb032ef1880 8 #include "pid.h"
alt0710 23:0bb032ef1880 9 #include "math.h"
alt0710 23:0bb032ef1880 10
alt0710 23:0bb032ef1880 11
alt0710 23:0bb032ef1880 12 /* **mbedクラス** */
alt0710 23:0bb032ef1880 13 #ifdef LPC4088
alt0710 23:0bb032ef1880 14
alt0710 23:0bb032ef1880 15 #elif STM32
alt0710 23:0bb032ef1880 16 Ticker JdgFailTimer;
alt0710 23:0bb032ef1880 17
alt0710 23:0bb032ef1880 18 LPF DribMot_Current(0.8);
alt0710 23:0bb032ef1880 19 PID DribMot_PID(1.0,0.1,0,40);
alt0710 23:0bb032ef1880 20 #endif
alt0710 23:0bb032ef1880 21
alt0710 23:0bb032ef1880 22 /* **変数定義** */
alt0710 23:0bb032ef1880 23 #ifndef EXCHANGE_REQUEST
alt0710 23:0bb032ef1880 24
alt0710 23:0bb032ef1880 25 #else
alt0710 23:0bb032ef1880 26
alt0710 23:0bb032ef1880 27 #endif
alt0710 23:0bb032ef1880 28 //static float dribbleMotor_current[5] = {0};
alt0710 23:0bb032ef1880 29
alt0710 23:0bb032ef1880 30
alt0710 23:0bb032ef1880 31 /* **ローカル関数定義** */
alt0710 23:0bb032ef1880 32 void jdgFail(void);
alt0710 23:0bb032ef1880 33 void jdgOverCur_DribMot(void);
alt0710 23:0bb032ef1880 34 /* **ローカル関数** */
alt0710 23:0bb032ef1880 35 void jdgFail(void)
alt0710 23:0bb032ef1880 36 {
alt0710 23:0bb032ef1880 37 jdgOverCur_DribMot();
alt0710 23:0bb032ef1880 38 }
alt0710 23:0bb032ef1880 39
alt0710 23:0bb032ef1880 40 /* ************************************************************************************************************************************************* */
alt0710 23:0bb032ef1880 41 /* ドリブルモータ制御　過電流検出 */
alt0710 23:0bb032ef1880 42 /* ************************************************************************************************************************************************* */
alt0710 23:0bb032ef1880 43 void jdgOverCur_DribMot(void)
alt0710 23:0bb032ef1880 44 {
alt0710 23:0bb032ef1880 45 StatusManager::dribbleMotor_current = DribMot_Current.getLPF_Out( InterfaceManager::dribbleMotor_current.read() * (float)3.3 * (float)9.091 );
alt0710 23:0bb032ef1880 46 DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current);
alt0710 23:0bb032ef1880 47 if( StatusManager::dribbleMotor_current > (float)JDG_TH_OVERCUR_DIRBMOT )
alt0710 23:0bb032ef1880 48 {
alt0710 23:0bb032ef1880 49 StatusManager::dribbleMot_OvrCur = 1;
alt0710 23:0bb032ef1880 50 InterfaceManager::dribbleMotor.write((float)1.0 );
alt0710 23:0bb032ef1880 51 }
alt0710 23:0bb032ef1880 52 else
alt0710 23:0bb032ef1880 53 {
alt0710 23:0bb032ef1880 54 StatusManager::dribbleMot_OvrCur = 0;
alt0710 23:0bb032ef1880 55 InterfaceManager::dribbleMotor.write((float)1.0 - DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current) * (float)0.01 );
alt0710 23:0bb032ef1880 56 }
alt0710 23:0bb032ef1880 57
alt0710 23:0bb032ef1880 58 #if 0
alt0710 23:0bb032ef1880 59 dribbleMotor_current[0] = InterfaceManager::dribbleMotor_current.read();
alt0710 23:0bb032ef1880 60
alt0710 23:0bb032ef1880 61 StatusManager::dribbleMotor_current = ( dribbleMotor_current[0] + dribbleMotor_current[1] + dribbleMotor_current[2] + dribbleMotor_current[3] + dribbleMotor_current[4]) / (float)5.0 ;
alt0710 23:0bb032ef1880 62 StatusManager::dribbleMotor_current -= (float)0.481;/* 0[A]オフセット　ここ自動取得にする */
alt0710 23:0bb032ef1880 63 StatusManager::dribbleMotor_current *= 3.3 * 9.091;
alt0710 23:0bb032ef1880 64
alt0710 23:0bb032ef1880 65 dribbleMotor_current[4] = dribbleMotor_current[3];
alt0710 23:0bb032ef1880 66 dribbleMotor_current[3] = dribbleMotor_current[2];
alt0710 23:0bb032ef1880 67 dribbleMotor_current[2] = dribbleMotor_current[1];
alt0710 23:0bb032ef1880 68 dribbleMotor_current[1] = dribbleMotor_current[0];
alt0710 23:0bb032ef1880 69 #endif
alt0710 23:0bb032ef1880 70
alt0710 23:0bb032ef1880 71 }
alt0710 23:0bb032ef1880 72 /* ************************************************************************************************************************************************* */
alt0710 23:0bb032ef1880 73 /* **グルーバル関数** */
alt0710 23:0bb032ef1880 74
alt0710 23:0bb032ef1880 75 /* **クラス** */
alt0710 23:0bb032ef1880 76 class InterfaceManager;
alt0710 23:0bb032ef1880 77
alt0710 23:0bb032ef1880 78 FailSafe::FailSafe()
alt0710 23:0bb032ef1880 79 {
alt0710 23:0bb032ef1880 80
alt0710 23:0bb032ef1880 81
alt0710 23:0bb032ef1880 82 }
alt0710 23:0bb032ef1880 83
alt0710 23:0bb032ef1880 84 void FailSafe::init(void)
alt0710 23:0bb032ef1880 85 {
alt0710 23:0bb032ef1880 86 JdgFailTimer.attach(&jdgFail,JDG_CYCLE_1MS);
alt0710 23:0bb032ef1880 87
alt0710 23:0bb032ef1880 88 }
alt0710 23:0bb032ef1880 89