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

Committer:
alt0710
Date:
Sun Sep 22 10:21:31 2019 +0000
Revision:
34:857b6cca2021
Parent:
31:7b003082f31d 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alt0710 6:21b3fbe57b88 1
alt0710 6:21b3fbe57b88 2 #include "mbed.h"
alt0710 6:21b3fbe57b88 3 #include "comm.h"
alt0710 6:21b3fbe57b88 4 #include "mode.h"
alt0710 6:21b3fbe57b88 5 #include "interface_manager.h"
alt0710 6:21b3fbe57b88 6 #include "status_manager.h"
alt0710 6:21b3fbe57b88 7 #include "parameter_manager.h"
alt0710 6:21b3fbe57b88 8 #include "normal_routine.h"
alt0710 6:21b3fbe57b88 9
alt0710 6:21b3fbe57b88 10 /* **mbedクラス** */
alt0710 6:21b3fbe57b88 11 #ifdef LPC4088
alt0710 6:21b3fbe57b88 12
alt0710 6:21b3fbe57b88 13 #elif STM32
alt0710 6:21b3fbe57b88 14
alt0710 6:21b3fbe57b88 15 #endif
alt0710 6:21b3fbe57b88 16
alt0710 30:8ee22331bfaf 17 #ifdef ON_INDICATER
alt0710 30:8ee22331bfaf 18 static char batVoltage[1];
alt0710 30:8ee22331bfaf 19 #endif
alt0710 24:37a074e767c8 20
alt0710 6:21b3fbe57b88 21 /* **ローカル関数定義** */
alt0710 6:21b3fbe57b88 22
alt0710 6:21b3fbe57b88 23 /* **ローカル関数** */
alt0710 6:21b3fbe57b88 24
alt0710 6:21b3fbe57b88 25 /* **グルーバル関数** */
alt0710 6:21b3fbe57b88 26
alt0710 6:21b3fbe57b88 27 /* **クラス** */
alt0710 6:21b3fbe57b88 28 class InterfaceManager;
alt0710 7:1f1bdb4e4f33 29 class StatusManager;
alt0710 7:1f1bdb4e4f33 30 class ParameterManager;
alt0710 7:1f1bdb4e4f33 31
alt0710 23:0bb032ef1880 32
alt0710 23:0bb032ef1880 33
alt0710 6:21b3fbe57b88 34 //メンバ変数の初期化は、定義順（Ｗａｒｎｉｎｇになる）
alt0710 6:21b3fbe57b88 35 NormalRoutine::NormalRoutine()
alt0710 23:0bb032ef1880 36 {
alt0710 23:0bb032ef1880 37 failsafe.init();
alt0710 6:21b3fbe57b88 38 }
alt0710 6:21b3fbe57b88 39
alt0710 6:21b3fbe57b88 40 void NormalRoutine::run(void)
alt0710 6:21b3fbe57b88 41 {
alt0710 7:1f1bdb4e4f33 42 packet.splitPacket();
alt0710 7:1f1bdb4e4f33 43
alt0710 7:1f1bdb4e4f33 44 //Kick
alt0710 7:1f1bdb4e4f33 45 kicker.setCharge(ParameterManager::machine_data.charge_flag);
alt0710 7:1f1bdb4e4f33 46 kicker.setKick(ParameterManager::machine_data.kick_flag,ParameterManager::machine_data.kick_type,ParameterManager::machine_data.kickers_power);
alt0710 10:9cd22f673968 47 kicker.forceFireKick();
alt0710 7:1f1bdb4e4f33 48
alt0710 7:1f1bdb4e4f33 49 //Wheel
alt0710 15:2bf8eafacef8 50 wheel.setVelocity(ParameterManager::machine_data.v_mm_per_sec,ParameterManager::machine_data.angle_degree,ParameterManager::machine_data.vw_mrad_per_sec);
alt0710 15:2bf8eafacef8 51 //wheel.setVelocity(5000,0,0);
alt0710 14:f6cc949a8046 52
alt0710 11:7f5259ad738c 53 //Drible
alt0710 14:f6cc949a8046 54 dribble.setdribble(ParameterManager::machine_data.dribble_flag, ParameterManager::machine_data.dribbler_power);
alt0710 18:8db19ccf7e02 55 //dribble.setdribble(1, 3);
alt0710 7:1f1bdb4e4f33 56
alt0710 23:0bb032ef1880 57 StatusManager::supply_voltage = InterfaceManager::batVoltage.read();
alt0710 31:7b003082f31d 58 StatusManager::supply_voltage *= (float)3.3 * (float)6.251;
alt0710 23:0bb032ef1880 59
alt0710 23:0bb032ef1880 60
alt0710 23:0bb032ef1880 61
alt0710 24:37a074e767c8 62 //sprintf(str,"T%dOv:%d\nC:%3.2f",StatusManager::DriveTime,StatusManager::dribbleMot_OvrCur,StatusManager::dribbleMotor_current);
alt0710 7:1f1bdb4e4f33 63 createLcdData();
alt0710 24:37a074e767c8 64 InterfaceManager::i2c.setLCDMsg(str,sizeof(str));
alt0710 30:8ee22331bfaf 65
alt0710 30:8ee22331bfaf 66 #ifdef ON_INDICATER
alt0710 30:8ee22331bfaf 67 batVoltage[0] = (char)(StatusManager::supply_voltage * 10);
alt0710 31:7b003082f31d 68 //batVoltage[0] = 150;
alt0710 30:8ee22331bfaf 69 InterfaceManager::i2c.setIndicateMsg( batVoltage , sizeof(batVoltage));
alt0710 30:8ee22331bfaf 70 #endif
alt0710 30:8ee22331bfaf 71
alt0710 6:21b3fbe57b88 72 }
alt0710 6:21b3fbe57b88 73
alt0710 7:1f1bdb4e4f33 74 void NormalRoutine::createLcdData(void)
alt0710 7:1f1bdb4e4f33 75 {
alt0710 7:1f1bdb4e4f33 76
alt0710 7:1f1bdb4e4f33 77 /*
alt0710 7:1f1bdb4e4f33 78
alt0710 7:1f1bdb4e4f33 79 * 0 : 正常終了
alt0710 7:1f1bdb4e4f33 80
alt0710 7:1f1bdb4e4f33 81 * -1 : タイムアウト
alt0710 7:1f1bdb4e4f33 82
alt0710 7:1f1bdb4e4f33 83 * -2 : オーバーランエラー
alt0710 7:1f1bdb4e4f33 84
alt0710 7:1f1bdb4e4f33 85 * -3 : フレーミングエラー
alt0710 7:1f1bdb4e4f33 86
alt0710 7:1f1bdb4e4f33 87 * 1 : 受信中
alt0710 7:1f1bdb4e4f33 88
alt0710 7:1f1bdb4e4f33 89 * 2 : フレームリーダー1まで受信
alt0710 7:1f1bdb4e4f33 90
alt0710 7:1f1bdb4e4f33 91 * 3 : フレームリーダー2まで受信
alt0710 7:1f1bdb4e4f33 92
alt0710 7:1f1bdb4e4f33 93 * 4 : マシンIDまで受信
alt0710 7:1f1bdb4e4f33 94
alt0710 7:1f1bdb4e4f33 95 */
alt0710 7:1f1bdb4e4f33 96 switch(StatusManager::uart){
alt0710 7:1f1bdb4e4f33 97 case 0:
alt0710 31:7b003082f31d 98 //sprintf(str,"ID%dB%3.1f\nComplete",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 31:7b003082f31d 99 sprintf(str,"ID%d:CP\nB%3.1fBN%d",ParameterManager::machine_id,StatusManager::supply_voltage,StatusManager::MainBoard);
alt0710 7:1f1bdb4e4f33 100 sprintf(str_uart,"Complete");
alt0710 7:1f1bdb4e4f33 101 break;
alt0710 7:1f1bdb4e4f33 102
alt0710 7:1f1bdb4e4f33 103 case 1:
alt0710 34:857b6cca2021 104 //sprintf(str,"ID%dB%3.1f\nComplete",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 34:857b6cca2021 105 sprintf(str,"ID%d:RC\nB%3.1fBN%d",ParameterManager::machine_id,StatusManager::supply_voltage,StatusManager::MainBoard);
alt0710 34:857b6cca2021 106 sprintf(str_uart,"Recived");
alt0710 7:1f1bdb4e4f33 107 break;
alt0710 7:1f1bdb4e4f33 108
alt0710 7:1f1bdb4e4f33 109 case 2:
alt0710 31:7b003082f31d 110 //sprintf(str,"ID%dB%3.1f\nHeader1",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 31:7b003082f31d 111 sprintf(str,"ID%d:H1\nB%3.1fBN%d",ParameterManager::machine_id,StatusManager::supply_voltage,StatusManager::MainBoard);
alt0710 7:1f1bdb4e4f33 112 sprintf(str_uart,"Header1");
alt0710 7:1f1bdb4e4f33 113 break;
alt0710 6:21b3fbe57b88 114
alt0710 7:1f1bdb4e4f33 115 case 3:
alt0710 31:7b003082f31d 116 //sprintf(str,"ID%dB%3.1f\nHeader2",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 31:7b003082f31d 117 sprintf(str,"ID%d:H2\nB%3.1fBN%d",ParameterManager::machine_id,StatusManager::supply_voltage,StatusManager::MainBoard);
alt0710 7:1f1bdb4e4f33 118 sprintf(str_uart,"Header2");
alt0710 7:1f1bdb4e4f33 119 break;
alt0710 7:1f1bdb4e4f33 120
alt0710 7:1f1bdb4e4f33 121 case 4:
alt0710 31:7b003082f31d 122 //sprintf(str,"ID%dB%3.1f\nCheckER1",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 31:7b003082f31d 123 sprintf(str,"ID%d:CE1\nB%3.1fBN%d",ParameterManager::machine_id,StatusManager::supply_voltage,StatusManager::MainBoard);
alt0710 7:1f1bdb4e4f33 124 sprintf(str_uart,"CheckERR");
alt0710 7:1f1bdb4e4f33 125 break;
alt0710 7:1f1bdb4e4f33 126
alt0710 7:1f1bdb4e4f33 127 case 5:
alt0710 31:7b003082f31d 128 //sprintf(str,"ID%dB%3.1f\nCheckER2",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 31:7b003082f31d 129 sprintf(str,"ID%d:CE2\nB%3.1fBN%d",ParameterManager::machine_id,StatusManager::supply_voltage,StatusManager::MainBoard);
alt0710 7:1f1bdb4e4f33 130 sprintf(str_uart,"CheckERR");
alt0710 7:1f1bdb4e4f33 131 break;
alt0710 7:1f1bdb4e4f33 132
alt0710 7:1f1bdb4e4f33 133 case -1:
alt0710 30:8ee22331bfaf 134 //sprintf(str,"ID%dB%3.1f\nTimeOut",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 30:8ee22331bfaf 135 //sprintf(str,"ID%dB%d\nTimeOut",ParameterManager::machine_id,StatusManager::indicate_i2c_enable);
alt0710 30:8ee22331bfaf 136 sprintf(str,"ID%d:TO\nB%3.1fBN%d",ParameterManager::machine_id,StatusManager::supply_voltage,StatusManager::MainBoard);
alt0710 7:1f1bdb4e4f33 137 sprintf(str_uart,"TimeOut");
alt0710 7:1f1bdb4e4f33 138 break;
alt0710 7:1f1bdb4e4f33 139
alt0710 7:1f1bdb4e4f33 140 case -2:
alt0710 23:0bb032ef1880 141 sprintf(str,"ID%dB%3.1f\nOERR",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 31:7b003082f31d 142
alt0710 7:1f1bdb4e4f33 143 sprintf(str_uart,"OERR");
alt0710 7:1f1bdb4e4f33 144 break;
alt0710 7:1f1bdb4e4f33 145
alt0710 7:1f1bdb4e4f33 146 case -3:
alt0710 23:0bb032ef1880 147 sprintf(str,"ID%dB%3.1f\nFERR",ParameterManager::machine_id,StatusManager::supply_voltage);
alt0710 7:1f1bdb4e4f33 148 sprintf(str_uart,"FERR");
alt0710 7:1f1bdb4e4f33 149 break;
alt0710 7:1f1bdb4e4f33 150
alt0710 7:1f1bdb4e4f33 151 default:
alt0710 23:0bb032ef1880 152 sprintf(str,"ID%dB%3.1f\n %d",ParameterManager::machine_id,StatusManager::supply_voltage, StatusManager::uart);
alt0710 7:1f1bdb4e4f33 153 break;
alt0710 7:1f1bdb4e4f33 154 }
alt0710 7:1f1bdb4e4f33 155
alt0710 7:1f1bdb4e4f33 156
alt0710 7:1f1bdb4e4f33 157 //sprintf(str,"ID%d EN: \n %s",ParameterManager::machine_id ,str_uart);
alt0710 7:1f1bdb4e4f33 158 }