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

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

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 1:c359b1fcfb5a 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 31:7b003082f31d 40 // sprintf(str,"V1:%d\nV2:%d",StatusManager::Vel_Wheel1,StatusManager::Vel_Wheel2);
alt0710 31:7b003082f31d 41 // }
alt0710 31:7b003082f31d 42 // else
alt0710 31:7b003082f31d 43 // {
alt0710 31:7b003082f31d 44 // change_scleen = 0;
alt0710 31:7b003082f31d 45 // sprintf(str,"V3:%d\n",StatusManager::Vel_Wheel3);
alt0710 31:7b003082f31d 46 // }
alt0710 31:7b003082f31d 47 //
alt0710 31:7b003082f31d 48 // InterfaceManager::i2c.setLCDMsg(str,sizeof(str));
alt0710 1:c359b1fcfb5a 49 printfLCD(InterfaceManager::i2c.LCDMsg);
alt0710 1:c359b1fcfb5a 50
alt0710 1:c359b1fcfb5a 51
alt0710 1:c359b1fcfb5a 52
alt0710 1:c359b1fcfb5a 53 }
alt0710 1:c359b1fcfb5a 54
alt0710 1:c359b1fcfb5a 55 static void initLCD(void)
alt0710 1:c359b1fcfb5a 56 {
alt0710 1:c359b1fcfb5a 57 xdev_out(sendCharacter);
alt0710 1:c359b1fcfb5a 58
alt0710 1:c359b1fcfb5a 59 sendCommand(0x38);
alt0710 1:c359b1fcfb5a 60 sendCommand(0x39);
alt0710 1:c359b1fcfb5a 61 sendCommand(0x14);
alt0710 1:c359b1fcfb5a 62 sendCommand(0x70);
alt0710 1:c359b1fcfb5a 63 sendCommand(0x56);
alt0710 1:c359b1fcfb5a 64 sendCommand(0x6C);
alt0710 1:c359b1fcfb5a 65 wait_ms(200);
alt0710 1:c359b1fcfb5a 66 sendCommand(0x38);
alt0710 1:c359b1fcfb5a 67 sendCommand(0x0C);
alt0710 1:c359b1fcfb5a 68 sendCommand(0x01);
alt0710 1:c359b1fcfb5a 69 wait_ms(2);
alt0710 1:c359b1fcfb5a 70
alt0710 1:c359b1fcfb5a 71 initialiseCursorPosition();
alt0710 1:c359b1fcfb5a 72 }
alt0710 1:c359b1fcfb5a 73 extern void printfLCD(const char* fmt, ...)
alt0710 1:c359b1fcfb5a 74 {
alt0710 1:c359b1fcfb5a 75 va_list arp;
alt0710 1:c359b1fcfb5a 76
alt0710 1:c359b1fcfb5a 77 clearDisp();
alt0710 1:c359b1fcfb5a 78
alt0710 1:c359b1fcfb5a 79 va_start(arp, fmt);
alt0710 1:c359b1fcfb5a 80 xvprintf(fmt,arp);
alt0710 1:c359b1fcfb5a 81 va_end(arp);
alt0710 1:c359b1fcfb5a 82
alt0710 1:c359b1fcfb5a 83 initialiseCursorPosition();
alt0710 1:c359b1fcfb5a 84 }
alt0710 1:c359b1fcfb5a 85
alt0710 1:c359b1fcfb5a 86 static void sendCommand(unsigned char command)
alt0710 1:c359b1fcfb5a 87 {
alt0710 1:c359b1fcfb5a 88 char data[2];
alt0710 1:c359b1fcfb5a 89 data[0]= 0x80;
alt0710 1:c359b1fcfb5a 90 data[1]= command;
alt0710 1:c359b1fcfb5a 91 i2cMBED.write(ADDRESS_LCD, data, 2);
alt0710 1:c359b1fcfb5a 92 wait_us(27);
alt0710 1:c359b1fcfb5a 93 }
alt0710 1:c359b1fcfb5a 94
alt0710 1:c359b1fcfb5a 95 static void sendCharacter(unsigned char a)
alt0710 1:c359b1fcfb5a 96 {
alt0710 1:c359b1fcfb5a 97 if(a == '\n') { //改行コードのときは改行する
alt0710 1:c359b1fcfb5a 98 moveCursor(0x40);
alt0710 1:c359b1fcfb5a 99 return;
alt0710 1:c359b1fcfb5a 100 }
alt0710 1:c359b1fcfb5a 101 char data[2];
alt0710 1:c359b1fcfb5a 102 data[0]= 0xC0;
alt0710 1:c359b1fcfb5a 103 data[1]= a;
alt0710 1:c359b1fcfb5a 104 i2cMBED.write(ADDRESS_LCD, data, 2);
alt0710 1:c359b1fcfb5a 105 wait_us(27);
alt0710 1:c359b1fcfb5a 106 }
alt0710 1:c359b1fcfb5a 107
alt0710 1:c359b1fcfb5a 108 static void initialiseCursorPosition(void)
alt0710 1:c359b1fcfb5a 109 {
alt0710 1:c359b1fcfb5a 110 /* 通信終了時にカーソルを頭に戻す */
alt0710 1:c359b1fcfb5a 111 char data[2];
alt0710 1:c359b1fcfb5a 112 data[0]= 0x00;
alt0710 1:c359b1fcfb5a 113 data[1]= 0x80;
alt0710 1:c359b1fcfb5a 114 i2cMBED.write(ADDRESS_LCD, data, 2);
alt0710 1:c359b1fcfb5a 115
alt0710 1:c359b1fcfb5a 116 }
alt0710 1:c359b1fcfb5a 117
alt0710 1:c359b1fcfb5a 118
alt0710 1:c359b1fcfb5a 119 static void clearDisp(void)
alt0710 1:c359b1fcfb5a 120 {
alt0710 1:c359b1fcfb5a 121 sendCommand(0x01);
alt0710 1:c359b1fcfb5a 122 wait_ms(1);
alt0710 1:c359b1fcfb5a 123 }
alt0710 1:c359b1fcfb5a 124
alt0710 1:c359b1fcfb5a 125
alt0710 1:c359b1fcfb5a 126 static void moveCursor(unsigned char address)
alt0710 1:c359b1fcfb5a 127 {
alt0710 1:c359b1fcfb5a 128 sendCommand(0x80 | address);
alt0710 1:c359b1fcfb5a 129 }
alt0710 1:c359b1fcfb5a 130
alt0710 1:c359b1fcfb5a 131
alt0710 1:c359b1fcfb5a 132 /* **グルーバル関数** */
alt0710 1:c359b1fcfb5a 133 extern void initI2C(void)
alt0710 1:c359b1fcfb5a 134 {
alt0710 31:7b003082f31d 135 //#ifndef ON_INDICATER
alt0710 30:8ee22331bfaf 136 i2cMBED.frequency(400000);
alt0710 31:7b003082f31d 137 //#else
alt0710 31:7b003082f31d 138 // i2cMBED.frequency(100000);
alt0710 31:7b003082f31d 139 //#endif
alt0710 1:c359b1fcfb5a 140 initLCD();
alt0710 2:ba11cc8540d3 141 i2c_Timer.attach(i2c_send, I2C_SEND_CYCLE_TIME);
alt0710 1:c359b1fcfb5a 142 }
alt0710 1:c359b1fcfb5a 143
alt0710 1:c359b1fcfb5a 144 /* **クラス** */
alt0710 1:c359b1fcfb5a 145 I2C_Roots::I2C_Roots()
alt0710 1:c359b1fcfb5a 146 {
alt0710 1:c359b1fcfb5a 147
alt0710 1:c359b1fcfb5a 148
alt0710 1:c359b1fcfb5a 149 }
alt0710 1:c359b1fcfb5a 150
alt0710 1:c359b1fcfb5a 151 void I2C_Roots::setLCDMsg(char* msg,char msg_num)
alt0710 1:c359b1fcfb5a 152 {
alt0710 1:c359b1fcfb5a 153 for(char i = 0; i <= msg_num ; i++)
alt0710 1:c359b1fcfb5a 154 {
alt0710 1:c359b1fcfb5a 155 LCDMsg[i] = *(msg + i);
alt0710 1:c359b1fcfb5a 156 }
alt0710 1:c359b1fcfb5a 157
alt0710 1:c359b1fcfb5a 158
alt0710 24:37a074e767c8 159 }
alt0710 24:37a074e767c8 160
alt0710 24:37a074e767c8 161 void I2C_Roots::setIndicateMsg(char* msg,char msg_num)
alt0710 24:37a074e767c8 162 {
alt0710 24:37a074e767c8 163 for(char i = 0; i <= msg_num ; i++)
alt0710 24:37a074e767c8 164 {
alt0710 24:37a074e767c8 165 IndicateMsg[i] = *(msg + i);
alt0710 24:37a074e767c8 166 }
alt0710 24:37a074e767c8 167
alt0710 30:8ee22331bfaf 168 StatusManager::indicate_i2c_enable = i2cMBED.write(ADDRESS_INDICATE, IndicateMsg, msg_num);
alt0710 24:37a074e767c8 169 }
alt0710 24:37a074e767c8 170
alt0710 24:37a074e767c8 171 void I2C_Roots::setIDMsg(char* msg,char msg_num)
alt0710 24:37a074e767c8 172 {
alt0710 24:37a074e767c8 173 for(char i = 0; i <= msg_num ; i++)
alt0710 24:37a074e767c8 174 {
alt0710 24:37a074e767c8 175 IDMsg[i] = *(msg + i);
alt0710 24:37a074e767c8 176 }
alt0710 24:37a074e767c8 177 i2cMBED.write(ADDRESS_EEPROM, IDMsg, msg_num);
alt0710 24:37a074e767c8 178 }
alt0710 24:37a074e767c8 179
alt0710 24:37a074e767c8 180 void I2C_Roots::readIDMsg(void)
alt0710 24:37a074e767c8 181 {
alt0710 24:37a074e767c8 182 char msg_num = 2;
alt0710 24:37a074e767c8 183 for(char i = 0; i <= msg_num ; i++)
alt0710 24:37a074e767c8 184 {
alt0710 24:37a074e767c8 185 IDMsg[i] = 0;
alt0710 24:37a074e767c8 186 }
alt0710 24:37a074e767c8 187 msg_num = 2;
alt0710 24:37a074e767c8 188 i2cMBED.write(ADDRESS_EEPROM, IDMsg, msg_num, 1);
alt0710 24:37a074e767c8 189
alt0710 24:37a074e767c8 190 msg_num = 1;
alt0710 24:37a074e767c8 191 i2cMBED.read(ADDRESS_EEPROM, IDMsg, msg_num, 1);
alt0710 24:37a074e767c8 192 ParameterManager::machine_id = IDMsg[0];
alt0710 24:37a074e767c8 193
alt0710 27:c7d8f514209a 194 if(ParameterManager::machine_id > MAX_ID)
alt0710 27:c7d8f514209a 195 {
alt0710 27:c7d8f514209a 196 ParameterManager::machine_id = MIN_ID;
alt0710 27:c7d8f514209a 197 }
alt0710 27:c7d8f514209a 198
alt0710 1:c359b1fcfb5a 199 }