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@23:0bb032ef1880, 2018-04-28 (annotated)

Committer:
alt0710
Date:
Sat Apr 28 07:18:22 2018 +0000
Revision:
23:0bb032ef1880
Child:
24:37a074e767c8
???????????????????????

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 23:0bb032ef1880 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 = (float)40.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 23:0bb032ef1880 38 Integral_limit = m_Integral_limit;
alt0710 23:0bb032ef1880 39 }
alt0710 23:0bb032ef1880 40
alt0710 23:0bb032ef1880 41 float PID::getPID_Out(float req, float ref)
alt0710 23:0bb032ef1880 42 {
alt0710 23:0bb032ef1880 43 err[0] = req - ref;
alt0710 23:0bb032ef1880 44
alt0710 23:0bb032ef1880 45 output[0] = Kp * err[0];
alt0710 23:0bb032ef1880 46 output[1] = Ki * err[0] + output[1];
alt0710 23:0bb032ef1880 47 output[2] = Kd * (err[0] - err[1]);
alt0710 23:0bb032ef1880 48
alt0710 23:0bb032ef1880 49 err[1] = err[0];
alt0710 23:0bb032ef1880 50
alt0710 23:0bb032ef1880 51 output[3] = output[0] + output[1] + output[2];
alt0710 23:0bb032ef1880 52
alt0710 23:0bb032ef1880 53 if( output[3] >= Integral_limit)
alt0710 23:0bb032ef1880 54 {
alt0710 23:0bb032ef1880 55 output[1] = Integral_limit - (output[0] + output[2]);
alt0710 23:0bb032ef1880 56 output[3] = Integral_limit;
alt0710 23:0bb032ef1880 57 }
alt0710 23:0bb032ef1880 58 else if( output[3] <= -Integral_limit)
alt0710 23:0bb032ef1880 59 {
alt0710 23:0bb032ef1880 60 output[1] = -Integral_limit - (output[0] + output[2]);
alt0710 23:0bb032ef1880 61 output[3] = -Integral_limit;
alt0710 23:0bb032ef1880 62
alt0710 23:0bb032ef1880 63 }
alt0710 23:0bb032ef1880 64
alt0710 23:0bb032ef1880 65 return output[3];
alt0710 23:0bb032ef1880 66 }
alt0710 23:0bb032ef1880 67