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@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 "pid.h"
alt0710 23:0bb032ef1880 8 #include "math.h"
alt0710 23:0bb032ef1880 9
alt0710 23:0bb032ef1880 10 /* **mbedクラス** */
alt0710 23:0bb032ef1880 11 #ifdef LPC4088
alt0710 23:0bb032ef1880 12
alt0710 23:0bb032ef1880 13 #elif STM32
alt0710 23:0bb032ef1880 14
alt0710 23:0bb032ef1880 15 #endif
alt0710 23:0bb032ef1880 16
alt0710 23:0bb032ef1880 17 /* **ローカル関数定義** */
alt0710 23:0bb032ef1880 18
alt0710 23:0bb032ef1880 19 /* **ローカル関数** */
alt0710 23:0bb032ef1880 20
alt0710 23:0bb032ef1880 21 /* **グルーバル関数** */
alt0710 23:0bb032ef1880 22
alt0710 23:0bb032ef1880 23 /* **クラス** */
alt0710 23:0bb032ef1880 24 class InterfaceManager;
alt0710 23:0bb032ef1880 25 //メンバ変数の初期化は、定義順（Ｗａｒｎｉｎｇになる）
alt0710 24:37a074e767c8 26 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)
alt0710 23:0bb032ef1880 27 {
alt0710 23:0bb032ef1880 28 output[0] = 0;
alt0710 23:0bb032ef1880 29 output[1] = 0;
alt0710 23:0bb032ef1880 30 output[2] = 0;
alt0710 23:0bb032ef1880 31 output[3] = 0;
alt0710 23:0bb032ef1880 32 err[0] = 0;
alt0710 23:0bb032ef1880 33 err[1] = 0;
alt0710 23:0bb032ef1880 34 err[2] = 0;
alt0710 23:0bb032ef1880 35 Kp = m_Kp;
alt0710 23:0bb032ef1880 36 Ki = m_Ki;
alt0710 23:0bb032ef1880 37 Kd = m_Kd;
alt0710 24:37a074e767c8 38 Integral_limit_max = m_Integral_limit_max;
alt0710 24:37a074e767c8 39 Integral_limit_min = m_Integral_limit_min;
alt0710 23:0bb032ef1880 40 }
alt0710 23:0bb032ef1880 41
alt0710 23:0bb032ef1880 42 float PID::getPID_Out(float req, float ref)
alt0710 23:0bb032ef1880 43 {
alt0710 23:0bb032ef1880 44 err[0] = req - ref;
alt0710 23:0bb032ef1880 45
alt0710 23:0bb032ef1880 46 output[0] = Kp * err[0];
alt0710 23:0bb032ef1880 47 output[1] = Ki * err[0] + output[1];
alt0710 23:0bb032ef1880 48 output[2] = Kd * (err[0] - err[1]);
alt0710 23:0bb032ef1880 49
alt0710 23:0bb032ef1880 50 err[1] = err[0];
alt0710 23:0bb032ef1880 51
alt0710 23:0bb032ef1880 52 output[3] = output[0] + output[1] + output[2];
alt0710 23:0bb032ef1880 53
alt0710 24:37a074e767c8 54 if( output[3] >= Integral_limit_max)
alt0710 23:0bb032ef1880 55 {
alt0710 24:37a074e767c8 56 output[1] = Integral_limit_max - (output[0] + output[2]);
alt0710 24:37a074e767c8 57 output[3] = Integral_limit_max;
alt0710 23:0bb032ef1880 58 }
alt0710 24:37a074e767c8 59 else if( output[3] <= Integral_limit_min)
alt0710 23:0bb032ef1880 60 {
alt0710 24:37a074e767c8 61 output[1] = Integral_limit_min - (output[0] + output[2]);
alt0710 24:37a074e767c8 62 output[3] = Integral_limit_min;
alt0710 23:0bb032ef1880 63
alt0710 23:0bb032ef1880 64 }
alt0710 23:0bb032ef1880 65
alt0710 23:0bb032ef1880 66 return output[3];
alt0710 23:0bb032ef1880 67 }
alt0710 23:0bb032ef1880 68