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

Committer:
alt0710
Date:
2019-09-22
Revision:
35:b1b00cfaa163
Parent:
24:37a074e767c8

File content as of revision 35:b1b00cfaa163:


#include "mbed.h"
#include "comm.h"
#include "interface_manager.h"
#include "parameter_manager.h"
#include "status_manager.h"
#include "fail_safe.h"
#include "filter.h"
#include "pid.h"
#include "math.h"


/* **mbedクラス** */
#ifdef LPC4088

#elif  STM32
Ticker JdgFailTimer;

//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 jdgFail(void)
{
    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();      
}

/* ************************************************************************************************************************************************* */
/*  ドリブルモータ制御　過電流検出                                                                                                                          */
/* ************************************************************************************************************************************************* */
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 );
//     }
     
//#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   

}
/* ************************************************************************************************************************************************* */
/* **グルーバル関数** */

/* **クラス** */
class InterfaceManager;

FailSafe::FailSafe()
{
        
    
}

void FailSafe::init(void)
{
    JdgFailTimer.attach(&jdgFail , JDG_CYCLE_1MS);
    
}