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@34:857b6cca2021, 2019-09-22 (annotated)

Committer:
alt0710
Date:
Sun Sep 22 10:21:31 2019 +0000
Revision:
34:857b6cca2021
Parent:
33:13efae0ae6f4 
test

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 34:857b6cca2021 101 // order.Mot0Order.Vel_short = -150;
alt0710 34:857b6cca2021 102 // order.Mot1Order.Vel_short = -150;
alt0710 34:857b6cca2021 103 // order.Mot2Order.Vel_short = 300;
alt0710 34:857b6cca2021 104 // order.Mot3Order.Vel_short = 0;
alt0710 33:13efae0ae6f4 105
alt0710 34:857b6cca2021 106 order.Mot0Order.Vel_short = (short)(Vel_Mot0_double + floor_value[0]);
alt0710 34:857b6cca2021 107 order.Mot1Order.Vel_short = (short)(Vel_Mot1_double + floor_value[1]);
alt0710 34:857b6cca2021 108 order.Mot2Order.Vel_short = (short)(Vel_Mot2_double + floor_value[2]);
alt0710 34:857b6cca2021 109 order.Mot3Order.Vel_short = (short)(Vel_Mot3_double + floor_value[3]);
alt0710 17:e5e7b19dd707 110
alt0710 17:e5e7b19dd707 111 #ifndef EXCHANGE_REQUEST
alt0710 17:e5e7b19dd707 112 order.Mot0Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 113 order.Mot1Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 114 order.Mot2Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 115 order.Mot3Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 116 #endif
alt0710 17:e5e7b19dd707 117 // order.Mot0Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 118 // order.Mot1Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 119 // order.Mot2Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 120 // order.Mot3Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 121
alt0710 32:718efbf4dc8a 122 StatusManager::Vel_Wheel1 = order.Mot0Order.Vel_short;
alt0710 32:718efbf4dc8a 123 StatusManager::Vel_Wheel2 = order.Mot1Order.Vel_short;
alt0710 32:718efbf4dc8a 124 StatusManager::Vel_Wheel3 = order.Mot2Order.Vel_short;
alt0710 7:1f1bdb4e4f33 125
alt0710 7:1f1bdb4e4f33 126 /* モータ軸がCCWで車輪がCWに回転するので，極性反転*/
alt0710 12:b7d7523733cd 127 // order.Mot0Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 128 // order.Mot1Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 129 // order.Mot2Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 130 // order.Mot3Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 131
alt0710 7:1f1bdb4e4f33 132 }
alt0710 7:1f1bdb4e4f33 133
alt0710 7:1f1bdb4e4f33 134 double Wheel::operationV0(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 135 {
alt0710 7:1f1bdb4e4f33 136 double v;
alt0710 7:1f1bdb4e4f33 137
alt0710 7:1f1bdb4e4f33 138 v = (CONST_V0_COEFFICIENT_VX_ * vx) + (CONST_V0_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 139
alt0710 7:1f1bdb4e4f33 140 return v;
alt0710 7:1f1bdb4e4f33 141 }
alt0710 7:1f1bdb4e4f33 142
alt0710 7:1f1bdb4e4f33 143 double Wheel::operationV1(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 144 {
alt0710 7:1f1bdb4e4f33 145 double v;
alt0710 7:1f1bdb4e4f33 146
alt0710 7:1f1bdb4e4f33 147 v = (CONST_V1_COEFFICIENT_VX_ * vx) + (CONST_V1_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 148
alt0710 7:1f1bdb4e4f33 149 return v;
alt0710 7:1f1bdb4e4f33 150 }
alt0710 7:1f1bdb4e4f33 151
alt0710 7:1f1bdb4e4f33 152 double Wheel::operationV2(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 153 {
alt0710 7:1f1bdb4e4f33 154 double v;
alt0710 7:1f1bdb4e4f33 155
alt0710 7:1f1bdb4e4f33 156 v = (CONST_V2_COEFFICIENT_VX_ * vx) + (CONST_V2_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 157
alt0710 7:1f1bdb4e4f33 158 return v;
alt0710 7:1f1bdb4e4f33 159 }
alt0710 7:1f1bdb4e4f33 160
alt0710 7:1f1bdb4e4f33 161 double Wheel::operationV3(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 162 {
alt0710 7:1f1bdb4e4f33 163 double v;
alt0710 7:1f1bdb4e4f33 164
alt0710 7:1f1bdb4e4f33 165 v = (CONST_V3_COEFFICIENT_VX_ * vx) + (CONST_V3_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 166
alt0710 7:1f1bdb4e4f33 167 return v;
alt0710 7:1f1bdb4e4f33 168 }