CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_F

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   AQM0802A HMC6352 L6470_lib PID mbed

Fork of CatPot_Main_ver1 by CatPot 2015-2016

main.cpp@3:8d718ddb84c9, 2014-12-27 (annotated)

Committer:
ryuna
Date:
Sat Dec 27 07:33:07 2014 +0000
Revision:
3:8d718ddb84c9
Parent:
2:054444aa1990
Child:
4:8444360f08e2
fumiya????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:ae08e2e1d82d 1 /***********************************
ryuna 0:ae08e2e1d82d 2 *RoboCupJunior Soccer B Open 2014
ryuna 0:ae08e2e1d82d 3 *Koshinestu progrum
ryuna 0:ae08e2e1d82d 4 *
ryuna 0:ae08e2e1d82d 5 *このプログラムでは以下の処理をする．
ryuna 0:ae08e2e1d82d 6 *　LPC1114FN28/102からI2Cを用いて各種センサデータを収集
ryuna 0:ae08e2e1d82d 7 *　
ryuna 0:ae08e2e1d82d 8 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
ryuna 0:ae08e2e1d82d 9 *
ryuna 1:7d4921b5d638 10 *　MotorDriverにmaxonに命令
ryuna 0:ae08e2e1d82d 11 *　
ryuna 0:ae08e2e1d82d 12 *　SteppingMotorDriverにステアリング命令
ryuna 0:ae08e2e1d82d 13 *　
ryuna 0:ae08e2e1d82d 14 *　LCDでデバック
ryuna 0:ae08e2e1d82d 15 *
ryuna 0:ae08e2e1d82d 16 *　スイッチ4つとスタートスイッチで処理を実行
ryuna 1:7d4921b5d638 17 *
ryuna 1:7d4921b5d638 18 * キッカーのスイッチがどこかに入る
ryuna 0:ae08e2e1d82d 19 *
ryuna 0:ae08e2e1d82d 20 *************************
ryuna 0:ae08e2e1d82d 21 *
ryuna 0:ae08e2e1d82d 22 *Pin Map
ryuna 0:ae08e2e1d82d 23 *
ryuna 1:7d4921b5d638 24 * p5,p6,p7,p29,p30 >> SPI >>Stepping
ryuna 0:ae08e2e1d82d 25 *
ryuna 0:ae08e2e1d82d 26 * p9,p10 >> I2C orSerial >> LPC1114FN28/102 read
ryuna 0:ae08e2e1d82d 27 *
ryuna 0:ae08e2e1d82d 28 * p13,p14 >> Serial >> Motor
ryuna 0:ae08e2e1d82d 29 *
ryuna 0:ae08e2e1d82d 30 * p22~p26 >> DebugSw and StartSw
ryuna 0:ae08e2e1d82d 31 *
ryuna 0:ae08e2e1d82d 32 * p27,p28 >> I2C >> DebugLCD
ryuna 0:ae08e2e1d82d 33 *
ryuna 0:ae08e2e1d82d 34 *
ryuna 0:ae08e2e1d82d 35 *
ryuna 0:ae08e2e1d82d 36 ******************************/
ryuna 0:ae08e2e1d82d 37
ryuna 2:054444aa1990 38
ryuna 2:054444aa1990 39 /*
ryuna 2:054444aa1990 40
ryuna 2:054444aa1990 41 ****以下IRはNighは距離が近いものとする．
ryuna 2:054444aa1990 42
ryuna 2:054444aa1990 43 */
ryuna 2:054444aa1990 44
ryuna 2:054444aa1990 45
ryuna 2:054444aa1990 46
ryuna 2:054444aa1990 47
ryuna 2:054444aa1990 48
ryuna 2:054444aa1990 49
ryuna 0:ae08e2e1d82d 50 #include "mbed.h"
ryuna 0:ae08e2e1d82d 51 #include "L6470.h"
ryuna 0:ae08e2e1d82d 52 #include "PID.h"
ryuna 0:ae08e2e1d82d 53 #include "AQM0802A.h"
ryuna 3:8d718ddb84c9 54 #include "HMC6352.h"
ryuna 0:ae08e2e1d82d 55 #include <math.h>
ryuna 0:ae08e2e1d82d 56 #include <sstream>
ryuna 0:ae08e2e1d82d 57
ryuna 2:054444aa1990 58 #define ADDRESS_R 0xA0
ryuna 2:054444aa1990 59 #define ADDRESS_L 0xC0
ryuna 2:054444aa1990 60
ryuna 3:8d718ddb84c9 61 #define READ_IR 0x01 //送る物指定
ryuna 3:8d718ddb84c9 62 #define READ_PING 0x02
ryuna 3:8d718ddb84c9 63
ryuna 2:054444aa1990 64
ryuna 1:7d4921b5d638 65 DigitalIn DebugSw[4] = {p22,p23,p24,p25};
ryuna 1:7d4921b5d638 66 DigitalIn StartSw(p26,PullUp);
ryuna 1:7d4921b5d638 67 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
ryuna 0:ae08e2e1d82d 68
ryuna 1:7d4921b5d638 69 I2C Sensor(p9,p10);//sda,scl
ryuna 3:8d718ddb84c9 70 HMC6352 hmc6352(p9,p10);
ryuna 1:7d4921b5d638 71 AQM0802A Lcd(p28,p27);//sda,scl
ryuna 1:7d4921b5d638 72 L6470 Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp)
ryuna 1:7d4921b5d638 73 Serial Motor(p13,p14);//tx,rx
ryuna 1:7d4921b5d638 74 Serial pc(USBTX,USBRX);
ryuna 0:ae08e2e1d82d 75
ryuna 3:8d718ddb84c9 76 DigitalOut Kick(p21);
ryuna 0:ae08e2e1d82d 77
ryuna 0:ae08e2e1d82d 78 extern string StringFIN;
ryuna 0:ae08e2e1d82d 79 extern void array(int,int,int,int);
ryuna 0:ae08e2e1d82d 80
ryuna 0:ae08e2e1d82d 81 int speed[4] = {0};
ryuna 0:ae08e2e1d82d 82
ryuna 3:8d718ddb84c9 83 typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
ryuna 0:ae08e2e1d82d 84
ryuna 3:8d718ddb84c9 85
ryuna 3:8d718ddb84c9 86 unsigned int IrReceiveR(uint8_t *far){
ryuna 3:8d718ddb84c9 87 /*順位解析版
ryuna 2:054444aa1990 88 Slave側はIRを要求した場合IRのみを返してくるとする．
ryuna 2:054444aa1990 89 irは値が大きいほうが近いと仮定する
ryuna 2:054444aa1990 90
ryuna 2:054444aa1990 91
ryuna 2:054444aa1990 92 */
ryuna 3:8d718ddb84c9 93 char data_r[3],data_l[3];
ryuna 2:054444aa1990 94 bool val;
ryuna 2:054444aa1990 95 Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
ryuna 2:054444aa1990 96
ryuna 3:8d718ddb84c9 97 val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
ryuna 2:054444aa1990 98 Led[0] = val;
ryuna 2:054444aa1990 99 wait_ms(5);
ryuna 3:8d718ddb84c9 100 val = Sensor.read(ADDRESS_L|1, data_l, 3);
ryuna 2:054444aa1990 101 Led[0] = !val;
ryuna 2:054444aa1990 102
ryuna 2:054444aa1990 103 if(data_r[2]>data_l[2]){
ryuna 2:054444aa1990 104 if(data_r[2]>data_l[1]){
ryuna 3:8d718ddb84c9 105 return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
ryuna 2:054444aa1990 106 }else{
ryuna 3:8d718ddb84c9 107 return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6;
ryuna 2:054444aa1990 108 }
ryuna 2:054444aa1990 109 }else{
ryuna 2:054444aa1990 110 if(data_l[2]>data_r[1]){
ryuna 3:8d718ddb84c9 111 return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
ryuna 2:054444aa1990 112 }else{
ryuna 3:8d718ddb84c9 113 return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6);
ryuna 2:054444aa1990 114 }
ryuna 2:054444aa1990 115
ryuna 2:054444aa1990 116 }
ryuna 2:054444aa1990 117
ryuna 2:054444aa1990 118 }
ryuna 2:054444aa1990 119
ryuna 3:8d718ddb84c9 120 uint8_t IrReceiveS(){
ryuna 3:8d718ddb84c9 121 /*単純解析版 一番大きい位置だけ返す
ryuna 3:8d718ddb84c9 122
ryuna 3:8d718ddb84c9 123 Slave側はIRを要求した場合IRのみを返してくるとする．
ryuna 3:8d718ddb84c9 124 irは値が大きいほうが近いと仮定する
ryuna 3:8d718ddb84c9 125
ryuna 3:8d718ddb84c9 126
ryuna 3:8d718ddb84c9 127 */
ryuna 3:8d718ddb84c9 128 char data_r[3],data_l[3];
ryuna 3:8d718ddb84c9 129 bool val;
ryuna 3:8d718ddb84c9 130 Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
ryuna 3:8d718ddb84c9 131
ryuna 3:8d718ddb84c9 132 val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
ryuna 3:8d718ddb84c9 133 Led[0] = val;
ryuna 3:8d718ddb84c9 134 wait_ms(5);
ryuna 3:8d718ddb84c9 135 val = Sensor.read(ADDRESS_L|1, data_l, 3);
ryuna 3:8d718ddb84c9 136 Led[0] = !val;
ryuna 3:8d718ddb84c9 137
ryuna 3:8d718ddb84c9 138 if(data_r[2]>data_l[2]){
ryuna 3:8d718ddb84c9 139 return data_r[0]/6;
ryuna 3:8d718ddb84c9 140
ryuna 3:8d718ddb84c9 141 }else{
ryuna 3:8d718ddb84c9 142 return data_l[0]/6+6;
ryuna 3:8d718ddb84c9 143 }
ryuna 3:8d718ddb84c9 144
ryuna 3:8d718ddb84c9 145 }
ryuna 3:8d718ddb84c9 146 uint8_t IrReceiveM(){
ryuna 3:8d718ddb84c9 147 /*値解析版　一番大きい場所の値を返す（位置はとりあえずない）
ryuna 3:8d718ddb84c9 148 Slave側はIRを要求した場合IRのみを返してくるとする．
ryuna 3:8d718ddb84c9 149 irは値が大きいほうが近いと仮定する
ryuna 3:8d718ddb84c9 150
ryuna 3:8d718ddb84c9 151
ryuna 3:8d718ddb84c9 152 */
ryuna 3:8d718ddb84c9 153 char data_r[3],data_l[3];
ryuna 3:8d718ddb84c9 154 bool val;
ryuna 3:8d718ddb84c9 155 Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
ryuna 3:8d718ddb84c9 156
ryuna 3:8d718ddb84c9 157 val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
ryuna 3:8d718ddb84c9 158 Led[0] = val;
ryuna 3:8d718ddb84c9 159 wait_ms(5);
ryuna 3:8d718ddb84c9 160 val = Sensor.read(ADDRESS_L|1, data_l, 3);
ryuna 3:8d718ddb84c9 161 Led[0] = !val;
ryuna 3:8d718ddb84c9 162
ryuna 3:8d718ddb84c9 163 if(data_r[2]>data_l[2]){
ryuna 3:8d718ddb84c9 164 return data_r[1];
ryuna 3:8d718ddb84c9 165
ryuna 3:8d718ddb84c9 166 }else{
ryuna 3:8d718ddb84c9 167 return data_l[1];
ryuna 3:8d718ddb84c9 168
ryuna 3:8d718ddb84c9 169 }
ryuna 3:8d718ddb84c9 170
ryuna 3:8d718ddb84c9 171 }
ryuna 3:8d718ddb84c9 172 unsigned int IrReceiveSM(){
ryuna 3:8d718ddb84c9 173 /*値解析版　一番大きい値とその位置を返す
ryuna 3:8d718ddb84c9 174 Slave側はIRを要求した場合IRのみを返してくるとする．
ryuna 3:8d718ddb84c9 175 irは値が大きいほうが近いと仮定する
ryuna 3:8d718ddb84c9 176
ryuna 3:8d718ddb84c9 177
ryuna 3:8d718ddb84c9 178 */
ryuna 3:8d718ddb84c9 179 char data_r[3],data_l[3];
ryuna 3:8d718ddb84c9 180 bool val;
ryuna 3:8d718ddb84c9 181 Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
ryuna 3:8d718ddb84c9 182
ryuna 3:8d718ddb84c9 183 val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
ryuna 3:8d718ddb84c9 184 Led[0] = val;
ryuna 3:8d718ddb84c9 185 wait_ms(5);
ryuna 3:8d718ddb84c9 186 val = Sensor.read(ADDRESS_L|1, data_l, 3);
ryuna 3:8d718ddb84c9 187 Led[0] = !val;
ryuna 3:8d718ddb84c9 188
ryuna 3:8d718ddb84c9 189 if(data_r[2]>data_l[2]){
ryuna 3:8d718ddb84c9 190 return data_r[0]/6<<8 + data_r[1];
ryuna 3:8d718ddb84c9 191
ryuna 3:8d718ddb84c9 192 }else{
ryuna 3:8d718ddb84c9 193 return ((data_l[0]/6)+6) <<8 + data_l[1];
ryuna 3:8d718ddb84c9 194
ryuna 3:8d718ddb84c9 195 }
ryuna 3:8d718ddb84c9 196
ryuna 3:8d718ddb84c9 197 }
ryuna 3:8d718ddb84c9 198 uint8_t LineReceive(){
ryuna 3:8d718ddb84c9 199 //先に要求しない場合はLineの状況を送信すること．//4bit //前ー右ー後ろー左　
ryuna 2:054444aa1990 200 char data_r[2],data_l[2];
ryuna 2:054444aa1990 201 bool val;
ryuna 3:8d718ddb84c9 202 //example val = slave.read(address,data,length,repeat);
ryuna 2:054444aa1990 203 val = Sensor.read(ADDRESS_R|1, data_r, 1);
ryuna 2:054444aa1990 204 Led[1] = val;
ryuna 2:054444aa1990 205 wait_ms(5);
ryuna 2:054444aa1990 206 val = Sensor.read(ADDRESS_L|1, data_l, 1);
ryuna 2:054444aa1990 207 Led[1] = !val;
ryuna 2:054444aa1990 208
ryuna 3:8d718ddb84c9 209
ryuna 3:8d718ddb84c9 210 return data_r[0]<<2 + data_l[0];
ryuna 2:054444aa1990 211
ryuna 2:054444aa1990 212
ryuna 2:054444aa1990 213 }
ryuna 3:8d718ddb84c9 214
ryuna 3:8d718ddb84c9 215
ryuna 3:8d718ddb84c9 216 void PingReceiveRL(char ping[]){//ping[0]==Right,ping[1]==Left
ryuna 3:8d718ddb84c9 217 char pingdata[1] = {READ_PING};
ryuna 3:8d718ddb84c9 218 bool val;
ryuna 3:8d718ddb84c9 219 val = Sensor.write(ADDRESS_R|0,pingdata , 1);
ryuna 3:8d718ddb84c9 220 Led[2] = val;
ryuna 3:8d718ddb84c9 221 val = Sensor.read(ADDRESS_R|1, ping, 2);
ryuna 3:8d718ddb84c9 222 Led[2] = !val;
ryuna 3:8d718ddb84c9 223
ryuna 3:8d718ddb84c9 224
ryuna 3:8d718ddb84c9 225
ryuna 3:8d718ddb84c9 226
ryuna 3:8d718ddb84c9 227 }
ryuna 2:054444aa1990 228 void rotate(unsigned int times, bool dir){
ryuna 2:054444aa1990 229 /*
ryuna 2:054444aa1990 230 *回転速度，回転方向，回転角度等設定変更ののち回転．
ryuna 2:054444aa1990 231 *
ryuna 2:054444aa1990 232 */
ryuna 3:8d718ddb84c9 233
ryuna 3:8d718ddb84c9 234
ryuna 3:8d718ddb84c9 235
ryuna 3:8d718ddb84c9 236
ryuna 2:054444aa1990 237
ryuna 2:054444aa1990 238 }
ryuna 2:054444aa1990 239
ryuna 2:054444aa1990 240 void move(int vr, int vl){
ryuna 0:ae08e2e1d82d 241 double pwm[4] = {0};
ryuna 0:ae08e2e1d82d 242 uint8_t i = 0;
ryuna 2:054444aa1990 243 pwm[0] = vr/10.0;
ryuna 2:054444aa1990 244 pwm[1] = vl/10.0;
ryuna 2:054444aa1990 245 pwm[2] = 0;
ryuna 2:054444aa1990 246 pwm[3] = 0;
ryuna 0:ae08e2e1d82d 247
ryuna 0:ae08e2e1d82d 248 for(i = 0; i < 4; i++){
ryuna 0:ae08e2e1d82d 249 if(pwm[i] > 100){
ryuna 0:ae08e2e1d82d 250 pwm[i] = 100;
ryuna 0:ae08e2e1d82d 251 }else if(pwm[i] < -100){
ryuna 0:ae08e2e1d82d 252 pwm[i] = -100;
ryuna 0:ae08e2e1d82d 253 }
ryuna 0:ae08e2e1d82d 254 speed[i] = pwm[i];
ryuna 0:ae08e2e1d82d 255 }
ryuna 0:ae08e2e1d82d 256 }
ryuna 0:ae08e2e1d82d 257
ryuna 3:8d718ddb84c9 258 void ControlRobo0(int *CompassDef)//LeftFront
ryuna 3:8d718ddb84c9 259 {}
ryuna 3:8d718ddb84c9 260 void ControlRobo1(int *CompassDef)//Front
ryuna 3:8d718ddb84c9 261 {
ryuna 3:8d718ddb84c9 262 /*前にボールがある場合の動作*/
ryuna 3:8d718ddb84c9 263 int Compass;
ryuna 3:8d718ddb84c9 264 char Ping[2];
ryuna 3:8d718ddb84c9 265 uint8_t Line,LineIr;
ryuna 3:8d718ddb84c9 266 unsigned int IrNumMax;
ryuna 3:8d718ddb84c9 267
ryuna 3:8d718ddb84c9 268 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 269 if(!((Compass>=150)&&(Compass<=210))){
ryuna 3:8d718ddb84c9 270 while(!((Compass>=150)&&(Compass<=210))){
ryuna 3:8d718ddb84c9 271 move(20,-20);//適当な回転。
ryuna 3:8d718ddb84c9 272 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 273 }
ryuna 3:8d718ddb84c9 274 return;
ryuna 3:8d718ddb84c9 275 }
ryuna 3:8d718ddb84c9 276 if(IrReceiveM()<=150){//適当な150
ryuna 3:8d718ddb84c9 277 /*ステッピングを正面に設定*/
ryuna 3:8d718ddb84c9 278 /*busy_wait()*/
ryuna 3:8d718ddb84c9 279 /*モーターを前進*/
ryuna 3:8d718ddb84c9 280 return;
ryuna 3:8d718ddb84c9 281 }
ryuna 3:8d718ddb84c9 282 PingReceiveRL(Ping);
ryuna 3:8d718ddb84c9 283 if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当
ryuna 3:8d718ddb84c9 284 /*ステッピングを少しだけ傾ける*/
ryuna 3:8d718ddb84c9 285 /*モーターを左右差つけて回す(ロボットは少し傾く)*/
ryuna 3:8d718ddb84c9 286 /*busy_wait()*/
ryuna 3:8d718ddb84c9 287 /*モーターを前進*/
ryuna 3:8d718ddb84c9 288 while(1){
ryuna 3:8d718ddb84c9 289 Line = LineReceive();
ryuna 3:8d718ddb84c9 290
ryuna 3:8d718ddb84c9 291 if(Line){ //ラインを検知していないか
ryuna 3:8d718ddb84c9 292 LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。
ryuna 3:8d718ddb84c9 293 while(LineIr){
ryuna 3:8d718ddb84c9 294 move(0,0);//
ryuna 3:8d718ddb84c9 295 Led[1] = Led[2] = Led[3] = 1;
ryuna 3:8d718ddb84c9 296
ryuna 3:8d718ddb84c9 297 LineIr = LineReceive() & (IrReceiveS()/3 + 1);
ryuna 3:8d718ddb84c9 298 }
ryuna 3:8d718ddb84c9 299 Led[1] = Led[2] = Led[3] = 0;
ryuna 3:8d718ddb84c9 300
ryuna 3:8d718ddb84c9 301 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 302 while(!((Compass>=150)&&(Compass<=210))){
ryuna 3:8d718ddb84c9 303 move(20,-20);//適当な回転。
ryuna 3:8d718ddb84c9 304 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 305 }
ryuna 3:8d718ddb84c9 306 return;
ryuna 3:8d718ddb84c9 307 }
ryuna 3:8d718ddb84c9 308 Kick = 1;
ryuna 3:8d718ddb84c9 309 wait_ms(200);
ryuna 3:8d718ddb84c9 310 Kick = 0;
ryuna 3:8d718ddb84c9 311
ryuna 3:8d718ddb84c9 312 IrNumMax = IrReceiveSM();//位置と大きさ
ryuna 3:8d718ddb84c9 313
ryuna 3:8d718ddb84c9 314 if(!((IrNumMax&0x00)>>8) == 1){//この1はボールの関数の1を表す.
ryuna 3:8d718ddb84c9 315 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 316 while(!((Compass>=150)&&(Compass<=210))){
ryuna 3:8d718ddb84c9 317 move(20,-20);//適当な回転。
ryuna 3:8d718ddb84c9 318 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 319 }
ryuna 3:8d718ddb84c9 320 return;
ryuna 3:8d718ddb84c9 321 }
ryuna 3:8d718ddb84c9 322
ryuna 3:8d718ddb84c9 323 if((IrNumMax&0x00FF)<150){
ryuna 3:8d718ddb84c9 324 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 325 while(!((Compass>=150)&&(Compass<=210))){
ryuna 3:8d718ddb84c9 326 move(20,-20);//適当な回転。
ryuna 3:8d718ddb84c9 327 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:8d718ddb84c9 328 }
ryuna 3:8d718ddb84c9 329 return;
ryuna 3:8d718ddb84c9 330 }
ryuna 3:8d718ddb84c9 331
ryuna 3:8d718ddb84c9 332 }
ryuna 3:8d718ddb84c9 333
ryuna 3:8d718ddb84c9 334 }else if((Ping[0]>40)&&(Ping[1]>90)){
ryuna 3:8d718ddb84c9 335
ryuna 3:8d718ddb84c9 336 }else{
ryuna 3:8d718ddb84c9 337
ryuna 3:8d718ddb84c9 338 }
ryuna 3:8d718ddb84c9 339
ryuna 3:8d718ddb84c9 340 }
ryuna 3:8d718ddb84c9 341 void ControlRobo2(int *CompassDef)//RightFront
ryuna 3:8d718ddb84c9 342 {
ryuna 3:8d718ddb84c9 343
ryuna 3:8d718ddb84c9 344
ryuna 3:8d718ddb84c9 345
ryuna 3:8d718ddb84c9 346
ryuna 3:8d718ddb84c9 347 }
ryuna 3:8d718ddb84c9 348 void ControlRobo3(int *CompassDef)//RightFront
ryuna 3:8d718ddb84c9 349 {}
ryuna 3:8d718ddb84c9 350 void ControlRobo4(int *CompassDef)//Right
ryuna 3:8d718ddb84c9 351 {
ryuna 3:8d718ddb84c9 352
ryuna 3:8d718ddb84c9 353
ryuna 3:8d718ddb84c9 354
ryuna 3:8d718ddb84c9 355 }
ryuna 3:8d718ddb84c9 356 void ControlRobo5(int *CompassDef)//RightBack
ryuna 3:8d718ddb84c9 357 {}
ryuna 3:8d718ddb84c9 358 void ControlRobo6(int *CompassDef)//BackRight
ryuna 3:8d718ddb84c9 359 {}
ryuna 3:8d718ddb84c9 360 void ControlRobo7(int *CompassDef)//Back
ryuna 3:8d718ddb84c9 361 {}
ryuna 3:8d718ddb84c9 362 void ControlRobo8(int *CompassDef)//BackLeft
ryuna 3:8d718ddb84c9 363 {}
ryuna 3:8d718ddb84c9 364 void ControlRobo9(int *CompassDef)//LeftBack
ryuna 3:8d718ddb84c9 365 {}
ryuna 3:8d718ddb84c9 366 void ControlRobo10(int *CompassDef)//Left
ryuna 3:8d718ddb84c9 367 {}
ryuna 3:8d718ddb84c9 368 void ControlRobo11(int *CompassDef)//LeftFront
ryuna 3:8d718ddb84c9 369 {}
ryuna 3:8d718ddb84c9 370 void GoHome(int *CompassDef)//Ball is not found.
ryuna 3:8d718ddb84c9 371 {}
ryuna 3:8d718ddb84c9 372
ryuna 3:8d718ddb84c9 373
ryuna 2:054444aa1990 374
ryuna 2:054444aa1990 375
ryuna 1:7d4921b5d638 376 //通信（モータ用）
ryuna 1:7d4921b5d638 377 void tx_motor(){
ryuna 1:7d4921b5d638 378 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 1:7d4921b5d638 379 Motor.printf("%s",StringFIN.c_str());
ryuna 1:7d4921b5d638 380 }
ryuna 0:ae08e2e1d82d 381
ryuna 3:8d718ddb84c9 382 void SetUp(int *compass_def){
ryuna 2:054444aa1990 383 /*初期化*/
ryuna 3:8d718ddb84c9 384 int Compass;
ryuna 3:8d718ddb84c9 385 Kick = 0;
ryuna 3:8d718ddb84c9 386 DebugSw[0].mode(PullUp);
ryuna 3:8d718ddb84c9 387 DebugSw[1].mode(PullUp);
ryuna 3:8d718ddb84c9 388 DebugSw[2].mode(PullUp);
ryuna 3:8d718ddb84c9 389 DebugSw[3].mode(PullUp);
ryuna 1:7d4921b5d638 390 Motor.baud(115200); //ボーレート設定
ryuna 1:7d4921b5d638 391 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 1:7d4921b5d638 392 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 2:054444aa1990 393 move(0,0);//停止
ryuna 2:054444aa1990 394
ryuna 2:054444aa1990 395 Step.Resets();
ryuna 2:054444aa1990 396 Step.ReleseSW(0,1);//記憶がない
ryuna 2:054444aa1990 397
ryuna 3:8d718ddb84c9 398 hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ryuna 3:8d718ddb84c9 399 *compass_def = (hmc6352.sample() / 10);
ryuna 3:8d718ddb84c9 400
ryuna 3:8d718ddb84c9 401 Compass = ((hmc6352.sample() / 10) + 540 - *compass_def) % 360;
ryuna 3:8d718ddb84c9 402 /*前を向くように設定をするべき*/
ryuna 2:054444aa1990 403 Lcd.cls();
ryuna 2:054444aa1990 404
ryuna 2:054444aa1990 405 /*初期化終了*/
ryuna 0:ae08e2e1d82d 406
ryuna 0:ae08e2e1d82d 407
ryuna 2:054444aa1990 408 }
ryuna 2:054444aa1990 409 void StartLoop(){
ryuna 2:054444aa1990 410 /*
ryuna 2:054444aa1990 411 *スイッチが押されるまでロボットはスタートしない．
ryuna 2:054444aa1990 412 *
ryuna 2:054444aa1990 413 *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
ryuna 2:054444aa1990 414 *
ryuna 2:054444aa1990 415 *
ryuna 2:054444aa1990 416 */
ryuna 2:054444aa1990 417
ryuna 2:054444aa1990 418 }
ryuna 2:054444aa1990 419 int main() {
ryuna 3:8d718ddb84c9 420
ryuna 3:8d718ddb84c9 421 /*Ir*/
ryuna 3:8d718ddb84c9 422 uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
ryuna 2:054444aa1990 423
ryuna 2:054444aa1990 424 /*Line*/
ryuna 3:8d718ddb84c9 425 uint8_t Line;
ryuna 3:8d718ddb84c9 426
ryuna 3:8d718ddb84c9 427 /*Compass*/
ryuna 3:8d718ddb84c9 428 int CompassDef = 0, Compass = 0;
ryuna 2:054444aa1990 429
ryuna 3:8d718ddb84c9 430 /*State */
ryuna 3:8d718ddb84c9 431 //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
ryuna 3:8d718ddb84c9 432 uint8_t LineIr = NA;
ryuna 3:8d718ddb84c9 433
ryuna 2:054444aa1990 434
ryuna 2:054444aa1990 435
ryuna 3:8d718ddb84c9 436 /*関数ポインタ*/
ryuna 3:8d718ddb84c9 437 void (*ControlRobo[13])(int *);//irの位置による分岐
ryuna 3:8d718ddb84c9 438 ControlRobo[0] = ControlRobo0;
ryuna 3:8d718ddb84c9 439 ControlRobo[1] = ControlRobo1;
ryuna 3:8d718ddb84c9 440 ControlRobo[2] = ControlRobo2;
ryuna 3:8d718ddb84c9 441 ControlRobo[3] = ControlRobo3;
ryuna 3:8d718ddb84c9 442 ControlRobo[4] = ControlRobo4;
ryuna 3:8d718ddb84c9 443 ControlRobo[5] = ControlRobo5;
ryuna 3:8d718ddb84c9 444 ControlRobo[6] = ControlRobo6;
ryuna 3:8d718ddb84c9 445 ControlRobo[7] = ControlRobo7;
ryuna 3:8d718ddb84c9 446 ControlRobo[8] = ControlRobo8;
ryuna 3:8d718ddb84c9 447 ControlRobo[9] = ControlRobo9;
ryuna 3:8d718ddb84c9 448 ControlRobo[10] = ControlRobo10;
ryuna 3:8d718ddb84c9 449 ControlRobo[11] = ControlRobo11;
ryuna 3:8d718ddb84c9 450 ControlRobo[12] = GoHome;
ryuna 2:054444aa1990 451
ryuna 3:8d718ddb84c9 452
ryuna 3:8d718ddb84c9 453 SetUp(&CompassDef);
ryuna 2:054444aa1990 454
ryuna 2:054444aa1990 455 StartLoop();
ryuna 2:054444aa1990 456
ryuna 0:ae08e2e1d82d 457 while(1) {
ryuna 2:054444aa1990 458
ryuna 2:054444aa1990 459 Line = LineReceive();
ryuna 2:054444aa1990 460
ryuna 3:8d718ddb84c9 461 if(Line){ //ラインを検知していないか
ryuna 3:8d718ddb84c9 462 LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。
ryuna 3:8d718ddb84c9 463 while(LineIr){
ryuna 3:8d718ddb84c9 464 move(0,0);//
ryuna 3:8d718ddb84c9 465 Led[1] = Led[2] = Led[3] = 1;
ryuna 2:054444aa1990 466
ryuna 3:8d718ddb84c9 467 LineIr = LineReceive() & (IrReceiveS()/3 + 1);
ryuna 3:8d718ddb84c9 468 }
ryuna 3:8d718ddb84c9 469 Led[1] = Led[2] = Led[3] = 0;
ryuna 3:8d718ddb84c9 470 continue;
ryuna 3:8d718ddb84c9 471
ryuna 3:8d718ddb84c9 472 }
ryuna 3:8d718ddb84c9 473
ryuna 3:8d718ddb84c9 474 if(Step.BusyCheck()){//ステッピングが動いていないか
ryuna 3:8d718ddb84c9 475 continue;
ryuna 3:8d718ddb84c9 476
ryuna 3:8d718ddb84c9 477 }
ryuna 3:8d718ddb84c9 478
ryuna 3:8d718ddb84c9 479 IrNum = IrReceiveS();
ryuna 3:8d718ddb84c9 480
ryuna 3:8d718ddb84c9 481 (*ControlRobo[IrNum])(&CompassDef);//irの位置によったループ等の処理に移る。
ryuna 3:8d718ddb84c9 482
ryuna 3:8d718ddb84c9 483 //wait_ms(0);
ryuna 3:8d718ddb84c9 484
ryuna 0:ae08e2e1d82d 485 }
ryuna 0:ae08e2e1d82d 486 }