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

actuator/wheel.cpp@32:718efbf4dc8a, 2018-05-06 (annotated)

Committer:
alt0710
Date:
Sun May 06 02:17:10 2018 +0000
Revision:
32:718efbf4dc8a
Parent:
31:7b003082f31d
Child:
33:13efae0ae6f4
2018JapanOpen?????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alt0710 7:1f1bdb4e4f33 1
alt0710 7:1f1bdb4e4f33 2 #include "mbed.h"
alt0710 7:1f1bdb4e4f33 3 #include "comm.h"
alt0710 7:1f1bdb4e4f33 4 #include "interface_manager.h"
alt0710 7:1f1bdb4e4f33 5 #include "parameter_manager.h"
alt0710 7:1f1bdb4e4f33 6 #include "status_manager.h"
alt0710 7:1f1bdb4e4f33 7 #include "wheel.h"
alt0710 7:1f1bdb4e4f33 8 #include "math.h"
alt0710 7:1f1bdb4e4f33 9
alt0710 7:1f1bdb4e4f33 10 /* **mbedクラス** */
alt0710 7:1f1bdb4e4f33 11 #ifdef LPC4088
alt0710 7:1f1bdb4e4f33 12
alt0710 7:1f1bdb4e4f33 13 #elif STM32
alt0710 7:1f1bdb4e4f33 14
alt0710 7:1f1bdb4e4f33 15 #endif
alt0710 7:1f1bdb4e4f33 16
alt0710 7:1f1bdb4e4f33 17 /* **ローカル関数定義** */
alt0710 7:1f1bdb4e4f33 18
alt0710 7:1f1bdb4e4f33 19 /* **ローカル関数** */
alt0710 7:1f1bdb4e4f33 20
alt0710 7:1f1bdb4e4f33 21 /* **グルーバル関数** */
alt0710 7:1f1bdb4e4f33 22
alt0710 7:1f1bdb4e4f33 23 /* **クラス** */
alt0710 7:1f1bdb4e4f33 24 class InterfaceManager;
alt0710 7:1f1bdb4e4f33 25 //メンバ変数の初期化は、定義順（Ｗａｒｎｉｎｇになる）
alt0710 7:1f1bdb4e4f33 26 Wheel::Wheel()
alt0710 7:1f1bdb4e4f33 27 {
alt0710 7:1f1bdb4e4f33 28 order.Mot0Order.Vel_short = 0;
alt0710 7:1f1bdb4e4f33 29 order.Mot1Order.Vel_short = 0;
alt0710 7:1f1bdb4e4f33 30 order.Mot2Order.Vel_short = 0;
alt0710 7:1f1bdb4e4f33 31 order.Mot3Order.Vel_short = 0;
alt0710 7:1f1bdb4e4f33 32 }
alt0710 7:1f1bdb4e4f33 33
alt0710 7:1f1bdb4e4f33 34 void Wheel::setVelocity(short V_value, short V_angle, short V_omega)
alt0710 7:1f1bdb4e4f33 35 {
alt0710 7:1f1bdb4e4f33 36 Vel_Value = V_value;
alt0710 7:1f1bdb4e4f33 37 Vel_Angle = V_angle;
alt0710 7:1f1bdb4e4f33 38 Vel_Omega = V_omega;
alt0710 7:1f1bdb4e4f33 39
alt0710 32:718efbf4dc8a 40 StatusManager::Angle = V_angle;
alt0710 32:718efbf4dc8a 41
alt0710 7:1f1bdb4e4f33 42 calcWheelVelocity();
alt0710 7:1f1bdb4e4f33 43
alt0710 7:1f1bdb4e4f33 44 InterfaceManager::can.setMotVel(order);
alt0710 7:1f1bdb4e4f33 45 }
alt0710 7:1f1bdb4e4f33 46
alt0710 7:1f1bdb4e4f33 47 void Wheel::calcWheelVelocity(void)
alt0710 7:1f1bdb4e4f33 48 {
alt0710 7:1f1bdb4e4f33 49 convertPhasorToVector();
alt0710 7:1f1bdb4e4f33 50 resolveVector();
alt0710 7:1f1bdb4e4f33 51 }
alt0710 7:1f1bdb4e4f33 52
alt0710 7:1f1bdb4e4f33 53 void Wheel::convertPhasorToVector(void)
alt0710 7:1f1bdb4e4f33 54 {
alt0710 7:1f1bdb4e4f33 55 double Vel_Angle_rad;
alt0710 7:1f1bdb4e4f33 56
alt0710 7:1f1bdb4e4f33 57 Vel_Angle_rad = (double)Vel_Angle * DEG2RAD;
alt0710 7:1f1bdb4e4f33 58 Vel_X = (double)Vel_Value * cos(Vel_Angle_rad);
alt0710 7:1f1bdb4e4f33 59 Vel_Y = (double)Vel_Value * sin(Vel_Angle_rad);
alt0710 7:1f1bdb4e4f33 60 Vel_OMEGA = (double)Vel_Omega;
alt0710 7:1f1bdb4e4f33 61 }
alt0710 7:1f1bdb4e4f33 62
alt0710 7:1f1bdb4e4f33 63 void Wheel::resolveVector(void)
alt0710 7:1f1bdb4e4f33 64 {
alt0710 7:1f1bdb4e4f33 65 double floor_value[4];
alt0710 7:1f1bdb4e4f33 66
alt0710 7:1f1bdb4e4f33 67 Vel_Mot0_double = operationV0(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 7:1f1bdb4e4f33 68 Vel_Mot1_double = operationV1(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 7:1f1bdb4e4f33 69 Vel_Mot2_double = operationV2(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 7:1f1bdb4e4f33 70 Vel_Mot3_double = operationV3(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 17:e5e7b19dd707 71
alt0710 17:e5e7b19dd707 72 // Vel_Mot0_double = -500.0;
alt0710 17:e5e7b19dd707 73 // Vel_Mot1_double = -500.0;
alt0710 17:e5e7b19dd707 74 // Vel_Mot2_double = -500.0;
alt0710 17:e5e7b19dd707 75 // Vel_Mot3_double = -500.0;
alt0710 17:e5e7b19dd707 76
alt0710 7:1f1bdb4e4f33 77
alt0710 17:e5e7b19dd707 78 Vel_Mot0_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 17:e5e7b19dd707 79 Vel_Mot1_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 17:e5e7b19dd707 80 Vel_Mot2_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 17:e5e7b19dd707 81 Vel_Mot3_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 7:1f1bdb4e4f33 82
alt0710 7:1f1bdb4e4f33 83
alt0710 7:1f1bdb4e4f33 84 if(Vel_Mot0_double > 0)floor_value[0] = 0.5;
alt0710 7:1f1bdb4e4f33 85 else if(Vel_Mot0_double < 0){ floor_value[0] = -0.5;}
alt0710 7:1f1bdb4e4f33 86 else {floor_value[0] = 0;}
alt0710 7:1f1bdb4e4f33 87
alt0710 7:1f1bdb4e4f33 88 if(Vel_Mot1_double > 0)floor_value[1] = 0.5;
alt0710 7:1f1bdb4e4f33 89 else if(Vel_Mot1_double < 0){ floor_value[1] = -0.5;}
alt0710 7:1f1bdb4e4f33 90 else {floor_value[1] = 0;}
alt0710 7:1f1bdb4e4f33 91
alt0710 7:1f1bdb4e4f33 92 if(Vel_Mot2_double > 0)floor_value[2] = 0.5;
alt0710 7:1f1bdb4e4f33 93 else if(Vel_Mot2_double < 0){ floor_value[2] = -0.5;}
alt0710 7:1f1bdb4e4f33 94 else {floor_value[2] = 0;}
alt0710 7:1f1bdb4e4f33 95
alt0710 7:1f1bdb4e4f33 96 if(Vel_Mot3_double > 0)floor_value[3] = 0.5;
alt0710 7:1f1bdb4e4f33 97 else if(Vel_Mot3_double < 0){ floor_value[3] = -0.5;}
alt0710 7:1f1bdb4e4f33 98 else {floor_value[3] = 0;}
alt0710 7:1f1bdb4e4f33 99
alt0710 7:1f1bdb4e4f33 100
alt0710 7:1f1bdb4e4f33 101 order.Mot0Order.Vel_short = (short)(Vel_Mot0_double + floor_value[0]);
alt0710 7:1f1bdb4e4f33 102 order.Mot1Order.Vel_short = (short)(Vel_Mot1_double + floor_value[1]);
alt0710 7:1f1bdb4e4f33 103 order.Mot2Order.Vel_short = (short)(Vel_Mot2_double + floor_value[2]);
alt0710 7:1f1bdb4e4f33 104 order.Mot3Order.Vel_short = (short)(Vel_Mot3_double + floor_value[3]);
alt0710 17:e5e7b19dd707 105
alt0710 17:e5e7b19dd707 106 #ifndef EXCHANGE_REQUEST
alt0710 17:e5e7b19dd707 107 order.Mot0Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 108 order.Mot1Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 109 order.Mot2Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 110 order.Mot3Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 111 #endif
alt0710 17:e5e7b19dd707 112 // order.Mot0Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 113 // order.Mot1Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 114 // order.Mot2Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 115 // order.Mot3Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 116
alt0710 32:718efbf4dc8a 117 StatusManager::Vel_Wheel1 = order.Mot0Order.Vel_short;
alt0710 32:718efbf4dc8a 118 StatusManager::Vel_Wheel2 = order.Mot1Order.Vel_short;
alt0710 32:718efbf4dc8a 119 StatusManager::Vel_Wheel3 = order.Mot2Order.Vel_short;
alt0710 7:1f1bdb4e4f33 120
alt0710 7:1f1bdb4e4f33 121 /* モータ軸がCCWで車輪がCWに回転するので，極性反転*/
alt0710 12:b7d7523733cd 122 // order.Mot0Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 123 // order.Mot1Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 124 // order.Mot2Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 125 // order.Mot3Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 126
alt0710 7:1f1bdb4e4f33 127 }
alt0710 7:1f1bdb4e4f33 128
alt0710 7:1f1bdb4e4f33 129 double Wheel::operationV0(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 130 {
alt0710 7:1f1bdb4e4f33 131 double v;
alt0710 7:1f1bdb4e4f33 132
alt0710 7:1f1bdb4e4f33 133 v = (CONST_V0_COEFFICIENT_VX_ * vx) + (CONST_V0_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 134
alt0710 7:1f1bdb4e4f33 135 return v;
alt0710 7:1f1bdb4e4f33 136 }
alt0710 7:1f1bdb4e4f33 137
alt0710 7:1f1bdb4e4f33 138 double Wheel::operationV1(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 139 {
alt0710 7:1f1bdb4e4f33 140 double v;
alt0710 7:1f1bdb4e4f33 141
alt0710 7:1f1bdb4e4f33 142 v = (CONST_V1_COEFFICIENT_VX_ * vx) + (CONST_V1_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 143
alt0710 7:1f1bdb4e4f33 144 return v;
alt0710 7:1f1bdb4e4f33 145 }
alt0710 7:1f1bdb4e4f33 146
alt0710 7:1f1bdb4e4f33 147 double Wheel::operationV2(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 148 {
alt0710 7:1f1bdb4e4f33 149 double v;
alt0710 7:1f1bdb4e4f33 150
alt0710 7:1f1bdb4e4f33 151 v = (CONST_V2_COEFFICIENT_VX_ * vx) + (CONST_V2_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 152
alt0710 7:1f1bdb4e4f33 153 return v;
alt0710 7:1f1bdb4e4f33 154 }
alt0710 7:1f1bdb4e4f33 155
alt0710 7:1f1bdb4e4f33 156 double Wheel::operationV3(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 157 {
alt0710 7:1f1bdb4e4f33 158 double v;
alt0710 7:1f1bdb4e4f33 159
alt0710 7:1f1bdb4e4f33 160 v = (CONST_V3_COEFFICIENT_VX_ * vx) + (CONST_V3_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 161
alt0710 7:1f1bdb4e4f33 162 return v;
alt0710 7:1f1bdb4e4f33 163 }