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

main.cpp

Committer:
alt0710
Date:
2018-05-03
Revision:
31:7b003082f31d
Parent:
30:8ee22331bfaf
Child:
32:718efbf4dc8a

File content as of revision 31:7b003082f31d:

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

Timer t;
LPF DribMot_Current(0.8);
PID DribMot_PID(1.0,0.1,0.0,40.0,0.0);

int main() {
    init_interface();
    init_status();
    
    ParameterManager::machine_id = 3;
    
    ActMode actmode;
    ParameterManager::raw_data.machine_id = ParameterManager::machine_id;
    ParameterManager::raw_data.vphasor_value_of_machine = 0;
    ParameterManager::raw_data.vphasor_angle_of_machine = 0;
    ParameterManager::raw_data.vw_of_machine = 127;
    ParameterManager::raw_data.actions_of_machine = 0;
    ParameterManager::raw_data.power = 0;

    ParameterManager::machine_data.v_mm_per_sec = 0; //MAX:4m/s
    ParameterManager::machine_data.angle_degree = 0;
    ParameterManager::machine_data.vw_mrad_per_sec = 0; 
//#ifndef ON_INDICATER
    InterfaceManager::i2c.readIDMsg();
//#endif    
    
    while(1) {
        if(StatusManager::ball == 1)
        {
            InterfaceManager::LED  =  1;
        }
        else
        {
            InterfaceManager::LED  =  0;
        }
        
        actmode.run();
#if defined(MAIN_BORD_Ver_4) || defined(MAIN_BORD_Ver_5)          
        StatusManager::dribbleMotor_current = DribMot_Current.getLPF_Out( (InterfaceManager::dribbleMotor_current.read() - (float)0.4841) * (float)3.3 * (float)9.091 );
        //StatusManager::dribbleMotor_current = DribMot_Current.getLPF_Out( InterfaceManager::dribbleMotor_current.read());
        
        StatusManager::req_dribbleMotor_PID = DribMot_PID.getPID_Out( StatusManager::req_dribbleMotor_current , StatusManager::dribbleMotor_current);
        // 指示０の時Duty出力を０にする　０.１Aは最低指示0.133以下であれば良い
        if(StatusManager::req_dribbleMotor_current < (float)0.1)
        {
            StatusManager::req_dribbleMotor_PID = 0.0;
        }
        
        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 - StatusManager::req_dribbleMotor_PID  * (float)0.01 );
        }
#endif
        t.stop();
        StatusManager::DriveTime = (short)t.read_us();
        t.reset();
        t.start();
        
    }
}