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@12:b7d7523733cd, 2017-05-03 (annotated)

Committer:
alt0710
Date:
Wed May 03 03:35:32 2017 +0000
Revision:
12:b7d7523733cd
Parent:
7:1f1bdb4e4f33
Child:
17:e5e7b19dd707
dribbler????; I2C?CAN???????ver.

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 7:1f1bdb4e4f33 69
alt0710 7:1f1bdb4e4f33 70 Vel_Mot0_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ ;//* 10;
alt0710 7:1f1bdb4e4f33 71 Vel_Mot1_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ ;//* 10;
alt0710 7:1f1bdb4e4f33 72 Vel_Mot2_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ ;//* 10;
alt0710 7:1f1bdb4e4f33 73 Vel_Mot3_double *= CONST_GEER_RATIO_ / CONST_WHEEL_RADIUS_ ;//* 10;
alt0710 7:1f1bdb4e4f33 74
alt0710 7:1f1bdb4e4f33 75
alt0710 7:1f1bdb4e4f33 76 if(Vel_Mot0_double > 0)floor_value[0] = 0.5;
alt0710 7:1f1bdb4e4f33 77 else if(Vel_Mot0_double < 0){ floor_value[0] = -0.5;}
alt0710 7:1f1bdb4e4f33 78 else {floor_value[0] = 0;}
alt0710 7:1f1bdb4e4f33 79
alt0710 7:1f1bdb4e4f33 80 if(Vel_Mot1_double > 0)floor_value[1] = 0.5;
alt0710 7:1f1bdb4e4f33 81 else if(Vel_Mot1_double < 0){ floor_value[1] = -0.5;}
alt0710 7:1f1bdb4e4f33 82 else {floor_value[1] = 0;}
alt0710 7:1f1bdb4e4f33 83
alt0710 7:1f1bdb4e4f33 84 if(Vel_Mot2_double > 0)floor_value[2] = 0.5;
alt0710 7:1f1bdb4e4f33 85 else if(Vel_Mot2_double < 0){ floor_value[2] = -0.5;}
alt0710 7:1f1bdb4e4f33 86 else {floor_value[2] = 0;}
alt0710 7:1f1bdb4e4f33 87
alt0710 7:1f1bdb4e4f33 88 if(Vel_Mot3_double > 0)floor_value[3] = 0.5;
alt0710 7:1f1bdb4e4f33 89 else if(Vel_Mot3_double < 0){ floor_value[3] = -0.5;}
alt0710 7:1f1bdb4e4f33 90 else {floor_value[3] = 0;}
alt0710 7:1f1bdb4e4f33 91
alt0710 7:1f1bdb4e4f33 92
alt0710 7:1f1bdb4e4f33 93 order.Mot0Order.Vel_short = (short)(Vel_Mot0_double + floor_value[0]);
alt0710 7:1f1bdb4e4f33 94 order.Mot1Order.Vel_short = (short)(Vel_Mot1_double + floor_value[1]);
alt0710 7:1f1bdb4e4f33 95 order.Mot2Order.Vel_short = (short)(Vel_Mot2_double + floor_value[2]);
alt0710 7:1f1bdb4e4f33 96 order.Mot3Order.Vel_short = (short)(Vel_Mot3_double + floor_value[3]);
alt0710 7:1f1bdb4e4f33 97
alt0710 7:1f1bdb4e4f33 98
alt0710 7:1f1bdb4e4f33 99 /* モータ軸がCCWで車輪がCWに回転するので，極性反転*/
alt0710 12:b7d7523733cd 100 // order.Mot0Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 101 // order.Mot1Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 102 // order.Mot2Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 103 // order.Mot3Order.Vel_short *= -1;
alt0710 12:b7d7523733cd 104
alt0710 7:1f1bdb4e4f33 105 }
alt0710 7:1f1bdb4e4f33 106
alt0710 7:1f1bdb4e4f33 107 double Wheel::operationV0(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 108 {
alt0710 7:1f1bdb4e4f33 109 double v;
alt0710 7:1f1bdb4e4f33 110
alt0710 7:1f1bdb4e4f33 111 v = (CONST_V0_COEFFICIENT_VX_ * vx) + (CONST_V0_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 112
alt0710 7:1f1bdb4e4f33 113 return v;
alt0710 7:1f1bdb4e4f33 114 }
alt0710 7:1f1bdb4e4f33 115
alt0710 7:1f1bdb4e4f33 116 double Wheel::operationV1(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 117 {
alt0710 7:1f1bdb4e4f33 118 double v;
alt0710 7:1f1bdb4e4f33 119
alt0710 7:1f1bdb4e4f33 120 v = (CONST_V1_COEFFICIENT_VX_ * vx) + (CONST_V1_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 121
alt0710 7:1f1bdb4e4f33 122 return v;
alt0710 7:1f1bdb4e4f33 123 }
alt0710 7:1f1bdb4e4f33 124
alt0710 7:1f1bdb4e4f33 125 double Wheel::operationV2(double vx,double vy, double vw)
alt0710 7:1f1bdb4e4f33 126 {
alt0710 7:1f1bdb4e4f33 127 double v;
alt0710 7:1f1bdb4e4f33 128
alt0710 7:1f1bdb4e4f33 129 v = (CONST_V2_COEFFICIENT_VX_ * vx) + (CONST_V2_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 130
alt0710 7:1f1bdb4e4f33 131 return v;
alt0710 7:1f1bdb4e4f33 132 }
alt0710 7:1f1bdb4e4f33 133
alt0710 7:1f1bdb4e4f33 134 double Wheel::operationV3(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_V3_COEFFICIENT_VX_ * vx) + (CONST_V3_COEFFICIENT_VY_ * vy) + (CONST_MACHINE_RADIUS_ * vw );
alt0710 7:1f1bdb4e4f33 139
alt0710 7:1f1bdb4e4f33 140 return v;
alt0710 7:1f1bdb4e4f33 141 }