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@31:7b003082f31d, 2018-05-03 (annotated)

Committer:
alt0710
Date:
Thu May 03 08:04:30 2018 +0000
Revision:
31:7b003082f31d
Parent:
17:e5e7b19dd707
Child:
32:718efbf4dc8a
??????????; ?????statusmanager???

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 7:1f1bdb4e4f33 40 calcWheelVelocity();
alt0710 7:1f1bdb4e4f33 41
alt0710 7:1f1bdb4e4f33 42 InterfaceManager::can.setMotVel(order);
alt0710 7:1f1bdb4e4f33 43 }
alt0710 7:1f1bdb4e4f33 44
alt0710 7:1f1bdb4e4f33 45 void Wheel::calcWheelVelocity(void)
alt0710 7:1f1bdb4e4f33 46 {
alt0710 7:1f1bdb4e4f33 47 convertPhasorToVector();
alt0710 7:1f1bdb4e4f33 48 resolveVector();
alt0710 7:1f1bdb4e4f33 49 }
alt0710 7:1f1bdb4e4f33 50
alt0710 7:1f1bdb4e4f33 51 void Wheel::convertPhasorToVector(void)
alt0710 7:1f1bdb4e4f33 52 {
alt0710 7:1f1bdb4e4f33 53 double Vel_Angle_rad;
alt0710 7:1f1bdb4e4f33 54
alt0710 7:1f1bdb4e4f33 55 Vel_Angle_rad = (double)Vel_Angle * DEG2RAD;
alt0710 7:1f1bdb4e4f33 56 Vel_X = (double)Vel_Value * cos(Vel_Angle_rad);
alt0710 7:1f1bdb4e4f33 57 Vel_Y = (double)Vel_Value * sin(Vel_Angle_rad);
alt0710 7:1f1bdb4e4f33 58 Vel_OMEGA = (double)Vel_Omega;
alt0710 7:1f1bdb4e4f33 59 }
alt0710 7:1f1bdb4e4f33 60
alt0710 7:1f1bdb4e4f33 61 void Wheel::resolveVector(void)
alt0710 7:1f1bdb4e4f33 62 {
alt0710 7:1f1bdb4e4f33 63 double floor_value[4];
alt0710 7:1f1bdb4e4f33 64
alt0710 7:1f1bdb4e4f33 65 Vel_Mot0_double = operationV0(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 7:1f1bdb4e4f33 66 Vel_Mot1_double = operationV1(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 7:1f1bdb4e4f33 67 Vel_Mot2_double = operationV2(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 7:1f1bdb4e4f33 68 Vel_Mot3_double = operationV3(Vel_X, Vel_Y, Vel_OMEGA );
alt0710 17:e5e7b19dd707 69
alt0710 17:e5e7b19dd707 70 // Vel_Mot0_double = -500.0;
alt0710 17:e5e7b19dd707 71 // Vel_Mot1_double = -500.0;
alt0710 17:e5e7b19dd707 72 // Vel_Mot2_double = -500.0;
alt0710 17:e5e7b19dd707 73 // Vel_Mot3_double = -500.0;
alt0710 17:e5e7b19dd707 74
alt0710 7:1f1bdb4e4f33 75
alt0710 17:e5e7b19dd707 76 Vel_Mot0_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 17:e5e7b19dd707 77 Vel_Mot1_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 17:e5e7b19dd707 78 Vel_Mot2_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 17:e5e7b19dd707 79 Vel_Mot3_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ * 10;
alt0710 7:1f1bdb4e4f33 80
alt0710 7:1f1bdb4e4f33 81
alt0710 7:1f1bdb4e4f33 82 if(Vel_Mot0_double > 0)floor_value[0] = 0.5;
alt0710 7:1f1bdb4e4f33 83 else if(Vel_Mot0_double < 0){ floor_value[0] = -0.5;}
alt0710 7:1f1bdb4e4f33 84 else {floor_value[0] = 0;}
alt0710 7:1f1bdb4e4f33 85
alt0710 7:1f1bdb4e4f33 86 if(Vel_Mot1_double > 0)floor_value[1] = 0.5;
alt0710 7:1f1bdb4e4f33 87 else if(Vel_Mot1_double < 0){ floor_value[1] = -0.5;}
alt0710 7:1f1bdb4e4f33 88 else {floor_value[1] = 0;}
alt0710 7:1f1bdb4e4f33 89
alt0710 7:1f1bdb4e4f33 90 if(Vel_Mot2_double > 0)floor_value[2] = 0.5;
alt0710 7:1f1bdb4e4f33 91 else if(Vel_Mot2_double < 0){ floor_value[2] = -0.5;}
alt0710 7:1f1bdb4e4f33 92 else {floor_value[2] = 0;}
alt0710 7:1f1bdb4e4f33 93
alt0710 7:1f1bdb4e4f33 94 if(Vel_Mot3_double > 0)floor_value[3] = 0.5;
alt0710 7:1f1bdb4e4f33 95 else if(Vel_Mot3_double < 0){ floor_value[3] = -0.5;}
alt0710 7:1f1bdb4e4f33 96 else {floor_value[3] = 0;}
alt0710 7:1f1bdb4e4f33 97
alt0710 7:1f1bdb4e4f33 98
alt0710 7:1f1bdb4e4f33 99 order.Mot0Order.Vel_short = (short)(Vel_Mot0_double + floor_value[0]);
alt0710 7:1f1bdb4e4f33 100 order.Mot1Order.Vel_short = (short)(Vel_Mot1_double + floor_value[1]);
alt0710 7:1f1bdb4e4f33 101 order.Mot2Order.Vel_short = (short)(Vel_Mot2_double + floor_value[2]);
alt0710 7:1f1bdb4e4f33 102 order.Mot3Order.Vel_short = (short)(Vel_Mot3_double + floor_value[3]);
alt0710 17:e5e7b19dd707 103
alt0710 17:e5e7b19dd707 104 #ifndef EXCHANGE_REQUEST
alt0710 17:e5e7b19dd707 105 order.Mot0Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 106 order.Mot1Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 107 order.Mot2Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 108 order.Mot3Order.Vel_short *= 3;
alt0710 17:e5e7b19dd707 109 #endif
alt0710 17:e5e7b19dd707 110 // order.Mot0Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 111 // order.Mot1Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 112 // order.Mot2Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 113 // order.Mot3Order.Vel_short = 0;
alt0710 17:e5e7b19dd707 114
alt0710 31:7b003082f31d 115 StatusManager::Vel_Wheel1 = order.Mot1Order.Vel_short;
alt0710 31:7b003082f31d 116 StatusManager::Vel_Wheel2 = order.Mot2Order.Vel_short;
alt0710 31:7b003082f31d 117 StatusManager::Vel_Wheel3 = order.Mot3Order.Vel_short;
alt0710 7:1f1bdb4e4f33 118
alt0710 7:1f1bdb4e4f33 119 /* モータ軸がCCWで車輪がCWに回転するので，極性反転*/
alt0710 12:b7d7523733cd 120 // order.Mot0Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 121 // order.Mot1Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 122 // order.Mot2Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 123 // order.Mot3Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 124
alt0710 7:1f1bdb4e4f33 125 }
alt0710 7:1f1bdb4e4f33 126
alt0710 7:1f1bdb4e4f33 127 double Wheel::operationV0(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 128 {
alt0710 7:1f1bdb4e4f33 129 double v;
alt0710 7:1f1bdb4e4f33 130
alt0710 7:1f1bdb4e4f33 131 v = (CONST_V0_COEFFICIENT_VX_ * vx) + (CONST_V0_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 132
alt0710 7:1f1bdb4e4f33 133 return v;
alt0710 7:1f1bdb4e4f33 134 }
alt0710 7:1f1bdb4e4f33 135
alt0710 7:1f1bdb4e4f33 136 double Wheel::operationV1(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 137 {
alt0710 7:1f1bdb4e4f33 138 double v;
alt0710 7:1f1bdb4e4f33 139
alt0710 7:1f1bdb4e4f33 140 v = (CONST_V1_COEFFICIENT_VX_ * vx) + (CONST_V1_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 141
alt0710 7:1f1bdb4e4f33 142 return v;
alt0710 7:1f1bdb4e4f33 143 }
alt0710 7:1f1bdb4e4f33 144
alt0710 7:1f1bdb4e4f33 145 double Wheel::operationV2(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 146 {
alt0710 7:1f1bdb4e4f33 147 double v;
alt0710 7:1f1bdb4e4f33 148
alt0710 7:1f1bdb4e4f33 149 v = (CONST_V2_COEFFICIENT_VX_ * vx) + (CONST_V2_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 150
alt0710 7:1f1bdb4e4f33 151 return v;
alt0710 7:1f1bdb4e4f33 152 }
alt0710 7:1f1bdb4e4f33 153
alt0710 7:1f1bdb4e4f33 154 double Wheel::operationV3(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 155 {
alt0710 7:1f1bdb4e4f33 156 double v;
alt0710 7:1f1bdb4e4f33 157
alt0710 7:1f1bdb4e4f33 158 v = (CONST_V3_COEFFICIENT_VX_ * vx) + (CONST_V3_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 159
alt0710 7:1f1bdb4e4f33 160 return v;
alt0710 7:1f1bdb4e4f33 161 }