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

interface/i2c/i2c_roots.cpp@35:b1b00cfaa163, 2019-09-22 (annotated)

Committer:
alt0710
Date:
Sun Sep 22 10:23:42 2019 +0000
Revision:
35:b1b00cfaa163
Parent:
32:718efbf4dc8a 
2019JapanOpenSoft

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alt0710 1:c359b1fcfb5a 1
alt0710 1:c359b1fcfb5a 2 #include "mbed.h"
alt0710 1:c359b1fcfb5a 3 #include "comm.h"
alt0710 1:c359b1fcfb5a 4 #include "interface_manager.h"
alt0710 24:37a074e767c8 5 #include "status_manager.h"
alt0710 24:37a074e767c8 6 #include "parameter_manager.h"
alt0710 1:c359b1fcfb5a 7 #include "i2c_roots.h"
alt0710 1:c359b1fcfb5a 8
alt0710 1:c359b1fcfb5a 9 /* **mbedクラス** */
alt0710 1:c359b1fcfb5a 10 #ifdef LPC4088
alt0710 1:c359b1fcfb5a 11
alt0710 1:c359b1fcfb5a 12 #elif STM32
alt0710 1:c359b1fcfb5a 13 I2C i2cMBED(PB_7, PB_6); // sda, scl
alt0710 1:c359b1fcfb5a 14 #endif
alt0710 1:c359b1fcfb5a 15
alt0710 1:c359b1fcfb5a 16 Ticker i2c_Timer;
alt0710 1:c359b1fcfb5a 17
alt0710 1:c359b1fcfb5a 18 /* **ローカル関数定義** */
alt0710 1:c359b1fcfb5a 19 static void initLCD(void);
alt0710 1:c359b1fcfb5a 20
alt0710 1:c359b1fcfb5a 21 static void sendCommand(unsigned char command);
alt0710 1:c359b1fcfb5a 22 static void sendCharacter(unsigned char a);
alt0710 1:c359b1fcfb5a 23 static void clearDisp(void);
alt0710 1:c359b1fcfb5a 24 static void moveCursor(unsigned char address);
alt0710 1:c359b1fcfb5a 25 static void initialiseCursorPosition(void);
alt0710 31:7b003082f31d 26 //static char str[30];
alt0710 31:7b003082f31d 27 //static char change_scleen;
alt0710 2:ba11cc8540d3 28 /* **ローカル変数** */
alt0710 32:718efbf4dc8a 29
alt0710 1:c359b1fcfb5a 30
alt0710 1:c359b1fcfb5a 31
alt0710 1:c359b1fcfb5a 32 /* **ローカル関数** */
alt0710 1:c359b1fcfb5a 33 void i2c_send(void)
alt0710 1:c359b1fcfb5a 34 {
alt0710 2:ba11cc8540d3 35 //500msec周期で送信
alt0710 1:c359b1fcfb5a 36 //送信記述はここに書く
alt0710 31:7b003082f31d 37 // if(change_scleen != 1)
alt0710 31:7b003082f31d 38 // {
alt0710 31:7b003082f31d 39 // change_scleen = 1;
alt0710 32:718efbf4dc8a 40 // sprintf(str,"V:%d\nA:%d",ParameterManager::machine_data.v_mm_per_sec,ParameterManager::machine_data.angle_degree);
alt0710 31:7b003082f31d 41 // }
alt0710 31:7b003082f31d 42 // else
alt0710 31:7b003082f31d 43 // {
alt0710 31:7b003082f31d 44 // change_scleen = 0;
alt0710 32:718efbf4dc8a 45 // sprintf(str,"O:%d",ParameterManager::machine_data.vw_mrad_per_sec);
alt0710 32:718efbf4dc8a 46 //
alt0710 31:7b003082f31d 47 // }
alt0710 31:7b003082f31d 48 //
alt0710 31:7b003082f31d 49 // InterfaceManager::i2c.setLCDMsg(str,sizeof(str));
alt0710 1:c359b1fcfb5a 50 printfLCD(InterfaceManager::i2c.LCDMsg);
alt0710 1:c359b1fcfb5a 51
alt0710 1:c359b1fcfb5a 52
alt0710 1:c359b1fcfb5a 53
alt0710 1:c359b1fcfb5a 54 }
alt0710 1:c359b1fcfb5a 55
alt0710 1:c359b1fcfb5a 56 static void initLCD(void)
alt0710 1:c359b1fcfb5a 57 {
alt0710 1:c359b1fcfb5a 58 xdev_out(sendCharacter);
alt0710 1:c359b1fcfb5a 59
alt0710 1:c359b1fcfb5a 60 sendCommand(0x38);
alt0710 1:c359b1fcfb5a 61 sendCommand(0x39);
alt0710 1:c359b1fcfb5a 62 sendCommand(0x14);
alt0710 1:c359b1fcfb5a 63 sendCommand(0x70);
alt0710 1:c359b1fcfb5a 64 sendCommand(0x56);
alt0710 1:c359b1fcfb5a 65 sendCommand(0x6C);
alt0710 1:c359b1fcfb5a 66 wait_ms(200);
alt0710 1:c359b1fcfb5a 67 sendCommand(0x38);
alt0710 1:c359b1fcfb5a 68 sendCommand(0x0C);
alt0710 1:c359b1fcfb5a 69 sendCommand(0x01);
alt0710 1:c359b1fcfb5a 70 wait_ms(2);
alt0710 1:c359b1fcfb5a 71
alt0710 1:c359b1fcfb5a 72 initialiseCursorPosition();
alt0710 1:c359b1fcfb5a 73 }
alt0710 1:c359b1fcfb5a 74 extern void printfLCD(const char* fmt, ...)
alt0710 1:c359b1fcfb5a 75 {
alt0710 1:c359b1fcfb5a 76 va_list arp;
alt0710 1:c359b1fcfb5a 77
alt0710 1:c359b1fcfb5a 78 clearDisp();
alt0710 1:c359b1fcfb5a 79
alt0710 1:c359b1fcfb5a 80 va_start(arp, fmt);
alt0710 1:c359b1fcfb5a 81 xvprintf(fmt,arp);
alt0710 1:c359b1fcfb5a 82 va_end(arp);
alt0710 1:c359b1fcfb5a 83
alt0710 1:c359b1fcfb5a 84 initialiseCursorPosition();
alt0710 1:c359b1fcfb5a 85 }
alt0710 1:c359b1fcfb5a 86
alt0710 1:c359b1fcfb5a 87 static void sendCommand(unsigned char command)
alt0710 1:c359b1fcfb5a 88 {
alt0710 1:c359b1fcfb5a 89 char data[2];
alt0710 1:c359b1fcfb5a 90 data[0]= 0x80;
alt0710 1:c359b1fcfb5a 91 data[1]= command;
alt0710 1:c359b1fcfb5a 92 i2cMBED.write(ADDRESS_LCD, data, 2);
alt0710 1:c359b1fcfb5a 93 wait_us(27);
alt0710 1:c359b1fcfb5a 94 }
alt0710 1:c359b1fcfb5a 95
alt0710 1:c359b1fcfb5a 96 static void sendCharacter(unsigned char a)
alt0710 1:c359b1fcfb5a 97 {
alt0710 1:c359b1fcfb5a 98 if(a == '\n') { //改行コードのときは改行する
alt0710 1:c359b1fcfb5a 99 moveCursor(0x40);
alt0710 1:c359b1fcfb5a 100 return;
alt0710 1:c359b1fcfb5a 101 }
alt0710 1:c359b1fcfb5a 102 char data[2];
alt0710 1:c359b1fcfb5a 103 data[0]= 0xC0;
alt0710 1:c359b1fcfb5a 104 data[1]= a;
alt0710 1:c359b1fcfb5a 105 i2cMBED.write(ADDRESS_LCD, data, 2);
alt0710 1:c359b1fcfb5a 106 wait_us(27);
alt0710 1:c359b1fcfb5a 107 }
alt0710 1:c359b1fcfb5a 108
alt0710 1:c359b1fcfb5a 109 static void initialiseCursorPosition(void)
alt0710 1:c359b1fcfb5a 110 {
alt0710 1:c359b1fcfb5a 111 /* 通信終了時にカーソルを頭に戻す */
alt0710 1:c359b1fcfb5a 112 char data[2];
alt0710 1:c359b1fcfb5a 113 data[0]= 0x00;
alt0710 1:c359b1fcfb5a 114 data[1]= 0x80;
alt0710 1:c359b1fcfb5a 115 i2cMBED.write(ADDRESS_LCD, data, 2);
alt0710 1:c359b1fcfb5a 116
alt0710 1:c359b1fcfb5a 117 }
alt0710 1:c359b1fcfb5a 118
alt0710 1:c359b1fcfb5a 119
alt0710 1:c359b1fcfb5a 120 static void clearDisp(void)
alt0710 1:c359b1fcfb5a 121 {
alt0710 1:c359b1fcfb5a 122 sendCommand(0x01);
alt0710 1:c359b1fcfb5a 123 wait_ms(1);
alt0710 1:c359b1fcfb5a 124 }
alt0710 1:c359b1fcfb5a 125
alt0710 1:c359b1fcfb5a 126
alt0710 1:c359b1fcfb5a 127 static void moveCursor(unsigned char address)
alt0710 1:c359b1fcfb5a 128 {
alt0710 1:c359b1fcfb5a 129 sendCommand(0x80 | address);
alt0710 1:c359b1fcfb5a 130 }
alt0710 1:c359b1fcfb5a 131
alt0710 1:c359b1fcfb5a 132
alt0710 1:c359b1fcfb5a 133 /* **グルーバル関数** */
alt0710 1:c359b1fcfb5a 134 extern void initI2C(void)
alt0710 1:c359b1fcfb5a 135 {
alt0710 31:7b003082f31d 136 //#ifndef ON_INDICATER
alt0710 30:8ee22331bfaf 137 i2cMBED.frequency(400000);
alt0710 31:7b003082f31d 138 //#else
alt0710 31:7b003082f31d 139 // i2cMBED.frequency(100000);
alt0710 31:7b003082f31d 140 //#endif
alt0710 1:c359b1fcfb5a 141 initLCD();
alt0710 2:ba11cc8540d3 142 i2c_Timer.attach(i2c_send, I2C_SEND_CYCLE_TIME);
alt0710 1:c359b1fcfb5a 143 }
alt0710 1:c359b1fcfb5a 144
alt0710 1:c359b1fcfb5a 145 /* **クラス** */
alt0710 1:c359b1fcfb5a 146 I2C_Roots::I2C_Roots()
alt0710 1:c359b1fcfb5a 147 {
alt0710 1:c359b1fcfb5a 148
alt0710 1:c359b1fcfb5a 149
alt0710 1:c359b1fcfb5a 150 }
alt0710 1:c359b1fcfb5a 151
alt0710 1:c359b1fcfb5a 152 void I2C_Roots::setLCDMsg(char* msg,char msg_num)
alt0710 1:c359b1fcfb5a 153 {
alt0710 1:c359b1fcfb5a 154 for(char i = 0; i <= msg_num ; i++)
alt0710 1:c359b1fcfb5a 155 {
alt0710 1:c359b1fcfb5a 156 LCDMsg[i] = *(msg + i);
alt0710 1:c359b1fcfb5a 157 }
alt0710 1:c359b1fcfb5a 158
alt0710 1:c359b1fcfb5a 159
alt0710 24:37a074e767c8 160 }
alt0710 24:37a074e767c8 161
alt0710 24:37a074e767c8 162 void I2C_Roots::setIndicateMsg(char* msg,char msg_num)
alt0710 24:37a074e767c8 163 {
alt0710 24:37a074e767c8 164 for(char i = 0; i <= msg_num ; i++)
alt0710 24:37a074e767c8 165 {
alt0710 24:37a074e767c8 166 IndicateMsg[i] = *(msg + i);
alt0710 24:37a074e767c8 167 }
alt0710 24:37a074e767c8 168
alt0710 30:8ee22331bfaf 169 StatusManager::indicate_i2c_enable = i2cMBED.write(ADDRESS_INDICATE, IndicateMsg, msg_num);
alt0710 24:37a074e767c8 170 }
alt0710 24:37a074e767c8 171
alt0710 24:37a074e767c8 172 void I2C_Roots::setIDMsg(char* msg,char msg_num)
alt0710 24:37a074e767c8 173 {
alt0710 24:37a074e767c8 174 for(char i = 0; i <= msg_num ; i++)
alt0710 24:37a074e767c8 175 {
alt0710 24:37a074e767c8 176 IDMsg[i] = *(msg + i);
alt0710 24:37a074e767c8 177 }
alt0710 24:37a074e767c8 178 i2cMBED.write(ADDRESS_EEPROM, IDMsg, msg_num);
alt0710 24:37a074e767c8 179 }
alt0710 24:37a074e767c8 180
alt0710 24:37a074e767c8 181 void I2C_Roots::readIDMsg(void)
alt0710 24:37a074e767c8 182 {
alt0710 24:37a074e767c8 183 char msg_num = 2;
alt0710 24:37a074e767c8 184 for(char i = 0; i <= msg_num ; i++)
alt0710 24:37a074e767c8 185 {
alt0710 24:37a074e767c8 186 IDMsg[i] = 0;
alt0710 24:37a074e767c8 187 }
alt0710 24:37a074e767c8 188 msg_num = 2;
alt0710 24:37a074e767c8 189 i2cMBED.write(ADDRESS_EEPROM, IDMsg, msg_num, 1);
alt0710 24:37a074e767c8 190
alt0710 24:37a074e767c8 191 msg_num = 1;
alt0710 24:37a074e767c8 192 i2cMBED.read(ADDRESS_EEPROM, IDMsg, msg_num, 1);
alt0710 24:37a074e767c8 193 ParameterManager::machine_id = IDMsg[0];
alt0710 24:37a074e767c8 194
alt0710 27:c7d8f514209a 195 if(ParameterManager::machine_id > MAX_ID)
alt0710 27:c7d8f514209a 196 {
alt0710 27:c7d8f514209a 197 ParameterManager::machine_id = MIN_ID;
alt0710 27:c7d8f514209a 198 }
alt0710 27:c7d8f514209a 199
alt0710 1:c359b1fcfb5a 200 }