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

comm/pid/pid.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 "pid.h"
#include "math.h"

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

#elif  STM32

#endif

/* **ローカル関数定義** */

/* **ローカル関数** */

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

/* **クラス** */
class InterfaceManager;
//メンバ変数の初期化は、定義順（Ｗａｒｎｉｎｇになる）
PID::PID(float m_Kp = (float)1.0 , float m_Ki = (float)0.1 , float m_Kd = (float)0.0 , float m_Integral_limit_max = (float)40.0, float m_Integral_limit_min = (float)0.0)
{
   output[0] = 0;
   output[1] = 0;
   output[2] = 0;
   output[3] = 0;
   err[0] = 0;
   err[1] = 0;
   err[2] = 0;
   Kp = m_Kp;
   Ki = m_Ki;
   Kd = m_Kd;
   Integral_limit_max = m_Integral_limit_max;
   Integral_limit_min = m_Integral_limit_min;
}

float PID::getPID_Out(float req, float ref)
{
    err[0] = req - ref;
    
    output[0] = Kp * err[0];
    output[1] = Ki * err[0] + output[1];
    output[2] = Kd * (err[0] - err[1]);
    
    err[1] = err[0];
    
    output[3] = output[0] + output[1] + output[2];
    
    if( output[3] >= Integral_limit_max)
    {
        output[1] = Integral_limit_max - (output[0] + output[2]);
        output[3] = Integral_limit_max;
    }
    else if( output[3] <= Integral_limit_min)
    {
        output[1] = Integral_limit_min - (output[0] + output[2]);
        output[3] = Integral_limit_min;
        
    }
    
    return output[3];
}