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

mode/operation_check/operation_check.cpp@25:15f75825fc36, 2018-04-29 (annotated)

Committer:
alt0710
Date:
Sun Apr 29 08:51:32 2018 +0000
Revision:
25:15f75825fc36
Parent:
23:0bb032ef1880
Child:
32:718efbf4dc8a
????????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alt0710 14:f6cc949a8046 1
alt0710 14:f6cc949a8046 2 #include "mbed.h"
alt0710 14:f6cc949a8046 3 #include "comm.h"
alt0710 14:f6cc949a8046 4 #include "mode.h"
alt0710 14:f6cc949a8046 5 #include "interface_manager.h"
alt0710 14:f6cc949a8046 6 #include "status_manager.h"
alt0710 14:f6cc949a8046 7 #include "parameter_manager.h"
alt0710 14:f6cc949a8046 8 #include "operation_check.h"
alt0710 14:f6cc949a8046 9
alt0710 14:f6cc949a8046 10 /* **mbedクラス** */
alt0710 14:f6cc949a8046 11 #ifdef LPC4088
alt0710 14:f6cc949a8046 12
alt0710 14:f6cc949a8046 13 #elif STM32
alt0710 25:15f75825fc36 14 Timeout time_charge;
alt0710 14:f6cc949a8046 15 #endif
alt0710 14:f6cc949a8046 16
alt0710 25:15f75825fc36 17 static char m_charge;
alt0710 14:f6cc949a8046 18
alt0710 25:15f75825fc36 19 /* **ローカル関数定義** */
alt0710 25:15f75825fc36 20 void charge_off(void);
alt0710 14:f6cc949a8046 21 /* **ローカル関数** */
alt0710 25:15f75825fc36 22 void charge_off(void)
alt0710 25:15f75825fc36 23 {
alt0710 25:15f75825fc36 24 m_charge = 0;
alt0710 25:15f75825fc36 25 time_charge.detach();
alt0710 25:15f75825fc36 26 }
alt0710 14:f6cc949a8046 27
alt0710 14:f6cc949a8046 28 /* **グルーバル関数** */
alt0710 14:f6cc949a8046 29
alt0710 14:f6cc949a8046 30 /* **クラス** */
alt0710 14:f6cc949a8046 31 class InterfaceManager;
alt0710 14:f6cc949a8046 32 class StatusManager;
alt0710 14:f6cc949a8046 33 class ParameterManager;
alt0710 14:f6cc949a8046 34
alt0710 14:f6cc949a8046 35
alt0710 25:15f75825fc36 36
alt0710 14:f6cc949a8046 37 //メンバ変数の初期化は、定義順（Ｗａｒｎｉｎｇになる）
alt0710 14:f6cc949a8046 38 OperationCheck::OperationCheck()
alt0710 20:354ed2681d0a 39 :checkMode(SELECT),
alt0710 20:354ed2681d0a 40 button_count(0),
alt0710 20:354ed2681d0a 41 sideSW_num(0),
alt0710 15:2bf8eafacef8 42 charge_flag(0),
alt0710 15:2bf8eafacef8 43 kick_flag(0),
alt0710 19:0bbd90be9aa3 44 power(0),
alt0710 21:d18c2dfaaba4 45 ball(0),
alt0710 21:d18c2dfaaba4 46 vel_value(0),
alt0710 21:d18c2dfaaba4 47 vel_angle(0)
alt0710 14:f6cc949a8046 48 {
alt0710 14:f6cc949a8046 49 }
alt0710 14:f6cc949a8046 50
alt0710 14:f6cc949a8046 51 void OperationCheck::run(void)
alt0710 14:f6cc949a8046 52 {
alt0710 20:354ed2681d0a 53 switch(checkMode)
alt0710 15:2bf8eafacef8 54 {
alt0710 20:354ed2681d0a 55 case SELECT:
alt0710 20:354ed2681d0a 56 selectCheckMode();
alt0710 20:354ed2681d0a 57 viewSelect();
alt0710 21:d18c2dfaaba4 58 resetValue();
alt0710 20:354ed2681d0a 59 break;
alt0710 20:354ed2681d0a 60
alt0710 20:354ed2681d0a 61 case WHEEL:
alt0710 21:d18c2dfaaba4 62 //sprintf(str,"WHEEL");
alt0710 21:d18c2dfaaba4 63 checkWheel();
alt0710 20:354ed2681d0a 64 checkBack();
alt0710 20:354ed2681d0a 65 break;
alt0710 20:354ed2681d0a 66
alt0710 20:354ed2681d0a 67 case DRIBBLE:
alt0710 20:354ed2681d0a 68 sprintf(str,"DRIBBLE");
alt0710 21:d18c2dfaaba4 69 checkDribble();
alt0710 20:354ed2681d0a 70 checkBack();
alt0710 20:354ed2681d0a 71 break;
alt0710 20:354ed2681d0a 72
alt0710 20:354ed2681d0a 73 case STR_KICK:
alt0710 20:354ed2681d0a 74 sprintf(str,"STR_KICK");
alt0710 21:d18c2dfaaba4 75 checkStrKick();
alt0710 20:354ed2681d0a 76 checkBack();
alt0710 20:354ed2681d0a 77 break;
alt0710 20:354ed2681d0a 78
alt0710 20:354ed2681d0a 79 case CHIP_KICK:
alt0710 20:354ed2681d0a 80 sprintf(str,"CHO_KICK");
alt0710 21:d18c2dfaaba4 81 checkChpKick();
alt0710 20:354ed2681d0a 82 checkBack();
alt0710 20:354ed2681d0a 83 break;
alt0710 20:354ed2681d0a 84
alt0710 20:354ed2681d0a 85 default:
alt0710 20:354ed2681d0a 86 sprintf(str,"ERROR");
alt0710 20:354ed2681d0a 87 break;
alt0710 15:2bf8eafacef8 88 }
alt0710 21:d18c2dfaaba4 89
alt0710 21:d18c2dfaaba4 90 if(InterfaceManager::ball_sensor == 0)
alt0710 21:d18c2dfaaba4 91 {
alt0710 21:d18c2dfaaba4 92 InterfaceManager::LED = 0;
alt0710 21:d18c2dfaaba4 93 }
alt0710 21:d18c2dfaaba4 94 else if(InterfaceManager::ball_sensor == 1)
alt0710 21:d18c2dfaaba4 95 {
alt0710 21:d18c2dfaaba4 96 InterfaceManager::LED = 1;
alt0710 21:d18c2dfaaba4 97 }
alt0710 20:354ed2681d0a 98 // //充電：左
alt0710 20:354ed2681d0a 99 // //強制放電：下
alt0710 20:354ed2681d0a 100 // //キック：上
alt0710 20:354ed2681d0a 101 // /* ***************** 充電 ******************************* */
alt0710 20:354ed2681d0a 102 // if(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
alt0710 20:354ed2681d0a 103 // {
alt0710 20:354ed2681d0a 104 // charge_flag = 1;
alt0710 20:354ed2681d0a 105 // }
alt0710 20:354ed2681d0a 106 // if(StatusManager::is_kicking == 1)
alt0710 20:354ed2681d0a 107 // {
alt0710 20:354ed2681d0a 108 // charge_flag = 0;
alt0710 20:354ed2681d0a 109 // kick_flag = 0;
alt0710 20:354ed2681d0a 110 // }
alt0710 20:354ed2681d0a 111 //
alt0710 20:354ed2681d0a 112 // /* ***************** キック ******************************* */
alt0710 20:354ed2681d0a 113 // if(InterfaceManager::button.getButtonStatus(CROSS_UP))
alt0710 20:354ed2681d0a 114 // {
alt0710 20:354ed2681d0a 115 // kick_flag = 1;
alt0710 20:354ed2681d0a 116 // }
alt0710 20:354ed2681d0a 117 //
alt0710 20:354ed2681d0a 118 // /* ***************** 威力 ******************************* */
alt0710 20:354ed2681d0a 119 // if(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
alt0710 20:354ed2681d0a 120 // {
alt0710 20:354ed2681d0a 121 // power++;
alt0710 20:354ed2681d0a 122 // if(power > 15)
alt0710 20:354ed2681d0a 123 // {
alt0710 20:354ed2681d0a 124 // power = 0;
alt0710 20:354ed2681d0a 125 // }
alt0710 20:354ed2681d0a 126 // }
alt0710 20:354ed2681d0a 127 //
alt0710 20:354ed2681d0a 128 // if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
alt0710 20:354ed2681d0a 129 // {
alt0710 20:354ed2681d0a 130 // dribble.setdribble(1, power);
alt0710 20:354ed2681d0a 131 // }
alt0710 20:354ed2681d0a 132 // else
alt0710 20:354ed2681d0a 133 // {
alt0710 20:354ed2681d0a 134 // dribble.setdribble(0, 0);
alt0710 20:354ed2681d0a 135 // }
alt0710 20:354ed2681d0a 136 //
alt0710 20:354ed2681d0a 137 // if(InterfaceManager::ball_sensor)
alt0710 20:354ed2681d0a 138 // {
alt0710 20:354ed2681d0a 139 // ball = 1;
alt0710 20:354ed2681d0a 140 // }
alt0710 20:354ed2681d0a 141 // else
alt0710 20:354ed2681d0a 142 // {
alt0710 20:354ed2681d0a 143 // ball = 0;
alt0710 20:354ed2681d0a 144 // }
alt0710 20:354ed2681d0a 145 // /* ***************** 強制放電 ***************************** */
alt0710 20:354ed2681d0a 146 //// kicker.setCharge(charge_flag);
alt0710 20:354ed2681d0a 147 //// kicker.setKick(kick_flag, STRAIGHT, power);
alt0710 20:354ed2681d0a 148 //// kicker.forceFireKick();
alt0710 20:354ed2681d0a 149 //
alt0710 20:354ed2681d0a 150 // //createLcdData();
alt0710 20:354ed2681d0a 151 // sprintf(str,"F:%dP:%d\nB:%dK:%d",charge_flag,power,ball,kick_flag);
alt0710 20:354ed2681d0a 152 // //sprintf(str,"Hello");
alt0710 20:354ed2681d0a 153 InterfaceManager::i2c.setLCDMsg(str,sizeof(str));
alt0710 20:354ed2681d0a 154
alt0710 20:354ed2681d0a 155 }
alt0710 20:354ed2681d0a 156
alt0710 20:354ed2681d0a 157 void OperationCheck::selectCheckMode(void)
alt0710 20:354ed2681d0a 158 {
alt0710 20:354ed2681d0a 159 if(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
alt0710 15:2bf8eafacef8 160 {
alt0710 20:354ed2681d0a 161 button_count++;
alt0710 15:2bf8eafacef8 162 }
alt0710 14:f6cc949a8046 163
alt0710 20:354ed2681d0a 164 if(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
alt0710 15:2bf8eafacef8 165 {
alt0710 20:354ed2681d0a 166 button_count--;
alt0710 20:354ed2681d0a 167 }
alt0710 20:354ed2681d0a 168
alt0710 20:354ed2681d0a 169 if(button_count > 3)
alt0710 20:354ed2681d0a 170 {
alt0710 20:354ed2681d0a 171 button_count = 0;
alt0710 20:354ed2681d0a 172 }
alt0710 20:354ed2681d0a 173 if(button_count < 0)
alt0710 20:354ed2681d0a 174 {
alt0710 20:354ed2681d0a 175 button_count = 3;
alt0710 15:2bf8eafacef8 176 }
alt0710 15:2bf8eafacef8 177
alt0710 20:354ed2681d0a 178 if(InterfaceManager::button.getButtonStatus(CROSS_UP))
alt0710 15:2bf8eafacef8 179 {
alt0710 20:354ed2681d0a 180 switch(button_count)
alt0710 15:2bf8eafacef8 181 {
alt0710 20:354ed2681d0a 182 case 0:
alt0710 20:354ed2681d0a 183 checkMode = WHEEL;
alt0710 20:354ed2681d0a 184 break;
alt0710 20:354ed2681d0a 185
alt0710 20:354ed2681d0a 186 case 1:
alt0710 20:354ed2681d0a 187 checkMode = DRIBBLE;
alt0710 20:354ed2681d0a 188 break;
alt0710 20:354ed2681d0a 189
alt0710 20:354ed2681d0a 190 case 2:
alt0710 20:354ed2681d0a 191 checkMode = STR_KICK;
alt0710 20:354ed2681d0a 192 break;
alt0710 20:354ed2681d0a 193
alt0710 20:354ed2681d0a 194 case 3:
alt0710 20:354ed2681d0a 195 checkMode = CHIP_KICK;
alt0710 20:354ed2681d0a 196 break;
alt0710 20:354ed2681d0a 197
alt0710 20:354ed2681d0a 198 default:
alt0710 20:354ed2681d0a 199 checkMode = SELECT;
alt0710 20:354ed2681d0a 200 break;
alt0710 15:2bf8eafacef8 201 }
alt0710 15:2bf8eafacef8 202 }
alt0710 20:354ed2681d0a 203
alt0710 20:354ed2681d0a 204 }
alt0710 19:0bbd90be9aa3 205
alt0710 20:354ed2681d0a 206 void OperationCheck::viewSelect(void)
alt0710 20:354ed2681d0a 207 {
alt0710 20:354ed2681d0a 208 switch(button_count)
alt0710 19:0bbd90be9aa3 209 {
alt0710 20:354ed2681d0a 210 case 0:
alt0710 20:354ed2681d0a 211 sprintf(str,"CHECK\nWHEEL");
alt0710 20:354ed2681d0a 212 break;
alt0710 20:354ed2681d0a 213
alt0710 20:354ed2681d0a 214 case 1:
alt0710 20:354ed2681d0a 215 sprintf(str,"CHECK\nDRIBBLE");
alt0710 20:354ed2681d0a 216 break;
alt0710 20:354ed2681d0a 217
alt0710 20:354ed2681d0a 218 case 2:
alt0710 20:354ed2681d0a 219 sprintf(str,"CHECK\nSTR_KICK");
alt0710 20:354ed2681d0a 220 break;
alt0710 20:354ed2681d0a 221
alt0710 20:354ed2681d0a 222 case 3:
alt0710 20:354ed2681d0a 223 sprintf(str,"CHECK\nCHP_KICK");
alt0710 20:354ed2681d0a 224 break;
alt0710 20:354ed2681d0a 225
alt0710 20:354ed2681d0a 226 default:
alt0710 20:354ed2681d0a 227 sprintf(str,"ERROR");
alt0710 20:354ed2681d0a 228 break;
alt0710 20:354ed2681d0a 229 }
alt0710 20:354ed2681d0a 230 }
alt0710 20:354ed2681d0a 231
alt0710 20:354ed2681d0a 232 void OperationCheck::checkBack(void)
alt0710 20:354ed2681d0a 233 {
alt0710 20:354ed2681d0a 234 if(InterfaceManager::button.getButtonStatus(CROSS_UP))
alt0710 20:354ed2681d0a 235 {
alt0710 20:354ed2681d0a 236 checkMode = SELECT;
alt0710 19:0bbd90be9aa3 237 }
alt0710 15:2bf8eafacef8 238
alt0710 14:f6cc949a8046 239 }
alt0710 14:f6cc949a8046 240
alt0710 14:f6cc949a8046 241 void OperationCheck::createLcdData(void)
alt0710 14:f6cc949a8046 242 {
alt0710 14:f6cc949a8046 243
alt0710 14:f6cc949a8046 244 /*
alt0710 14:f6cc949a8046 245
alt0710 14:f6cc949a8046 246 * 0 : 正常終了
alt0710 14:f6cc949a8046 247
alt0710 14:f6cc949a8046 248 * -1 : タイムアウト
alt0710 14:f6cc949a8046 249
alt0710 14:f6cc949a8046 250 * -2 : オーバーランエラー
alt0710 14:f6cc949a8046 251
alt0710 14:f6cc949a8046 252 * -3 : フレーミングエラー
alt0710 14:f6cc949a8046 253
alt0710 14:f6cc949a8046 254 * 1 : 受信中
alt0710 14:f6cc949a8046 255
alt0710 14:f6cc949a8046 256 * 2 : フレームリーダー1まで受信
alt0710 14:f6cc949a8046 257
alt0710 14:f6cc949a8046 258 * 3 : フレームリーダー2まで受信
alt0710 14:f6cc949a8046 259
alt0710 14:f6cc949a8046 260 * 4 : マシンIDまで受信
alt0710 14:f6cc949a8046 261
alt0710 14:f6cc949a8046 262 */
alt0710 14:f6cc949a8046 263 switch(StatusManager::uart){
alt0710 14:f6cc949a8046 264 case 0:
alt0710 14:f6cc949a8046 265 sprintf(str,"ID%d EN:\nComplete",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 266
alt0710 14:f6cc949a8046 267 break;
alt0710 14:f6cc949a8046 268
alt0710 14:f6cc949a8046 269 case 1:
alt0710 14:f6cc949a8046 270 break;
alt0710 14:f6cc949a8046 271
alt0710 14:f6cc949a8046 272 case 2:
alt0710 14:f6cc949a8046 273 sprintf(str,"ID%d EN:\nHeader1",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 274
alt0710 14:f6cc949a8046 275 break;
alt0710 14:f6cc949a8046 276
alt0710 14:f6cc949a8046 277 case 3:
alt0710 14:f6cc949a8046 278 sprintf(str,"ID%d EN:\nHeader2",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 279
alt0710 14:f6cc949a8046 280 break;
alt0710 14:f6cc949a8046 281
alt0710 14:f6cc949a8046 282 case 4:
alt0710 14:f6cc949a8046 283 sprintf(str,"ID%d EN:\nCheckER1",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 284
alt0710 14:f6cc949a8046 285 break;
alt0710 14:f6cc949a8046 286
alt0710 14:f6cc949a8046 287 case 5:
alt0710 14:f6cc949a8046 288 sprintf(str,"ID%d EN:\nCheckER2",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 289
alt0710 14:f6cc949a8046 290 break;
alt0710 14:f6cc949a8046 291
alt0710 14:f6cc949a8046 292 case -1:
alt0710 14:f6cc949a8046 293 sprintf(str,"ID%d EN:\nTimeOut",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 294
alt0710 14:f6cc949a8046 295 break;
alt0710 14:f6cc949a8046 296
alt0710 14:f6cc949a8046 297 case -2:
alt0710 14:f6cc949a8046 298 sprintf(str,"ID%d EN:\nOERR",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 299
alt0710 14:f6cc949a8046 300 break;
alt0710 14:f6cc949a8046 301
alt0710 14:f6cc949a8046 302 case -3:
alt0710 14:f6cc949a8046 303 sprintf(str,"ID%d EN:\nFERR",ParameterManager::machine_id);
alt0710 14:f6cc949a8046 304
alt0710 14:f6cc949a8046 305 break;
alt0710 14:f6cc949a8046 306
alt0710 14:f6cc949a8046 307 default:
alt0710 14:f6cc949a8046 308 sprintf(str,"ID%d EN: \n %d",ParameterManager::machine_id, StatusManager::uart);
alt0710 14:f6cc949a8046 309 break;
alt0710 14:f6cc949a8046 310 }
alt0710 14:f6cc949a8046 311
alt0710 14:f6cc949a8046 312 }
alt0710 21:d18c2dfaaba4 313
alt0710 21:d18c2dfaaba4 314 void OperationCheck::checkWheel(void)
alt0710 21:d18c2dfaaba4 315 {
alt0710 21:d18c2dfaaba4 316 switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
alt0710 21:d18c2dfaaba4 317 {
alt0710 21:d18c2dfaaba4 318 case SINGLE_CLICK:
alt0710 21:d18c2dfaaba4 319 vel_angle += 10;
alt0710 21:d18c2dfaaba4 320 break;
alt0710 21:d18c2dfaaba4 321
alt0710 21:d18c2dfaaba4 322 case DOUBLE_CLICK:
alt0710 21:d18c2dfaaba4 323 vel_angle -= 10;
alt0710 21:d18c2dfaaba4 324 break;
alt0710 21:d18c2dfaaba4 325 default:
alt0710 21:d18c2dfaaba4 326
alt0710 21:d18c2dfaaba4 327 break;
alt0710 21:d18c2dfaaba4 328 }
alt0710 21:d18c2dfaaba4 329
alt0710 21:d18c2dfaaba4 330 if(vel_angle < 0)
alt0710 21:d18c2dfaaba4 331 {
alt0710 21:d18c2dfaaba4 332 vel_angle = 360;
alt0710 21:d18c2dfaaba4 333 }
alt0710 21:d18c2dfaaba4 334 if(vel_angle > 360)
alt0710 21:d18c2dfaaba4 335 {
alt0710 21:d18c2dfaaba4 336 vel_angle = 0;
alt0710 21:d18c2dfaaba4 337 }
alt0710 21:d18c2dfaaba4 338
alt0710 21:d18c2dfaaba4 339 switch(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
alt0710 21:d18c2dfaaba4 340 {
alt0710 21:d18c2dfaaba4 341 case SINGLE_CLICK:
alt0710 21:d18c2dfaaba4 342 vel_value += 10;
alt0710 21:d18c2dfaaba4 343 break;
alt0710 21:d18c2dfaaba4 344
alt0710 21:d18c2dfaaba4 345 case DOUBLE_CLICK:
alt0710 21:d18c2dfaaba4 346 vel_value -= 10;
alt0710 21:d18c2dfaaba4 347 break;
alt0710 21:d18c2dfaaba4 348 default:
alt0710 21:d18c2dfaaba4 349
alt0710 21:d18c2dfaaba4 350 break;
alt0710 21:d18c2dfaaba4 351 }
alt0710 21:d18c2dfaaba4 352
alt0710 23:0bb032ef1880 353 if(vel_value < -100)
alt0710 21:d18c2dfaaba4 354 {
alt0710 23:0bb032ef1880 355 vel_value = 0;
alt0710 21:d18c2dfaaba4 356 }
alt0710 23:0bb032ef1880 357 if(vel_value > 100)
alt0710 21:d18c2dfaaba4 358 {
alt0710 23:0bb032ef1880 359 vel_value = 0;
alt0710 21:d18c2dfaaba4 360 }
alt0710 21:d18c2dfaaba4 361
alt0710 21:d18c2dfaaba4 362 if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
alt0710 21:d18c2dfaaba4 363 {
alt0710 21:d18c2dfaaba4 364 wheel.setVelocity(vel_value,vel_angle,0);
alt0710 21:d18c2dfaaba4 365 }
alt0710 21:d18c2dfaaba4 366 else
alt0710 21:d18c2dfaaba4 367 {
alt0710 21:d18c2dfaaba4 368 wheel.setVelocity(0,0,0);
alt0710 21:d18c2dfaaba4 369 }
alt0710 21:d18c2dfaaba4 370
alt0710 21:d18c2dfaaba4 371 sprintf(str,"V:%d\nA:%d",vel_value,vel_angle);
alt0710 21:d18c2dfaaba4 372
alt0710 21:d18c2dfaaba4 373
alt0710 21:d18c2dfaaba4 374 }
alt0710 21:d18c2dfaaba4 375 void OperationCheck::checkDribble(void)
alt0710 21:d18c2dfaaba4 376 {
alt0710 21:d18c2dfaaba4 377 switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
alt0710 21:d18c2dfaaba4 378 {
alt0710 21:d18c2dfaaba4 379 case SINGLE_CLICK:
alt0710 21:d18c2dfaaba4 380 power += 1;
alt0710 21:d18c2dfaaba4 381 break;
alt0710 21:d18c2dfaaba4 382
alt0710 21:d18c2dfaaba4 383 case DOUBLE_CLICK:
alt0710 21:d18c2dfaaba4 384 power -= 1;
alt0710 21:d18c2dfaaba4 385 break;
alt0710 21:d18c2dfaaba4 386 default:
alt0710 21:d18c2dfaaba4 387
alt0710 21:d18c2dfaaba4 388 break;
alt0710 21:d18c2dfaaba4 389 }
alt0710 21:d18c2dfaaba4 390
alt0710 21:d18c2dfaaba4 391 if(power < 0)
alt0710 21:d18c2dfaaba4 392 {
alt0710 21:d18c2dfaaba4 393 power = 15;
alt0710 21:d18c2dfaaba4 394 }
alt0710 21:d18c2dfaaba4 395 if(power > 15)
alt0710 21:d18c2dfaaba4 396 {
alt0710 21:d18c2dfaaba4 397 power = 0;
alt0710 21:d18c2dfaaba4 398 }
alt0710 21:d18c2dfaaba4 399
alt0710 21:d18c2dfaaba4 400 if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
alt0710 21:d18c2dfaaba4 401 {
alt0710 21:d18c2dfaaba4 402 dribble.setdribble(1,power);
alt0710 21:d18c2dfaaba4 403 }
alt0710 21:d18c2dfaaba4 404 else
alt0710 21:d18c2dfaaba4 405 {
alt0710 21:d18c2dfaaba4 406 dribble.setdribble(0,0);
alt0710 21:d18c2dfaaba4 407 }
alt0710 21:d18c2dfaaba4 408
alt0710 21:d18c2dfaaba4 409
alt0710 21:d18c2dfaaba4 410
alt0710 25:15f75825fc36 411 //sprintf(str,"DRIBBLE\nPOWER:%d",power);
alt0710 25:15f75825fc36 412 sprintf(str,"POWER:%d\nC:%f",power,StatusManager::dribbleMotor_current);
alt0710 21:d18c2dfaaba4 413
alt0710 21:d18c2dfaaba4 414 }
alt0710 21:d18c2dfaaba4 415 void OperationCheck::checkStrKick(void)
alt0710 21:d18c2dfaaba4 416 {
alt0710 21:d18c2dfaaba4 417 switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
alt0710 21:d18c2dfaaba4 418 {
alt0710 21:d18c2dfaaba4 419 case SINGLE_CLICK:
alt0710 21:d18c2dfaaba4 420 power += 1;
alt0710 21:d18c2dfaaba4 421 break;
alt0710 21:d18c2dfaaba4 422
alt0710 21:d18c2dfaaba4 423 case DOUBLE_CLICK:
alt0710 21:d18c2dfaaba4 424 power -= 1;
alt0710 21:d18c2dfaaba4 425 break;
alt0710 21:d18c2dfaaba4 426 default:
alt0710 21:d18c2dfaaba4 427
alt0710 21:d18c2dfaaba4 428 break;
alt0710 21:d18c2dfaaba4 429 }
alt0710 21:d18c2dfaaba4 430
alt0710 21:d18c2dfaaba4 431 if(power < 0)
alt0710 21:d18c2dfaaba4 432 {
alt0710 21:d18c2dfaaba4 433 power = 15;
alt0710 21:d18c2dfaaba4 434 }
alt0710 21:d18c2dfaaba4 435 if(power > 15)
alt0710 21:d18c2dfaaba4 436 {
alt0710 21:d18c2dfaaba4 437 power = 0;
alt0710 21:d18c2dfaaba4 438 }
alt0710 21:d18c2dfaaba4 439
alt0710 21:d18c2dfaaba4 440 switch(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
alt0710 21:d18c2dfaaba4 441 {
alt0710 21:d18c2dfaaba4 442 case SINGLE_CLICK:
alt0710 25:15f75825fc36 443 //charge_flag = 1;
alt0710 25:15f75825fc36 444 m_charge = 1;
alt0710 25:15f75825fc36 445 time_charge.attach(&charge_off,4.0);
alt0710 21:d18c2dfaaba4 446 break;
alt0710 21:d18c2dfaaba4 447
alt0710 21:d18c2dfaaba4 448 case DOUBLE_CLICK:
alt0710 25:15f75825fc36 449 m_charge = 0;
alt0710 25:15f75825fc36 450 //charge_flag = 0;
alt0710 21:d18c2dfaaba4 451 break;
alt0710 21:d18c2dfaaba4 452 default:
alt0710 21:d18c2dfaaba4 453
alt0710 21:d18c2dfaaba4 454 break;
alt0710 21:d18c2dfaaba4 455 }
alt0710 25:15f75825fc36 456 charge_flag = m_charge;
alt0710 25:15f75825fc36 457
alt0710 21:d18c2dfaaba4 458
alt0710 21:d18c2dfaaba4 459 if(StatusManager::is_kicking == 1)
alt0710 21:d18c2dfaaba4 460 {
alt0710 21:d18c2dfaaba4 461 charge_flag = 0;
alt0710 21:d18c2dfaaba4 462 kick_flag = 0;
alt0710 21:d18c2dfaaba4 463 }
alt0710 21:d18c2dfaaba4 464 kicker.setCharge(charge_flag);
alt0710 21:d18c2dfaaba4 465
alt0710 21:d18c2dfaaba4 466 if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
alt0710 21:d18c2dfaaba4 467 {
alt0710 21:d18c2dfaaba4 468 kick_flag = 1;
alt0710 21:d18c2dfaaba4 469 }
alt0710 21:d18c2dfaaba4 470 else
alt0710 21:d18c2dfaaba4 471 {
alt0710 21:d18c2dfaaba4 472 kick_flag = 0;
alt0710 21:d18c2dfaaba4 473 }
alt0710 21:d18c2dfaaba4 474 kicker.setKick(kick_flag,0,power);
alt0710 21:d18c2dfaaba4 475 kicker.forceFireKick();
alt0710 21:d18c2dfaaba4 476
alt0710 21:d18c2dfaaba4 477 sprintf(str,"C:%dF:%d\nPOWER:%d",charge_flag,StatusManager::charge_end,power);
alt0710 21:d18c2dfaaba4 478 }
alt0710 21:d18c2dfaaba4 479 void OperationCheck::checkChpKick(void)
alt0710 21:d18c2dfaaba4 480 {
alt0710 22:653b40db5a60 481 switch(InterfaceManager::button.getButtonStatus(CROSS_RIGHT))
alt0710 22:653b40db5a60 482 {
alt0710 22:653b40db5a60 483 case SINGLE_CLICK:
alt0710 22:653b40db5a60 484 power += 1;
alt0710 22:653b40db5a60 485 break;
alt0710 22:653b40db5a60 486
alt0710 22:653b40db5a60 487 case DOUBLE_CLICK:
alt0710 22:653b40db5a60 488 power -= 1;
alt0710 22:653b40db5a60 489 break;
alt0710 22:653b40db5a60 490 default:
alt0710 22:653b40db5a60 491
alt0710 22:653b40db5a60 492 break;
alt0710 22:653b40db5a60 493 }
alt0710 21:d18c2dfaaba4 494
alt0710 22:653b40db5a60 495 if(power < 0)
alt0710 22:653b40db5a60 496 {
alt0710 22:653b40db5a60 497 power = 15;
alt0710 22:653b40db5a60 498 }
alt0710 22:653b40db5a60 499 if(power > 15)
alt0710 22:653b40db5a60 500 {
alt0710 22:653b40db5a60 501 power = 0;
alt0710 22:653b40db5a60 502 }
alt0710 22:653b40db5a60 503
alt0710 22:653b40db5a60 504 switch(InterfaceManager::button.getButtonStatus(CROSS_LEFT))
alt0710 22:653b40db5a60 505 {
alt0710 22:653b40db5a60 506 case SINGLE_CLICK:
alt0710 22:653b40db5a60 507 charge_flag = 1;
alt0710 22:653b40db5a60 508 break;
alt0710 22:653b40db5a60 509
alt0710 22:653b40db5a60 510 case DOUBLE_CLICK:
alt0710 22:653b40db5a60 511 charge_flag = 0;
alt0710 22:653b40db5a60 512 break;
alt0710 22:653b40db5a60 513 default:
alt0710 22:653b40db5a60 514
alt0710 22:653b40db5a60 515 break;
alt0710 22:653b40db5a60 516 }
alt0710 22:653b40db5a60 517
alt0710 22:653b40db5a60 518 if(StatusManager::is_kicking == 1)
alt0710 22:653b40db5a60 519 {
alt0710 22:653b40db5a60 520 charge_flag = 0;
alt0710 22:653b40db5a60 521 kick_flag = 0;
alt0710 22:653b40db5a60 522 }
alt0710 22:653b40db5a60 523 kicker.setCharge(charge_flag);
alt0710 22:653b40db5a60 524
alt0710 22:653b40db5a60 525 if(InterfaceManager::button.getButtonStatus(CROSS_DOWN))
alt0710 22:653b40db5a60 526 {
alt0710 22:653b40db5a60 527 kick_flag = 1;
alt0710 22:653b40db5a60 528 }
alt0710 22:653b40db5a60 529 else
alt0710 22:653b40db5a60 530 {
alt0710 22:653b40db5a60 531 kick_flag = 0;
alt0710 22:653b40db5a60 532 }
alt0710 22:653b40db5a60 533 kicker.setKick(kick_flag,1,power);
alt0710 22:653b40db5a60 534 kicker.forceFireKick();
alt0710 22:653b40db5a60 535
alt0710 22:653b40db5a60 536 sprintf(str,"C:%dF:%d\nPOWER:%d",charge_flag,StatusManager::charge_end,power);
alt0710 21:d18c2dfaaba4 537 }
alt0710 21:d18c2dfaaba4 538
alt0710 21:d18c2dfaaba4 539 void OperationCheck::resetValue(void)
alt0710 21:d18c2dfaaba4 540 {
alt0710 21:d18c2dfaaba4 541 vel_value = 0;
alt0710 21:d18c2dfaaba4 542 vel_angle = 0;
alt0710 21:d18c2dfaaba4 543 power = 0;
alt0710 21:d18c2dfaaba4 544 charge_flag = 0;
alt0710 21:d18c2dfaaba4 545 kick_flag = 0;
alt0710 21:d18c2dfaaba4 546 }