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@35:b1b00cfaa163, 2019-09-22 (annotated)

Committer:
alt0710
Date:
Sun Sep 22 10:23:42 2019 +0000
Revision:
35:b1b00cfaa163
Parent:
24:37a074e767c8 
2019JapanOpenSoft

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