CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T

CatPot for defence on RoboCup in 2015 winter

Dependencies:   AQM0802A HMC6352 MultiSerial PID Servo mbed

main.cpp@3:2f74791564c9, 2015-03-11 (annotated)

Committer:
lilac0112_1
Date:
Wed Mar 11 07:47:19 2015 +0000
Revision:
3:2f74791564c9
Parent:
2:39135c67083d
Child:
4:7c488c059498
Send to F.F.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:d35efbf4d62e 1 /***********************************
lilac0112_1 2:39135c67083d 2 *RoboCupJunior Soccer B Open 2015
lilac0112_1 2:39135c67083d 3 *Koshinnestu progrum
lilac0112_1 0:d35efbf4d62e 4 *
lilac0112_1 0:d35efbf4d62e 5 *　
lilac0112_1 0:d35efbf4d62e 6 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
lilac0112_1 0:d35efbf4d62e 7 *
lilac0112_1 0:d35efbf4d62e 8 *　MotorDriverにmaxonに命令
lilac0112_1 0:d35efbf4d62e 9 *　
lilac0112_1 2:39135c67083d 10 *　servoにステアリング指示
lilac0112_1 0:d35efbf4d62e 11 *　
lilac0112_1 0:d35efbf4d62e 12 *　LCDでデバック
lilac0112_1 0:d35efbf4d62e 13 *
lilac0112_1 0:d35efbf4d62e 14 *　スイッチ4つとスタートスイッチで処理を実行
lilac0112_1 0:d35efbf4d62e 15 *
lilac0112_1 2:39135c67083d 16
lilac0112_1 0:d35efbf4d62e 17 *************************
lilac0112_1 2:39135c67083d 18 * Pin Map
lilac0112_1 2:39135c67083d 19 *
lilac0112_1 2:39135c67083d 20 * p5~p8 >> BusIn >> LineSensor
lilac0112_1 0:d35efbf4d62e 21 *
lilac0112_1 2:39135c67083d 22 * p9,p10 >> I2C >> LPC1114FN28/102 read & Compass
lilac0112_1 0:d35efbf4d62e 23 *
lilac0112_1 2:39135c67083d 24 * p13,p14 >> Serial >> Motor
lilac0112_1 2:39135c67083d 25 *
lilac0112_1 2:39135c67083d 26 * p21 >> PwmOut >> Servo
lilac0112_1 0:d35efbf4d62e 27 *
lilac0112_1 2:39135c67083d 28 * p22~p26 >> DigitalIn >> DebugSw and StartSw
lilac0112_1 0:d35efbf4d62e 29 *
lilac0112_1 2:39135c67083d 30 * p27,p28 >> I2C >> DebugLCD
lilac0112_1 0:d35efbf4d62e 31 *
lilac0112_1 2:39135c67083d 32 * p29 >> DigitalOut >> Kicker
lilac0112_1 2:39135c67083d 33 *
lilac0112_1 2:39135c67083d 34 * *never use pin number p11,p12,p15,p16,p17,p18,p19,p20,p30
lilac0112_1 2:39135c67083d 35 *
lilac0112_1 2:39135c67083d 36 *
lilac0112_1 0:d35efbf4d62e 37 ******************************/
lilac0112_1 0:d35efbf4d62e 38
lilac0112_1 0:d35efbf4d62e 39 #include "mbed.h"
lilac0112_1 2:39135c67083d 40 #include <math.h>
lilac0112_1 2:39135c67083d 41 #include <sstream>
lilac0112_1 3:2f74791564c9 42
lilac0112_1 3:2f74791564c9 43 #include "Servo.h"
lilac0112_1 0:d35efbf4d62e 44 #include "PID.h"
lilac0112_1 0:d35efbf4d62e 45 #include "AQM0802A.h"
lilac0112_1 2:39135c67083d 46 #include "main.h"
lilac0112_1 0:d35efbf4d62e 47
lilac0112_1 0:d35efbf4d62e 48
lilac0112_1 3:2f74791564c9 49 //#include "MultiSerial.h"
lilac0112_1 3:2f74791564c9 50 /*
lilac0112_1 3:2f74791564c9 51 void Receive(){
lilac0112_1 3:2f74791564c9 52 IrData[0] = rx_data[0];
lilac0112_1 3:2f74791564c9 53 IrData[1] = rx_data[1];
lilac0112_1 3:2f74791564c9 54 IrData[2] = rx_data[2];
lilac0112_1 2:39135c67083d 55 PingData[0] = rx_data[3];
lilac0112_1 2:39135c67083d 56 PingData[1] = rx_data[4];
lilac0112_1 2:39135c67083d 57 PingData[2] = rx_data[5];
lilac0112_1 2:39135c67083d 58 PingData[3] = rx_data[6];
lilac0112_1 3:2f74791564c9 59
lilac0112_1 0:d35efbf4d62e 60
lilac0112_1 3:2f74791564c9 61 if((IrData[0] == 255)||(IrData[1] == 255)||(IrData[2] == 255)){
lilac0112_1 2:39135c67083d 62 IrNum = 12;
lilac0112_1 2:39135c67083d 63 return;
lilac0112_1 0:d35efbf4d62e 64 }
lilac0112_1 2:39135c67083d 65 IrNum = IrData[0]/12;
lilac0112_1 3:2f74791564c9 66
lilac0112_1 0:d35efbf4d62e 67
lilac0112_1 0:d35efbf4d62e 68 }
lilac0112_1 3:2f74791564c9 69 */
lilac0112_1 3:2f74791564c9 70 void move(int vr,int vl, double vs ,int Degree){
lilac0112_1 0:d35efbf4d62e 71 double pwm[4] = {0};
lilac0112_1 0:d35efbf4d62e 72 uint8_t i = 0;
lilac0112_1 3:2f74791564c9 73 double vias=10.0;
lilac0112_1 3:2f74791564c9 74 double dif;
lilac0112_1 3:2f74791564c9 75
lilac0112_1 3:2f74791564c9 76 dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
lilac0112_1 3:2f74791564c9 77 if(abs(dif)<vias){
lilac0112_1 3:2f74791564c9 78
lilac0112_1 3:2f74791564c9 79 pwm[0] = vr;
lilac0112_1 3:2f74791564c9 80 pwm[1] = 0;
lilac0112_1 3:2f74791564c9 81 pwm[2] = 0;
lilac0112_1 3:2f74791564c9 82 pwm[3] = vl*(1.0)*(-1.0);
lilac0112_1 3:2f74791564c9 83 }
lilac0112_1 3:2f74791564c9 84 else{
lilac0112_1 3:2f74791564c9 85
lilac0112_1 3:2f74791564c9 86 if(dif<0){
lilac0112_1 3:2f74791564c9 87 //vs = vs - (20-vias);
lilac0112_1 3:2f74791564c9 88 vs = -(20+(abs(dif))*0.1);
lilac0112_1 3:2f74791564c9 89 if( abs(dif) >= 100) vs = -(20+(100)*0.1);
lilac0112_1 3:2f74791564c9 90 }
lilac0112_1 3:2f74791564c9 91 else{
lilac0112_1 3:2f74791564c9 92 //vs = vs + (20-vias);
lilac0112_1 3:2f74791564c9 93 vs = (20+(abs(dif))*0.1);
lilac0112_1 3:2f74791564c9 94 if( abs(dif) >= 100) vs = (20+(100)*0.1);
lilac0112_1 3:2f74791564c9 95 }
lilac0112_1 3:2f74791564c9 96
lilac0112_1 3:2f74791564c9 97 pwm[0] = vs;
lilac0112_1 3:2f74791564c9 98 pwm[1] = 0;
lilac0112_1 3:2f74791564c9 99 pwm[2] = 0;
lilac0112_1 3:2f74791564c9 100 pwm[3] = vs*(-1)*(-1);
lilac0112_1 3:2f74791564c9 101 }
lilac0112_1 3:2f74791564c9 102 if(Loop==1){
lilac0112_1 3:2f74791564c9 103 pwm[0] = 0;
lilac0112_1 3:2f74791564c9 104 pwm[1] = 0;
lilac0112_1 3:2f74791564c9 105 pwm[2] = 0;
lilac0112_1 3:2f74791564c9 106 pwm[3] = 0;
lilac0112_1 3:2f74791564c9 107 }
lilac0112_1 3:2f74791564c9 108 /*if(vs<0){
lilac0112_1 3:2f74791564c9 109 pwm[0] = vr - vs*k;
lilac0112_1 3:2f74791564c9 110 pwm[1] = 0;
lilac0112_1 3:2f74791564c9 111 pwm[2] = 0;
lilac0112_1 3:2f74791564c9 112 pwm[3] = vl*1.2 + vs*k;
lilac0112_1 3:2f74791564c9 113 }else{
lilac0112_1 3:2f74791564c9 114 pwm[0] = vr + vs*k;
lilac0112_1 3:2f74791564c9 115 pwm[1] = 0;
lilac0112_1 3:2f74791564c9 116 pwm[2] = 0;
lilac0112_1 3:2f74791564c9 117 pwm[3] = (vl - vs*k)*1.2;
lilac0112_1 3:2f74791564c9 118 }*/
lilac0112_1 0:d35efbf4d62e 119 for(i = 0; i < 4; i++){
lilac0112_1 0:d35efbf4d62e 120 if(pwm[i] > 100){
lilac0112_1 0:d35efbf4d62e 121 pwm[i] = 100;
lilac0112_1 0:d35efbf4d62e 122 }else if(pwm[i] < -100){
lilac0112_1 0:d35efbf4d62e 123 pwm[i] = -100;
lilac0112_1 0:d35efbf4d62e 124 }
lilac0112_1 0:d35efbf4d62e 125 speed[i] = pwm[i];
lilac0112_1 0:d35efbf4d62e 126 }
lilac0112_1 3:2f74791564c9 127 if(Degree > 110){
lilac0112_1 3:2f74791564c9 128 Degree = 110;
lilac0112_1 3:2f74791564c9 129 }else if(Degree < -110){
lilac0112_1 3:2f74791564c9 130 Degree = -110;
lilac0112_1 3:2f74791564c9 131 }
lilac0112_1 3:2f74791564c9 132 SetDegree = Degree;
lilac0112_1 3:2f74791564c9 133 S555.position(SetDegree*11.0/9.0);
lilac0112_1 2:39135c67083d 134 wait_ms(10);
lilac0112_1 2:39135c67083d 135 }
lilac0112_1 3:2f74791564c9 136
lilac0112_1 3:2f74791564c9 137 uint8_t PingChange(uint8_t LineData){
lilac0112_1 3:2f74791564c9 138 static uint8_t Last_Line;
lilac0112_1 3:2f74791564c9 139 static uint8_t Last_Ping;
lilac0112_1 3:2f74791564c9 140 uint8_t LinePing = 0;
lilac0112_1 3:2f74791564c9 141
lilac0112_1 3:2f74791564c9 142 if(!LineData){
lilac0112_1 3:2f74791564c9 143 return 0;
lilac0112_1 3:2f74791564c9 144 }
lilac0112_1 3:2f74791564c9 145
lilac0112_1 3:2f74791564c9 146 if(PingData[0] <40) LinePing = LinePing + 1;
lilac0112_1 3:2f74791564c9 147 if(PingData[1] <40) LinePing = LinePing + 2;
lilac0112_1 3:2f74791564c9 148 if(PingData[2] <40) LinePing = LinePing + 4;
lilac0112_1 3:2f74791564c9 149 if(PingData[3] <40) LinePing = LinePing + 8;
lilac0112_1 3:2f74791564c9 150
lilac0112_1 3:2f74791564c9 151
lilac0112_1 3:2f74791564c9 152 if(LinePing&0x01){
lilac0112_1 3:2f74791564c9 153 if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
lilac0112_1 3:2f74791564c9 154 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 155 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 156 return 1;
lilac0112_1 3:2f74791564c9 157 }
lilac0112_1 3:2f74791564c9 158 }
lilac0112_1 3:2f74791564c9 159 if(LinePing&0x02){
lilac0112_1 3:2f74791564c9 160 if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
lilac0112_1 3:2f74791564c9 161 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 162 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 163 return 2;
lilac0112_1 3:2f74791564c9 164 }
lilac0112_1 3:2f74791564c9 165 }
lilac0112_1 3:2f74791564c9 166 if(LinePing&0x04){
lilac0112_1 3:2f74791564c9 167 if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
lilac0112_1 3:2f74791564c9 168 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 169 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 170 return 4;
lilac0112_1 3:2f74791564c9 171 }
lilac0112_1 3:2f74791564c9 172 }
lilac0112_1 3:2f74791564c9 173 if(LinePing&0x08){
lilac0112_1 3:2f74791564c9 174 if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
lilac0112_1 3:2f74791564c9 175 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 176 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 177 return 8;
lilac0112_1 3:2f74791564c9 178 }
lilac0112_1 3:2f74791564c9 179 }
lilac0112_1 3:2f74791564c9 180 Last_Ping = 0;
lilac0112_1 3:2f74791564c9 181 Last_Line = 0;
lilac0112_1 3:2f74791564c9 182 return 0;
lilac0112_1 3:2f74791564c9 183
lilac0112_1 3:2f74791564c9 184 }
lilac0112_1 3:2f74791564c9 185
lilac0112_1 3:2f74791564c9 186 void fool (int *Degree, int *Power){
lilac0112_1 3:2f74791564c9 187 static int Last_Degree = 0;
lilac0112_1 2:39135c67083d 188 static int Last_Vector = 1;
lilac0112_1 3:2f74791564c9 189 int degree = *Degree;
lilac0112_1 2:39135c67083d 190 int Temp;
lilac0112_1 3:2f74791564c9 191
lilac0112_1 3:2f74791564c9 192
lilac0112_1 3:2f74791564c9 193 if((*Degree <0)||(*Degree >=360)){
lilac0112_1 3:2f74791564c9 194 *Degree = 0;
lilac0112_1 3:2f74791564c9 195 Last_Degree = 0;
lilac0112_1 3:2f74791564c9 196 Last_Vector = 1;
lilac0112_1 3:2f74791564c9 197 return ;
lilac0112_1 3:2f74791564c9 198 }
lilac0112_1 3:2f74791564c9 199
lilac0112_1 3:2f74791564c9 200 Temp = Last_Degree % 180;
lilac0112_1 3:2f74791564c9 201 if((Temp==90)){
lilac0112_1 3:2f74791564c9 202 Temp = *Degree % 180;
lilac0112_1 3:2f74791564c9 203 if((Temp==90)){
lilac0112_1 3:2f74791564c9 204 Temp = abs(*Degree - Last_Degree);
lilac0112_1 2:39135c67083d 205 if(Temp>160){
lilac0112_1 2:39135c67083d 206 Last_Vector = -1 * Last_Vector;//正転逆転切り替え
lilac0112_1 3:2f74791564c9 207 if(*Degree/180){
lilac0112_1 3:2f74791564c9 208 *Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180);
lilac0112_1 2:39135c67083d 209 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 210 }else{
lilac0112_1 3:2f74791564c9 211 *Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree);
lilac0112_1 2:39135c67083d 212 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 213 }
lilac0112_1 3:2f74791564c9 214 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 215 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 216 *Degree = 0;
lilac0112_1 3:2f74791564c9 217 }
lilac0112_1 2:39135c67083d 218 return;
lilac0112_1 2:39135c67083d 219 }else if((Last_Vector+2) == 1){
lilac0112_1 2:39135c67083d 220 /*逆転のまま角度拡張*/
lilac0112_1 3:2f74791564c9 221 if(*Degree/180){
lilac0112_1 3:2f74791564c9 222 *Degree = -360 + *Degree ;
lilac0112_1 3:2f74791564c9 223
lilac0112_1 2:39135c67083d 224 }
lilac0112_1 3:2f74791564c9 225 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 226 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 227 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 228 *Degree = 0;
lilac0112_1 3:2f74791564c9 229
lilac0112_1 3:2f74791564c9 230 }
lilac0112_1 2:39135c67083d 231 return;
lilac0112_1 2:39135c67083d 232
lilac0112_1 2:39135c67083d 233 }else if((Last_Vector+2) == 3){
lilac0112_1 2:39135c67083d 234 /*正転のまま*/
lilac0112_1 3:2f74791564c9 235 if(*Degree/180){
lilac0112_1 3:2f74791564c9 236 *Degree = -360 + *Degree ;
lilac0112_1 2:39135c67083d 237 }
lilac0112_1 3:2f74791564c9 238 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 239 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 240 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 241 *Degree = 0;
lilac0112_1 3:2f74791564c9 242 }
lilac0112_1 2:39135c67083d 243 return;
lilac0112_1 2:39135c67083d 244 }
lilac0112_1 2:39135c67083d 245 }
lilac0112_1 2:39135c67083d 246 }
lilac0112_1 2:39135c67083d 247 /*通常動作*/
lilac0112_1 3:2f74791564c9 248 if(*Degree == 0){
lilac0112_1 3:2f74791564c9 249 Last_Vector = DegreeToVector[0];
lilac0112_1 3:2f74791564c9 250 *Degree = Angle[*Degree%180];
lilac0112_1 3:2f74791564c9 251 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 252 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 253 return ;
lilac0112_1 3:2f74791564c9 254 }
lilac0112_1 3:2f74791564c9 255 Last_Vector = DegreeToVector[(*Degree-1)/90];
lilac0112_1 3:2f74791564c9 256 *Degree = Angle[*Degree%180];
lilac0112_1 2:39135c67083d 257 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 258 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 259 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 260 *Degree = 0;
lilac0112_1 3:2f74791564c9 261 }
lilac0112_1 2:39135c67083d 262
lilac0112_1 2:39135c67083d 263 }
lilac0112_1 3:2f74791564c9 264 int IrDegree(){
lilac0112_1 2:39135c67083d 265 /*irの1位の値，2位の場所からirの360へ持っていく*/
lilac0112_1 2:39135c67083d 266
lilac0112_1 3:2f74791564c9 267 uint8_t IrF ,IrS;
lilac0112_1 3:2f74791564c9 268 unsigned int irdegree = 0;
lilac0112_1 3:2f74791564c9 269 if(IrNum >=12){
lilac0112_1 3:2f74791564c9 270 return 0;
lilac0112_1 3:2f74791564c9 271 }
lilac0112_1 3:2f74791564c9 272 IrF = IrData[0]/12;
lilac0112_1 3:2f74791564c9 273 IrS = IrData[0]%12;
lilac0112_1 3:2f74791564c9 274
lilac0112_1 3:2f74791564c9 275 if(IrF == 0 ){
lilac0112_1 3:2f74791564c9 276 if(IrS == 11){
lilac0112_1 3:2f74791564c9 277 irdegree = 15;
lilac0112_1 2:39135c67083d 278
lilac0112_1 3:2f74791564c9 279 }else if(IrS == 1){
lilac0112_1 3:2f74791564c9 280 irdegree = 345;
lilac0112_1 3:2f74791564c9 281 }
lilac0112_1 3:2f74791564c9 282 return irdegree;
lilac0112_1 3:2f74791564c9 283 }
lilac0112_1 3:2f74791564c9 284 irdegree = 360 - IrF*30;
lilac0112_1 3:2f74791564c9 285 if(IrS&&(abs(IrF-IrS) == 1)){
lilac0112_1 3:2f74791564c9 286 irdegree = irdegree - (IrF - IrS)*15;
lilac0112_1 2:39135c67083d 287
lilac0112_1 2:39135c67083d 288 }
lilac0112_1 3:2f74791564c9 289 if(irdegree>=360){
lilac0112_1 3:2f74791564c9 290 return 0;
lilac0112_1 3:2f74791564c9 291 }
lilac0112_1 3:2f74791564c9 292
lilac0112_1 3:2f74791564c9 293 return irdegree;
lilac0112_1 0:d35efbf4d62e 294 }
lilac0112_1 0:d35efbf4d62e 295
lilac0112_1 2:39135c67083d 296
lilac0112_1 3:2f74791564c9 297 void IrFrontAction()//ball 12 or 0 o-clock
lilac0112_1 1:e3248f278663 298 {
lilac0112_1 3:2f74791564c9 299 if(IrData[1]>70){
lilac0112_1 3:2f74791564c9 300 if(abs(CompassPID)>10){
lilac0112_1 3:2f74791564c9 301 move(0,0,CompassPID,0);
lilac0112_1 3:2f74791564c9 302 return;
lilac0112_1 3:2f74791564c9 303 }
lilac0112_1 3:2f74791564c9 304 move(30,30,CompassPID,0);
lilac0112_1 3:2f74791564c9 305 return;
lilac0112_1 3:2f74791564c9 306 }
lilac0112_1 3:2f74791564c9 307 if(IrData[1]>60){
lilac0112_1 3:2f74791564c9 308 move(25,25,CompassPID,0);
lilac0112_1 3:2f74791564c9 309 return;
lilac0112_1 3:2f74791564c9 310 }
lilac0112_1 3:2f74791564c9 311 /*IrData[1]>500*/
lilac0112_1 3:2f74791564c9 312
lilac0112_1 3:2f74791564c9 313 if(PingData[0]<50){
lilac0112_1 3:2f74791564c9 314 if((PingData[1]<60)&&(PingData[1]>40)){
lilac0112_1 3:2f74791564c9 315 /*右側に居る*/
lilac0112_1 3:2f74791564c9 316 move(25,40,0,10);
lilac0112_1 3:2f74791564c9 317 wait_ms(100);
lilac0112_1 3:2f74791564c9 318 move(30,30,0,0);
lilac0112_1 3:2f74791564c9 319 return;
lilac0112_1 3:2f74791564c9 320 }
lilac0112_1 3:2f74791564c9 321 if((PingData[1]<60)&&(PingData[1]>40)){
lilac0112_1 3:2f74791564c9 322 /*左側に居る*/
lilac0112_1 3:2f74791564c9 323 move(40,25,0,-10);
lilac0112_1 3:2f74791564c9 324 wait_ms(100);
lilac0112_1 3:2f74791564c9 325 move(30,30,0,0);
lilac0112_1 3:2f74791564c9 326 return;
lilac0112_1 3:2f74791564c9 327 }
lilac0112_1 3:2f74791564c9 328 /*それ以外*/
lilac0112_1 3:2f74791564c9 329 move(10,10,CompassPID,0);
lilac0112_1 3:2f74791564c9 330 //Receive();
lilac0112_1 3:2f74791564c9 331 if(!(IrNum == 0)) return;
lilac0112_1 3:2f74791564c9 332 move(40,40,CompassPID,0);
lilac0112_1 3:2f74791564c9 333 return ;
lilac0112_1 3:2f74791564c9 334 }
lilac0112_1 3:2f74791564c9 335 move(20,20,CompassPID,0);
lilac0112_1 3:2f74791564c9 336
lilac0112_1 3:2f74791564c9 337
lilac0112_1 1:e3248f278663 338
lilac0112_1 1:e3248f278663 339 }
lilac0112_1 2:39135c67083d 340
lilac0112_1 3:2f74791564c9 341 void IrBackAction()//ball found six o-clock
lilac0112_1 0:d35efbf4d62e 342 {
lilac0112_1 2:39135c67083d 343 /***
lilac0112_1 2:39135c67083d 344 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
lilac0112_1 2:39135c67083d 345 *
lilac0112_1 2:39135c67083d 346 **/
lilac0112_1 3:2f74791564c9 347 if(PingData[1]>PingData[3]){
lilac0112_1 2:39135c67083d 348 /*右が大きい*/
lilac0112_1 3:2f74791564c9 349 if(IrData[1]>70){
lilac0112_1 3:2f74791564c9 350 move(-20,-20,CompassPID,45);
lilac0112_1 3:2f74791564c9 351 return;
lilac0112_1 3:2f74791564c9 352 }
lilac0112_1 3:2f74791564c9 353 if(IrData[1]>60){
lilac0112_1 3:2f74791564c9 354 move(-20,-20,CompassPID,60);
lilac0112_1 3:2f74791564c9 355 return;
lilac0112_1 3:2f74791564c9 356 }
lilac0112_1 3:2f74791564c9 357 move(-20,-20,CompassPID,80);
lilac0112_1 0:d35efbf4d62e 358 return;
lilac0112_1 0:d35efbf4d62e 359 }
lilac0112_1 2:39135c67083d 360 /*左が大きい*/
lilac0112_1 0:d35efbf4d62e 361
lilac0112_1 3:2f74791564c9 362 if(IrData[1]>70){
lilac0112_1 3:2f74791564c9 363 move(-20,-20,CompassPID,-45);
lilac0112_1 3:2f74791564c9 364 return;
lilac0112_1 0:d35efbf4d62e 365 }
lilac0112_1 3:2f74791564c9 366 if(IrData[1]>60){
lilac0112_1 3:2f74791564c9 367 move(-20,-20,CompassPID,-60);
lilac0112_1 3:2f74791564c9 368 return;
lilac0112_1 3:2f74791564c9 369 }
lilac0112_1 3:2f74791564c9 370 move(-20,-20,CompassPID,-80);
lilac0112_1 3:2f74791564c9 371 return;
lilac0112_1 2:39135c67083d 372
lilac0112_1 3:2f74791564c9 373 }
lilac0112_1 3:2f74791564c9 374
lilac0112_1 3:2f74791564c9 375 void GoHome()//Ball is not found.
lilac0112_1 3:2f74791564c9 376 {
lilac0112_1 2:39135c67083d 377
lilac0112_1 3:2f74791564c9 378 /*if(PingData[2] >=60){//後ろからの距離60cm
lilac0112_1 3:2f74791564c9 379 move(-20,-20,CompassPID,0);
lilac0112_1 2:39135c67083d 380 return ;
lilac0112_1 2:39135c67083d 381 }
lilac0112_1 3:2f74791564c9 382 */
lilac0112_1 3:2f74791564c9 383 move(0,0,CompassPID,0);//stop
lilac0112_1 3:2f74791564c9 384
lilac0112_1 2:39135c67083d 385 }
lilac0112_1 0:d35efbf4d62e 386
lilac0112_1 0:d35efbf4d62e 387
lilac0112_1 2:39135c67083d 388 void PidUpdate()
lilac0112_1 2:39135c67083d 389 {
lilac0112_1 0:d35efbf4d62e 390
lilac0112_1 3:2f74791564c9 391 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
lilac0112_1 3:2f74791564c9 392 InputPID = ((Compass/10) + 540 - CompassDef) % 360;
lilac0112_1 3:2f74791564c9 393
lilac0112_1 3:2f74791564c9 394 pid.setProcessValue(InputPID);
lilac0112_1 3:2f74791564c9 395 CompassPID = (pid.compute());
lilac0112_1 0:d35efbf4d62e 396 }
lilac0112_1 2:39135c67083d 397
lilac0112_1 2:39135c67083d 398
lilac0112_1 2:39135c67083d 399
lilac0112_1 2:39135c67083d 400
lilac0112_1 2:39135c67083d 401
lilac0112_1 2:39135c67083d 402
lilac0112_1 0:d35efbf4d62e 403
lilac0112_1 0:d35efbf4d62e 404 uint8_t SwRead(){
lilac0112_1 0:d35efbf4d62e 405 /******
lilac0112_1 2:39135c67083d 406 *retrun : sw_state
lilac0112_1 2:39135c67083d 407 *StartS = 0x01;
lilac0112_1 2:39135c67083d 408 *Debug2 = 0x02;
lilac0112_1 0:d35efbf4d62e 409 *Debug1 = 0x04;
lilac0112_1 2:39135c67083d 410 *Debug3 = 0x06;
lilac0112_1 2:39135c67083d 411 *Kicker = 0x08;
lilac0112_1 2:39135c67083d 412 *Calibration = 0x10;
lilac0112_1 2:39135c67083d 413
lilac0112_1 0:d35efbf4d62e 414 *
lilac0112_1 0:d35efbf4d62e 415 *****/
lilac0112_1 0:d35efbf4d62e 416 uint8_t i,temp,temp2;
lilac0112_1 2:39135c67083d 417 temp = ~Sw - 224;
lilac0112_1 2:39135c67083d 418 if(!(temp == Kicker
lilac0112_1 0:d35efbf4d62e 419 ||temp == Debug1
lilac0112_1 0:d35efbf4d62e 420 ||temp == Debug2
lilac0112_1 2:39135c67083d 421 ||temp == Debug3
lilac0112_1 3:2f74791564c9 422 ||temp == Debug12
lilac0112_1 3:2f74791564c9 423 ||temp == Debug23
lilac0112_1 2:39135c67083d 424 ||temp == StartS)) return 0;/*スイッチが押されていない*/
lilac0112_1 2:39135c67083d 425 if(!(temp == 0x00)){
lilac0112_1 0:d35efbf4d62e 426 for(i = 0; i < 50; i++);
lilac0112_1 2:39135c67083d 427 temp2 = ~Sw - 224;
lilac0112_1 0:d35efbf4d62e 428 if(temp == temp2){
lilac0112_1 0:d35efbf4d62e 429 return temp;
lilac0112_1 0:d35efbf4d62e 430 }
lilac0112_1 0:d35efbf4d62e 431 }
lilac0112_1 0:d35efbf4d62e 432 return 0;
lilac0112_1 0:d35efbf4d62e 433 }
lilac0112_1 0:d35efbf4d62e 434
lilac0112_1 0:d35efbf4d62e 435 //通信（モータ用）
lilac0112_1 0:d35efbf4d62e 436 void tx_motor(){
lilac0112_1 2:39135c67083d 437
lilac0112_1 3:2f74791564c9 438 /*
lilac0112_1 3:2f74791564c9 439 speed[0]=0;
lilac0112_1 3:2f74791564c9 440 speed[1]=0;
lilac0112_1 3:2f74791564c9 441 speed[2]=0;
lilac0112_1 3:2f74791564c9 442 speed[3]=0;
lilac0112_1 3:2f74791564c9 443 */
lilac0112_1 0:d35efbf4d62e 444 array(speed[0],speed[1],speed[3],speed[2]);
lilac0112_1 0:d35efbf4d62e 445 Motor.printf("%s",StringFIN.c_str());
lilac0112_1 0:d35efbf4d62e 446 }
lilac0112_1 0:d35efbf4d62e 447
lilac0112_1 2:39135c67083d 448 void SetUp(){
lilac0112_1 0:d35efbf4d62e 449 /*初期化*/
lilac0112_1 3:2f74791564c9 450
lilac0112_1 3:2f74791564c9 451
lilac0112_1 0:d35efbf4d62e 452 Motor.baud(115200); //ボーレート設定
lilac0112_1 0:d35efbf4d62e 453 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:d35efbf4d62e 454 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 2:39135c67083d 455
lilac0112_1 3:2f74791564c9 456 Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
lilac0112_1 3:2f74791564c9 457
lilac0112_1 3:2f74791564c9 458 S555.calibrate(0.0006, 120.0);
lilac0112_1 3:2f74791564c9 459 //S555.position(0.0); //初期位置にセット
lilac0112_1 2:39135c67083d 460 move(0,0,0,0);//停止
lilac0112_1 0:d35efbf4d62e 461
lilac0112_1 2:39135c67083d 462 Kick = 0;
lilac0112_1 2:39135c67083d 463 Sw.mode(PullUp);
lilac0112_1 0:d35efbf4d62e 464
lilac0112_1 2:39135c67083d 465 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 2:39135c67083d 466 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 2:39135c67083d 467 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 2:39135c67083d 468 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 3:2f74791564c9 469 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:d35efbf4d62e 470
lilac0112_1 2:39135c67083d 471 for(int i=0; i<15; i++){
lilac0112_1 3:2f74791564c9 472 CompassDef = (Compass / 10);
lilac0112_1 3:2f74791564c9 473 wait_ms(10);
lilac0112_1 2:39135c67083d 474 }
lilac0112_1 3:2f74791564c9 475 pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 3:2f74791564c9 476
lilac0112_1 0:d35efbf4d62e 477
lilac0112_1 0:d35efbf4d62e 478 }
lilac0112_1 0:d35efbf4d62e 479 void StartLoop(){
lilac0112_1 2:39135c67083d 480
lilac0112_1 0:d35efbf4d62e 481 uint8_t State = 0;
lilac0112_1 2:39135c67083d 482 uint8_t LineData = 0;
lilac0112_1 0:d35efbf4d62e 483 while(1){
lilac0112_1 2:39135c67083d 484 Led[0] = Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 2:39135c67083d 485 //Lcd.cls();
lilac0112_1 3:2f74791564c9 486
lilac0112_1 0:d35efbf4d62e 487 State = SwRead();
lilac0112_1 0:d35efbf4d62e 488 if(State == 0) continue;
lilac0112_1 0:d35efbf4d62e 489
lilac0112_1 2:39135c67083d 490 if(State == StartS){
lilac0112_1 2:39135c67083d 491 /*loop end & start*/
lilac0112_1 3:2f74791564c9 492 return;
lilac0112_1 0:d35efbf4d62e 493 }
lilac0112_1 2:39135c67083d 494
lilac0112_1 2:39135c67083d 495 if(State == Debug1){
lilac0112_1 2:39135c67083d 496 while((State == Debug1)){
lilac0112_1 2:39135c67083d 497 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 2:39135c67083d 498 Lcd.printf("%d\n",LineData);
lilac0112_1 2:39135c67083d 499
lilac0112_1 2:39135c67083d 500 wait_ms(100);
lilac0112_1 2:39135c67083d 501 State = SwRead();
lilac0112_1 2:39135c67083d 502 }
lilac0112_1 2:39135c67083d 503 Lcd.cls();
lilac0112_1 0:d35efbf4d62e 504 continue;
lilac0112_1 0:d35efbf4d62e 505
lilac0112_1 0:d35efbf4d62e 506 }
lilac0112_1 0:d35efbf4d62e 507 if(State == Debug2){
lilac0112_1 2:39135c67083d 508 while((State == Debug2)){
lilac0112_1 3:2f74791564c9 509 //Receive();
lilac0112_1 2:39135c67083d 510 Lcd.printf("%d\n",IrNum);
lilac0112_1 3:2f74791564c9 511 //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1],PingData[2]);
lilac0112_1 2:39135c67083d 512 wait_ms(100);
lilac0112_1 2:39135c67083d 513 State = SwRead();
lilac0112_1 2:39135c67083d 514 }
lilac0112_1 2:39135c67083d 515 Lcd.cls();
lilac0112_1 2:39135c67083d 516 continue;
lilac0112_1 0:d35efbf4d62e 517 }
lilac0112_1 2:39135c67083d 518
lilac0112_1 2:39135c67083d 519 if(State == Debug3){
lilac0112_1 2:39135c67083d 520 while((State == Debug3)){
lilac0112_1 2:39135c67083d 521 Lcd.printf("%d\n",((Compass / 10) + 540 - CompassDef) % 360);
lilac0112_1 3:2f74791564c9 522 wait_ms(100);
lilac0112_1 3:2f74791564c9 523 State = SwRead();
lilac0112_1 3:2f74791564c9 524 }
lilac0112_1 3:2f74791564c9 525 Lcd.cls();
lilac0112_1 3:2f74791564c9 526 continue;
lilac0112_1 3:2f74791564c9 527 }
lilac0112_1 3:2f74791564c9 528 if(State == Debug12){
lilac0112_1 3:2f74791564c9 529 while((State == Debug12)){
lilac0112_1 3:2f74791564c9 530 Lcd.printf("%d\n", PingData[0]);//Left
lilac0112_1 3:2f74791564c9 531 Lcd.printf("%d\n", PingData[1]);//Right
lilac0112_1 3:2f74791564c9 532 wait_ms(100);
lilac0112_1 3:2f74791564c9 533 State = SwRead();
lilac0112_1 3:2f74791564c9 534 }
lilac0112_1 3:2f74791564c9 535 Lcd.cls();
lilac0112_1 3:2f74791564c9 536 continue;
lilac0112_1 3:2f74791564c9 537 }
lilac0112_1 3:2f74791564c9 538 if(State == Debug23){
lilac0112_1 3:2f74791564c9 539 while((State == Debug23)){
lilac0112_1 3:2f74791564c9 540 Lcd.printf("%d\n", PingData[2]);//Back
lilac0112_1 3:2f74791564c9 541 Lcd.printf("%d\n", PingData[3]);//Front
lilac0112_1 2:39135c67083d 542 wait_ms(100);
lilac0112_1 2:39135c67083d 543 State = SwRead();
lilac0112_1 2:39135c67083d 544 }
lilac0112_1 2:39135c67083d 545 Lcd.cls();
lilac0112_1 2:39135c67083d 546 continue;
lilac0112_1 0:d35efbf4d62e 547 }
lilac0112_1 0:d35efbf4d62e 548
lilac0112_1 2:39135c67083d 549 if(State == Kicker){
lilac0112_1 3:2f74791564c9 550 while((State == Kicker)){
lilac0112_1 3:2f74791564c9 551 Lcd.printf("%f\n",CompassPID);
lilac0112_1 3:2f74791564c9 552 wait_ms(100);
lilac0112_1 3:2f74791564c9 553 State = SwRead();
lilac0112_1 3:2f74791564c9 554 }
lilac0112_1 3:2f74791564c9 555 Lcd.cls();
lilac0112_1 3:2f74791564c9 556 continue;
lilac0112_1 3:2f74791564c9 557
lilac0112_1 3:2f74791564c9 558 /*Led[0] = Led[1] = Led[2] = 0;
lilac0112_1 3:2f74791564c9 559 Kick = 1;
lilac0112_1 3:2f74791564c9 560 wait_ms(500);
lilac0112_1 3:2f74791564c9 561 Kick = 0;
lilac0112_1 2:39135c67083d 562 while((State == Kicker)){
lilac0112_1 2:39135c67083d 563 wait_ms(100);
lilac0112_1 2:39135c67083d 564 State = SwRead();
lilac0112_1 2:39135c67083d 565 }
lilac0112_1 3:2f74791564c9 566 continue;*/
lilac0112_1 2:39135c67083d 567 }
lilac0112_1 3:2f74791564c9 568 }
lilac0112_1 0:d35efbf4d62e 569
lilac0112_1 0:d35efbf4d62e 570 }
lilac0112_1 0:d35efbf4d62e 571 int main() {
lilac0112_1 0:d35efbf4d62e 572
lilac0112_1 3:2f74791564c9 573 //uint8_t IrNumOld = 0;//過去値
lilac0112_1 0:d35efbf4d62e 574 /*Line*/
lilac0112_1 2:39135c67083d 575 uint8_t LineData = 0;
lilac0112_1 3:2f74791564c9 576 uint8_t LinePing = 0;
lilac0112_1 3:2f74791564c9 577
lilac0112_1 2:39135c67083d 578
lilac0112_1 0:d35efbf4d62e 579 /*State */
lilac0112_1 3:2f74791564c9 580 uint8_t LineIr = 0;
lilac0112_1 3:2f74791564c9 581 uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06,
lilac0112_1 3:2f74791564c9 582
lilac0112_1 3:2f74791564c9 583 0x04,0x04,0x0B,0x08,0x08,0x09,0x00};
lilac0112_1 3:2f74791564c9 584
lilac0112_1 2:39135c67083d 585 /*行動設定*/
lilac0112_1 2:39135c67083d 586 int Power = 0;
lilac0112_1 3:2f74791564c9 587 int Degree = 0;
lilac0112_1 2:39135c67083d 588
lilac0112_1 2:39135c67083d 589 /*楽しい変数達*/
lilac0112_1 3:2f74791564c9 590 int nDegree =0;//基礎角
lilac0112_1 3:2f74791564c9 591 int addDegree = 0;//追加角
lilac0112_1 0:d35efbf4d62e 592
lilac0112_1 0:d35efbf4d62e 593
lilac0112_1 0:d35efbf4d62e 594
lilac0112_1 0:d35efbf4d62e 595 /*関数ポインタ*/
lilac0112_1 3:2f74791564c9 596
lilac0112_1 3:2f74791564c9 597
lilac0112_1 3:2f74791564c9 598 //void (*AnotherAction[3])(uint8_t [],double);
lilac0112_1 3:2f74791564c9 599 void (*AnotherAction[3])();
lilac0112_1 2:39135c67083d 600 AnotherAction[0] = IrFrontAction;
lilac0112_1 2:39135c67083d 601 AnotherAction[1] = IrBackAction;
lilac0112_1 2:39135c67083d 602 AnotherAction[2] = GoHome;
lilac0112_1 2:39135c67083d 603
lilac0112_1 3:2f74791564c9 604
lilac0112_1 2:39135c67083d 605 SetUp();/*set up routine*/
lilac0112_1 3:2f74791564c9 606
lilac0112_1 2:39135c67083d 607 StartLoop(); /*loop strat, switch push end*/
lilac0112_1 3:2f74791564c9 608 Led[0] = Led[1] = Led[2] = Led[3] = 0;
lilac0112_1 3:2f74791564c9 609 wait_ms(100);
lilac0112_1 2:39135c67083d 610
lilac0112_1 3:2f74791564c9 611 Loop = 0;
lilac0112_1 2:39135c67083d 612
lilac0112_1 3:2f74791564c9 613 while(1){
lilac0112_1 2:39135c67083d 614
lilac0112_1 3:2f74791564c9 615 S555.calibrate(0.0005, 120.0);
lilac0112_1 3:2f74791564c9 616
lilac0112_1 3:2f74791564c9 617 //Receive();
lilac0112_1 3:2f74791564c9 618 //Lcd.printf("%d\n",IrNum);
lilac0112_1 2:39135c67083d 619 /*白線を読んでいないか確認する*/
lilac0112_1 2:39135c67083d 620 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 3:2f74791564c9 621
lilac0112_1 2:39135c67083d 622 if(LineData){
lilac0112_1 3:2f74791564c9 623 LineIr = LineData & IrChange[IrNum];
lilac0112_1 3:2f74791564c9 624 LinePing = PingChange(LineData);
lilac0112_1 3:2f74791564c9 625 if(LineIr){
lilac0112_1 3:2f74791564c9 626 move(0,0,0,0);
lilac0112_1 3:2f74791564c9 627 while(LineIr){
lilac0112_1 3:2f74791564c9 628 Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 3:2f74791564c9 629 //Receive();
lilac0112_1 3:2f74791564c9 630 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 3:2f74791564c9 631 LineIr = LineData & IrChange[IrNum];
lilac0112_1 3:2f74791564c9 632 wait_ms(10);
lilac0112_1 3:2f74791564c9 633 }
lilac0112_1 3:2f74791564c9 634 }else if(LinePing){
lilac0112_1 3:2f74791564c9 635 move(0,0,0,0);
lilac0112_1 3:2f74791564c9 636 while(LinePing){
lilac0112_1 3:2f74791564c9 637 Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 3:2f74791564c9 638 //Receive();
lilac0112_1 3:2f74791564c9 639 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 3:2f74791564c9 640 LinePing = PingChange(LineData);
lilac0112_1 3:2f74791564c9 641
lilac0112_1 3:2f74791564c9 642 wait_ms(10);
lilac0112_1 3:2f74791564c9 643 }
lilac0112_1 0:d35efbf4d62e 644 }
lilac0112_1 3:2f74791564c9 645
lilac0112_1 3:2f74791564c9 646
lilac0112_1 0:d35efbf4d62e 647 Led[1] = Led[2] = Led[3] = 0;
lilac0112_1 2:39135c67083d 648
lilac0112_1 0:d35efbf4d62e 649 }
lilac0112_1 0:d35efbf4d62e 650
lilac0112_1 3:2f74791564c9 651 Power = 0;
lilac0112_1 3:2f74791564c9 652 Led[0] = 1;
lilac0112_1 3:2f74791564c9 653 Degree = 0;
lilac0112_1 3:2f74791564c9 654 SetC = 0.0;
lilac0112_1 3:2f74791564c9 655
lilac0112_1 0:d35efbf4d62e 656
lilac0112_1 2:39135c67083d 657 Led[3] = 1;
lilac0112_1 3:2f74791564c9 658 //Receive();
lilac0112_1 3:2f74791564c9 659 Degree = IrDegree();
lilac0112_1 0:d35efbf4d62e 660
lilac0112_1 3:2f74791564c9 661 if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
lilac0112_1 3:2f74791564c9 662 (AnotherAction[IrNum/6])();
lilac0112_1 2:39135c67083d 663 continue;
lilac0112_1 3:2f74791564c9 664 }
lilac0112_1 3:2f74791564c9 665
lilac0112_1 3:2f74791564c9 666 /*
lilac0112_1 3:2f74791564c9 667 if(IrNum == 12){
lilac0112_1 3:2f74791564c9 668 move(0,0,0,0);
lilac0112_1 3:2f74791564c9 669 wait_ms(10);
lilac0112_1 3:2f74791564c9 670 continue;
lilac0112_1 2:39135c67083d 671 }*/
lilac0112_1 3:2f74791564c9 672
lilac0112_1 3:2f74791564c9 673 nDegree = wrapDegree[Degree/15];
lilac0112_1 2:39135c67083d 674 Power = 20;
lilac0112_1 3:2f74791564c9 675
lilac0112_1 0:d35efbf4d62e 676
lilac0112_1 3:2f74791564c9 677 Degree = nDegree + addDegree;
lilac0112_1 3:2f74791564c9 678 if((Degree <0)||(Degree>=360)){
lilac0112_1 3:2f74791564c9 679 Degree = 0;
lilac0112_1 3:2f74791564c9 680 }
lilac0112_1 3:2f74791564c9 681 fool(&Degree,&Power);
lilac0112_1 3:2f74791564c9 682 move(Power,Power,CompassPID,Degree);
lilac0112_1 2:39135c67083d 683
lilac0112_1 3:2f74791564c9 684 //wait_ms(500);
lilac0112_1 3:2f74791564c9 685 Led[0] =0;
lilac0112_1 2:39135c67083d 686 wait_ms(10);
lilac0112_1 2:39135c67083d 687
lilac0112_1 0:d35efbf4d62e 688 }
lilac0112_1 0:d35efbf4d62e 689
lilac0112_1 3:2f74791564c9 690 while(0){
lilac0112_1 3:2f74791564c9 691 //デモプログラム
lilac0112_1 3:2f74791564c9 692 //Receive();
lilac0112_1 3:2f74791564c9 693 pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
lilac0112_1 3:2f74791564c9 694 //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
lilac0112_1 0:d35efbf4d62e 695
lilac0112_1 3:2f74791564c9 696 //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
lilac0112_1 3:2f74791564c9 697 //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
lilac0112_1 3:2f74791564c9 698 wait(0.1);
lilac0112_1 0:d35efbf4d62e 699 }
lilac0112_1 0:d35efbf4d62e 700
lilac0112_1 2:39135c67083d 701
lilac0112_1 2:39135c67083d 702
lilac0112_1 2:39135c67083d 703
lilac0112_1 0:d35efbf4d62e 704 }