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@5:c8e4d5cdc353, 2015-03-13 (annotated)

Committer:
lilac0112_1
Date:
Fri Mar 13 03:17:44 2015 +0000
Revision:
5:c8e4d5cdc353
Parent:
4:7c488c059498
Child:
6:2c9cb40ada59
???????????

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 4:7c488c059498 67 d
lilac0112_1 0:d35efbf4d62e 68 }
lilac0112_1 3:2f74791564c9 69 */
lilac0112_1 4:7c488c059498 70 void Interval(void){
lilac0112_1 4:7c488c059498 71 WaitFlag=0;
lilac0112_1 4:7c488c059498 72 }
lilac0112_1 3:2f74791564c9 73 void move(int vr,int vl, double vs ,int Degree){
lilac0112_1 0:d35efbf4d62e 74 double pwm[4] = {0};
lilac0112_1 0:d35efbf4d62e 75 uint8_t i = 0;
lilac0112_1 4:7c488c059498 76 double CompassVias=15.0;
lilac0112_1 4:7c488c059498 77 double CompassDif;
lilac0112_1 4:7c488c059498 78
lilac0112_1 4:7c488c059498 79 vr=vr*( 1.0);
lilac0112_1 4:7c488c059498 80 vl=vl*(-1.0);
lilac0112_1 4:7c488c059498 81 Degree = Degree*(-1.0);
lilac0112_1 3:2f74791564c9 82
lilac0112_1 4:7c488c059498 83 CompassDif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
lilac0112_1 5:c8e4d5cdc353 84 if(/*abs(CompassDif)<CompassVias*/1){
lilac0112_1 4:7c488c059498 85
lilac0112_1 4:7c488c059498 86
lilac0112_1 4:7c488c059498 87 CompassVias=15.0;
lilac0112_1 4:7c488c059498 88
lilac0112_1 4:7c488c059498 89 pwm[0] = LastPwm[0] = vr;
lilac0112_1 4:7c488c059498 90 pwm[1] = LastPwm[1] =0;
lilac0112_1 4:7c488c059498 91 pwm[2] = LastPwm[2] =0;
lilac0112_1 4:7c488c059498 92 pwm[3] = LastPwm[3] =vl*(1.0);
lilac0112_1 4:7c488c059498 93
lilac0112_1 4:7c488c059498 94 LastDeg = Degree;
lilac0112_1 4:7c488c059498 95
lilac0112_1 4:7c488c059498 96
lilac0112_1 3:2f74791564c9 97 }
lilac0112_1 3:2f74791564c9 98 else{
lilac0112_1 3:2f74791564c9 99
lilac0112_1 4:7c488c059498 100 CompassVias=10.0;
lilac0112_1 4:7c488c059498 101
lilac0112_1 4:7c488c059498 102 if(CompassDif<0){
lilac0112_1 4:7c488c059498 103 //vs = vs - (20-CompassVias);
lilac0112_1 4:7c488c059498 104 vs = -(15+(abs(CompassDif))*0.1);
lilac0112_1 4:7c488c059498 105 if( abs(CompassDif) >= 100) vs = -(20+(100)*0.1);
lilac0112_1 3:2f74791564c9 106 }
lilac0112_1 3:2f74791564c9 107 else{
lilac0112_1 4:7c488c059498 108 //vs = vs + (20-CompassVias);
lilac0112_1 4:7c488c059498 109 vs = (15+(abs(CompassDif))*0.1);
lilac0112_1 4:7c488c059498 110 if( abs(CompassDif) >= 100) vs = (20+(100)*0.1);
lilac0112_1 3:2f74791564c9 111 }
lilac0112_1 3:2f74791564c9 112
lilac0112_1 3:2f74791564c9 113 pwm[0] = vs;
lilac0112_1 3:2f74791564c9 114 pwm[1] = 0;
lilac0112_1 3:2f74791564c9 115 pwm[2] = 0;
lilac0112_1 3:2f74791564c9 116 pwm[3] = vs*(-1)*(-1);
lilac0112_1 4:7c488c059498 117
lilac0112_1 4:7c488c059498 118 Degree = 0.0;
lilac0112_1 3:2f74791564c9 119 }
lilac0112_1 3:2f74791564c9 120 if(Loop==1){
lilac0112_1 3:2f74791564c9 121 pwm[0] = 0;
lilac0112_1 3:2f74791564c9 122 pwm[1] = 0;
lilac0112_1 3:2f74791564c9 123 pwm[2] = 0;
lilac0112_1 3:2f74791564c9 124 pwm[3] = 0;
lilac0112_1 3:2f74791564c9 125 }
lilac0112_1 4:7c488c059498 126
lilac0112_1 4:7c488c059498 127
lilac0112_1 0:d35efbf4d62e 128 for(i = 0; i < 4; i++){
lilac0112_1 0:d35efbf4d62e 129 if(pwm[i] > 100){
lilac0112_1 0:d35efbf4d62e 130 pwm[i] = 100;
lilac0112_1 0:d35efbf4d62e 131 }else if(pwm[i] < -100){
lilac0112_1 0:d35efbf4d62e 132 pwm[i] = -100;
lilac0112_1 0:d35efbf4d62e 133 }
lilac0112_1 0:d35efbf4d62e 134 speed[i] = pwm[i];
lilac0112_1 0:d35efbf4d62e 135 }
lilac0112_1 3:2f74791564c9 136 if(Degree > 110){
lilac0112_1 3:2f74791564c9 137 Degree = 110;
lilac0112_1 3:2f74791564c9 138 }else if(Degree < -110){
lilac0112_1 3:2f74791564c9 139 Degree = -110;
lilac0112_1 3:2f74791564c9 140 }
lilac0112_1 4:7c488c059498 141
lilac0112_1 4:7c488c059498 142 if(WaitFlag==1) Degree = LastDegree;
lilac0112_1 4:7c488c059498 143
lilac0112_1 4:7c488c059498 144
lilac0112_1 4:7c488c059498 145 SetDegree = LastDegree = Degree;
lilac0112_1 3:2f74791564c9 146 S555.position(SetDegree*11.0/9.0);
lilac0112_1 4:7c488c059498 147
lilac0112_1 4:7c488c059498 148 if(WaitFlag==0){
lilac0112_1 4:7c488c059498 149 WaitFlag = 1;
lilac0112_1 4:7c488c059498 150 STimer.attach(Interval, WAIT_TIME);
lilac0112_1 4:7c488c059498 151 }
lilac0112_1 4:7c488c059498 152
lilac0112_1 4:7c488c059498 153 }
lilac0112_1 4:7c488c059498 154
lilac0112_1 4:7c488c059498 155 void moving(int IrNumber){
lilac0112_1 4:7c488c059498 156 move(vrl[IrNumber],vrl[IrNumber],0,IrServo[IrNumber]);
lilac0112_1 2:39135c67083d 157 }
lilac0112_1 3:2f74791564c9 158
lilac0112_1 3:2f74791564c9 159 uint8_t PingChange(uint8_t LineData){
lilac0112_1 3:2f74791564c9 160 static uint8_t Last_Line;
lilac0112_1 3:2f74791564c9 161 static uint8_t Last_Ping;
lilac0112_1 3:2f74791564c9 162 uint8_t LinePing = 0;
lilac0112_1 3:2f74791564c9 163
lilac0112_1 3:2f74791564c9 164 if(!LineData){
lilac0112_1 3:2f74791564c9 165 return 0;
lilac0112_1 3:2f74791564c9 166 }
lilac0112_1 3:2f74791564c9 167
lilac0112_1 4:7c488c059498 168 if(PingData[0] <20) LinePing = LinePing + 8;
lilac0112_1 4:7c488c059498 169 if(PingData[1] <20) LinePing = LinePing + 2;
lilac0112_1 4:7c488c059498 170 if(PingData[2] <20) LinePing = LinePing + 4;
lilac0112_1 4:7c488c059498 171 if(PingData[3] <20) LinePing = LinePing + 1;
lilac0112_1 3:2f74791564c9 172
lilac0112_1 3:2f74791564c9 173
lilac0112_1 3:2f74791564c9 174 if(LinePing&0x01){
lilac0112_1 3:2f74791564c9 175 if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
lilac0112_1 3:2f74791564c9 176 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 177 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 178 return 1;
lilac0112_1 3:2f74791564c9 179 }
lilac0112_1 3:2f74791564c9 180 }
lilac0112_1 3:2f74791564c9 181 if(LinePing&0x02){
lilac0112_1 3:2f74791564c9 182 if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
lilac0112_1 3:2f74791564c9 183 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 184 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 185 return 2;
lilac0112_1 3:2f74791564c9 186 }
lilac0112_1 3:2f74791564c9 187 }
lilac0112_1 3:2f74791564c9 188 if(LinePing&0x04){
lilac0112_1 3:2f74791564c9 189 if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
lilac0112_1 3:2f74791564c9 190 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 191 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 192 return 4;
lilac0112_1 3:2f74791564c9 193 }
lilac0112_1 3:2f74791564c9 194 }
lilac0112_1 3:2f74791564c9 195 if(LinePing&0x08){
lilac0112_1 3:2f74791564c9 196 if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
lilac0112_1 3:2f74791564c9 197 Last_Ping = LinePing;
lilac0112_1 3:2f74791564c9 198 Last_Line = LineData;
lilac0112_1 3:2f74791564c9 199 return 8;
lilac0112_1 3:2f74791564c9 200 }
lilac0112_1 3:2f74791564c9 201 }
lilac0112_1 3:2f74791564c9 202 Last_Ping = 0;
lilac0112_1 3:2f74791564c9 203 Last_Line = 0;
lilac0112_1 3:2f74791564c9 204 return 0;
lilac0112_1 3:2f74791564c9 205
lilac0112_1 3:2f74791564c9 206 }
lilac0112_1 3:2f74791564c9 207
lilac0112_1 3:2f74791564c9 208 void fool (int *Degree, int *Power){
lilac0112_1 3:2f74791564c9 209 static int Last_Degree = 0;
lilac0112_1 2:39135c67083d 210 static int Last_Vector = 1;
lilac0112_1 3:2f74791564c9 211 int degree = *Degree;
lilac0112_1 2:39135c67083d 212 int Temp;
lilac0112_1 3:2f74791564c9 213
lilac0112_1 3:2f74791564c9 214
lilac0112_1 3:2f74791564c9 215 if((*Degree <0)||(*Degree >=360)){
lilac0112_1 3:2f74791564c9 216 *Degree = 0;
lilac0112_1 3:2f74791564c9 217 Last_Degree = 0;
lilac0112_1 3:2f74791564c9 218 Last_Vector = 1;
lilac0112_1 3:2f74791564c9 219 return ;
lilac0112_1 3:2f74791564c9 220 }
lilac0112_1 3:2f74791564c9 221
lilac0112_1 3:2f74791564c9 222 Temp = Last_Degree % 180;
lilac0112_1 3:2f74791564c9 223 if((Temp==90)){
lilac0112_1 3:2f74791564c9 224 Temp = *Degree % 180;
lilac0112_1 3:2f74791564c9 225 if((Temp==90)){
lilac0112_1 3:2f74791564c9 226 Temp = abs(*Degree - Last_Degree);
lilac0112_1 2:39135c67083d 227 if(Temp>160){
lilac0112_1 2:39135c67083d 228 Last_Vector = -1 * Last_Vector;//正転逆転切り替え
lilac0112_1 3:2f74791564c9 229 if(*Degree/180){
lilac0112_1 3:2f74791564c9 230 *Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180);
lilac0112_1 2:39135c67083d 231 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 232 }else{
lilac0112_1 3:2f74791564c9 233 *Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree);
lilac0112_1 2:39135c67083d 234 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 235 }
lilac0112_1 3:2f74791564c9 236 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 237 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 238 *Degree = 0;
lilac0112_1 3:2f74791564c9 239 }
lilac0112_1 2:39135c67083d 240 return;
lilac0112_1 2:39135c67083d 241 }else if((Last_Vector+2) == 1){
lilac0112_1 2:39135c67083d 242 /*逆転のまま角度拡張*/
lilac0112_1 3:2f74791564c9 243 if(*Degree/180){
lilac0112_1 3:2f74791564c9 244 *Degree = -360 + *Degree ;
lilac0112_1 3:2f74791564c9 245
lilac0112_1 2:39135c67083d 246 }
lilac0112_1 3:2f74791564c9 247 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 248 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 249 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 250 *Degree = 0;
lilac0112_1 3:2f74791564c9 251
lilac0112_1 3:2f74791564c9 252 }
lilac0112_1 2:39135c67083d 253 return;
lilac0112_1 2:39135c67083d 254
lilac0112_1 2:39135c67083d 255 }else if((Last_Vector+2) == 3){
lilac0112_1 2:39135c67083d 256 /*正転のまま*/
lilac0112_1 3:2f74791564c9 257 if(*Degree/180){
lilac0112_1 3:2f74791564c9 258 *Degree = -360 + *Degree ;
lilac0112_1 2:39135c67083d 259 }
lilac0112_1 3:2f74791564c9 260 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 261 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 262 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 263 *Degree = 0;
lilac0112_1 3:2f74791564c9 264 }
lilac0112_1 2:39135c67083d 265 return;
lilac0112_1 2:39135c67083d 266 }
lilac0112_1 2:39135c67083d 267 }
lilac0112_1 2:39135c67083d 268 }
lilac0112_1 2:39135c67083d 269 /*通常動作*/
lilac0112_1 3:2f74791564c9 270 if(*Degree == 0){
lilac0112_1 3:2f74791564c9 271 Last_Vector = DegreeToVector[0];
lilac0112_1 3:2f74791564c9 272 *Degree = Angle[*Degree%180];
lilac0112_1 3:2f74791564c9 273 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 274 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 275 return ;
lilac0112_1 3:2f74791564c9 276 }
lilac0112_1 4:7c488c059498 277 if(*Degree == 270){
lilac0112_1 4:7c488c059498 278 Last_Vector = DegreeToVector[0];
lilac0112_1 4:7c488c059498 279 *Degree = -90;
lilac0112_1 4:7c488c059498 280 *Power = *Power * Last_Vector;
lilac0112_1 4:7c488c059498 281 Last_Degree = degree;
lilac0112_1 4:7c488c059498 282 return ;
lilac0112_1 4:7c488c059498 283 }
lilac0112_1 4:7c488c059498 284 Last_Vector = DegreeToVector[(*Degree)/90];
lilac0112_1 3:2f74791564c9 285 *Degree = Angle[*Degree%180];
lilac0112_1 2:39135c67083d 286 *Power = *Power * Last_Vector;
lilac0112_1 3:2f74791564c9 287 Last_Degree = degree;
lilac0112_1 3:2f74791564c9 288 if((*Degree <= -120)||(*Degree >=120)){
lilac0112_1 3:2f74791564c9 289 *Degree = 0;
lilac0112_1 3:2f74791564c9 290 }
lilac0112_1 2:39135c67083d 291
lilac0112_1 2:39135c67083d 292 }
lilac0112_1 3:2f74791564c9 293 int IrDegree(){
lilac0112_1 2:39135c67083d 294 /*irの1位の値，2位の場所からirの360へ持っていく*/
lilac0112_1 2:39135c67083d 295
lilac0112_1 3:2f74791564c9 296 uint8_t IrF ,IrS;
lilac0112_1 3:2f74791564c9 297 unsigned int irdegree = 0;
lilac0112_1 3:2f74791564c9 298 if(IrNum >=12){
lilac0112_1 3:2f74791564c9 299 return 0;
lilac0112_1 3:2f74791564c9 300 }
lilac0112_1 3:2f74791564c9 301 IrF = IrData[0]/12;
lilac0112_1 3:2f74791564c9 302 IrS = IrData[0]%12;
lilac0112_1 3:2f74791564c9 303
lilac0112_1 3:2f74791564c9 304 if(IrF == 0 ){
lilac0112_1 3:2f74791564c9 305 if(IrS == 11){
lilac0112_1 3:2f74791564c9 306 irdegree = 15;
lilac0112_1 2:39135c67083d 307
lilac0112_1 3:2f74791564c9 308 }else if(IrS == 1){
lilac0112_1 3:2f74791564c9 309 irdegree = 345;
lilac0112_1 3:2f74791564c9 310 }
lilac0112_1 3:2f74791564c9 311 return irdegree;
lilac0112_1 3:2f74791564c9 312 }
lilac0112_1 3:2f74791564c9 313 irdegree = 360 - IrF*30;
lilac0112_1 3:2f74791564c9 314 if(IrS&&(abs(IrF-IrS) == 1)){
lilac0112_1 3:2f74791564c9 315 irdegree = irdegree - (IrF - IrS)*15;
lilac0112_1 2:39135c67083d 316
lilac0112_1 2:39135c67083d 317 }
lilac0112_1 3:2f74791564c9 318 if(irdegree>=360){
lilac0112_1 3:2f74791564c9 319 return 0;
lilac0112_1 3:2f74791564c9 320 }
lilac0112_1 3:2f74791564c9 321
lilac0112_1 3:2f74791564c9 322 return irdegree;
lilac0112_1 0:d35efbf4d62e 323 }
lilac0112_1 0:d35efbf4d62e 324
lilac0112_1 2:39135c67083d 325
lilac0112_1 3:2f74791564c9 326 void IrFrontAction()//ball 12 or 0 o-clock
lilac0112_1 1:e3248f278663 327 {
lilac0112_1 3:2f74791564c9 328 if(IrData[1]>70){
lilac0112_1 3:2f74791564c9 329 if(abs(CompassPID)>10){
lilac0112_1 3:2f74791564c9 330 move(0,0,CompassPID,0);
lilac0112_1 3:2f74791564c9 331 return;
lilac0112_1 3:2f74791564c9 332 }
lilac0112_1 3:2f74791564c9 333 move(30,30,CompassPID,0);
lilac0112_1 3:2f74791564c9 334 return;
lilac0112_1 3:2f74791564c9 335 }
lilac0112_1 3:2f74791564c9 336 if(IrData[1]>60){
lilac0112_1 3:2f74791564c9 337 move(25,25,CompassPID,0);
lilac0112_1 3:2f74791564c9 338 return;
lilac0112_1 3:2f74791564c9 339 }
lilac0112_1 3:2f74791564c9 340 /*IrData[1]>500*/
lilac0112_1 3:2f74791564c9 341
lilac0112_1 3:2f74791564c9 342 if(PingData[0]<50){
lilac0112_1 3:2f74791564c9 343 if((PingData[1]<60)&&(PingData[1]>40)){
lilac0112_1 3:2f74791564c9 344 /*右側に居る*/
lilac0112_1 3:2f74791564c9 345 move(25,40,0,10);
lilac0112_1 3:2f74791564c9 346 wait_ms(100);
lilac0112_1 3:2f74791564c9 347 move(30,30,0,0);
lilac0112_1 3:2f74791564c9 348 return;
lilac0112_1 3:2f74791564c9 349 }
lilac0112_1 3:2f74791564c9 350 if((PingData[1]<60)&&(PingData[1]>40)){
lilac0112_1 3:2f74791564c9 351 /*左側に居る*/
lilac0112_1 3:2f74791564c9 352 move(40,25,0,-10);
lilac0112_1 3:2f74791564c9 353 wait_ms(100);
lilac0112_1 3:2f74791564c9 354 move(30,30,0,0);
lilac0112_1 3:2f74791564c9 355 return;
lilac0112_1 3:2f74791564c9 356 }
lilac0112_1 3:2f74791564c9 357 /*それ以外*/
lilac0112_1 3:2f74791564c9 358 move(10,10,CompassPID,0);
lilac0112_1 3:2f74791564c9 359 //Receive();
lilac0112_1 3:2f74791564c9 360 if(!(IrNum == 0)) return;
lilac0112_1 3:2f74791564c9 361 move(40,40,CompassPID,0);
lilac0112_1 3:2f74791564c9 362 return ;
lilac0112_1 3:2f74791564c9 363 }
lilac0112_1 3:2f74791564c9 364 move(20,20,CompassPID,0);
lilac0112_1 3:2f74791564c9 365
lilac0112_1 3:2f74791564c9 366
lilac0112_1 1:e3248f278663 367
lilac0112_1 1:e3248f278663 368 }
lilac0112_1 2:39135c67083d 369
lilac0112_1 3:2f74791564c9 370 void IrBackAction()//ball found six o-clock
lilac0112_1 0:d35efbf4d62e 371 {
lilac0112_1 2:39135c67083d 372 /***
lilac0112_1 2:39135c67083d 373 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
lilac0112_1 2:39135c67083d 374 *
lilac0112_1 2:39135c67083d 375 **/
lilac0112_1 3:2f74791564c9 376 if(PingData[1]>PingData[3]){
lilac0112_1 2:39135c67083d 377 /*右が大きい*/
lilac0112_1 3:2f74791564c9 378 if(IrData[1]>70){
lilac0112_1 3:2f74791564c9 379 move(-20,-20,CompassPID,45);
lilac0112_1 3:2f74791564c9 380 return;
lilac0112_1 3:2f74791564c9 381 }
lilac0112_1 3:2f74791564c9 382 if(IrData[1]>60){
lilac0112_1 3:2f74791564c9 383 move(-20,-20,CompassPID,60);
lilac0112_1 3:2f74791564c9 384 return;
lilac0112_1 3:2f74791564c9 385 }
lilac0112_1 3:2f74791564c9 386 move(-20,-20,CompassPID,80);
lilac0112_1 0:d35efbf4d62e 387 return;
lilac0112_1 0:d35efbf4d62e 388 }
lilac0112_1 2:39135c67083d 389 /*左が大きい*/
lilac0112_1 0:d35efbf4d62e 390
lilac0112_1 3:2f74791564c9 391 if(IrData[1]>70){
lilac0112_1 3:2f74791564c9 392 move(-20,-20,CompassPID,-45);
lilac0112_1 3:2f74791564c9 393 return;
lilac0112_1 0:d35efbf4d62e 394 }
lilac0112_1 3:2f74791564c9 395 if(IrData[1]>60){
lilac0112_1 3:2f74791564c9 396 move(-20,-20,CompassPID,-60);
lilac0112_1 3:2f74791564c9 397 return;
lilac0112_1 3:2f74791564c9 398 }
lilac0112_1 3:2f74791564c9 399 move(-20,-20,CompassPID,-80);
lilac0112_1 3:2f74791564c9 400 return;
lilac0112_1 2:39135c67083d 401
lilac0112_1 3:2f74791564c9 402 }
lilac0112_1 3:2f74791564c9 403
lilac0112_1 3:2f74791564c9 404 void GoHome()//Ball is not found.
lilac0112_1 3:2f74791564c9 405 {
lilac0112_1 4:7c488c059498 406 static bool State[20]={0};
lilac0112_1 4:7c488c059498 407 /*
lilac0112_1 4:7c488c059498 408 State
lilac0112_1 4:7c488c059498 409 0
lilac0112_1 4:7c488c059498 410 1
lilac0112_1 4:7c488c059498 411 2
lilac0112_1 4:7c488c059498 412 3
lilac0112_1 4:7c488c059498 413 4
lilac0112_1 4:7c488c059498 414 5
lilac0112_1 4:7c488c059498 415 6
lilac0112_1 4:7c488c059498 416 */
lilac0112_1 4:7c488c059498 417
lilac0112_1 4:7c488c059498 418
lilac0112_1 4:7c488c059498 419 for(int i=0; i<20; i++) State[i]=0;
lilac0112_1 4:7c488c059498 420
lilac0112_1 4:7c488c059498 421
lilac0112_1 4:7c488c059498 422
lilac0112_1 4:7c488c059498 423 if(PingData[2] >=60) State[0]=1;
lilac0112_1 4:7c488c059498 424
lilac0112_1 4:7c488c059498 425 if(PingData[2] <=40) State[1]=1;
lilac0112_1 4:7c488c059498 426
lilac0112_1 4:7c488c059498 427 if(PingData[2] >=90-10) State[5]=1;
lilac0112_1 4:7c488c059498 428
lilac0112_1 4:7c488c059498 429 if(PingData[2] <=40+10) State[6]=1;
lilac0112_1 4:7c488c059498 430
lilac0112_1 5:c8e4d5cdc353 431 if(PingData[2] >=30) State[7]=1;
lilac0112_1 4:7c488c059498 432
lilac0112_1 5:c8e4d5cdc353 433 if(PingData[2] <=10) State[8]=1;
lilac0112_1 4:7c488c059498 434
lilac0112_1 5:c8e4d5cdc353 435 if((PingData[2] >=15)&&(PingData[2] <=25)) State[9]=1;
lilac0112_1 4:7c488c059498 436
lilac0112_1 5:c8e4d5cdc353 437 if(PingData[2] <15) State[10]=1;
lilac0112_1 5:c8e4d5cdc353 438
lilac0112_1 5:c8e4d5cdc353 439 if(PingData[2] >25) State[11]=1;
lilac0112_1 4:7c488c059498 440
lilac0112_1 4:7c488c059498 441
lilac0112_1 4:7c488c059498 442
lilac0112_1 4:7c488c059498 443 if(PingData[0]>PingData[1]) State[2]=1;
lilac0112_1 4:7c488c059498 444
lilac0112_1 4:7c488c059498 445 if(
lilac0112_1 4:7c488c059498 446 (PingData[0]+PingData[1])
lilac0112_1 4:7c488c059498 447 >= 120
lilac0112_1 4:7c488c059498 448 ) State[3]=1;
lilac0112_1 4:7c488c059498 449
lilac0112_1 4:7c488c059498 450 if(
lilac0112_1 4:7c488c059498 451 (abs( int( int(PingData[0]) - int(PingData[1]) ) ) )
lilac0112_1 4:7c488c059498 452 <= 10
lilac0112_1 4:7c488c059498 453 ) State[4]=1;
lilac0112_1 4:7c488c059498 454
lilac0112_1 4:7c488c059498 455
lilac0112_1 4:7c488c059498 456 /*if(State[0]){
lilac0112_1 4:7c488c059498 457
lilac0112_1 4:7c488c059498 458 move(vrl[6],vrl[6],CompassPID,IrServo[6]);
lilac0112_1 4:7c488c059498 459 return ;
lilac0112_1 4:7c488c059498 460 }*/
lilac0112_1 4:7c488c059498 461
lilac0112_1 4:7c488c059498 462 if(State[4]){
lilac0112_1 5:c8e4d5cdc353 463
lilac0112_1 5:c8e4d5cdc353 464 if(State[9]){
lilac0112_1 5:c8e4d5cdc353 465 moving(12);
lilac0112_1 5:c8e4d5cdc353 466 return ;
lilac0112_1 5:c8e4d5cdc353 467 }
lilac0112_1 5:c8e4d5cdc353 468 if(State[10]){
lilac0112_1 5:c8e4d5cdc353 469 moving(0);
lilac0112_1 5:c8e4d5cdc353 470 return ;
lilac0112_1 5:c8e4d5cdc353 471 }
lilac0112_1 5:c8e4d5cdc353 472 if(State[11]){
lilac0112_1 5:c8e4d5cdc353 473 moving(6);
lilac0112_1 5:c8e4d5cdc353 474 return ;
lilac0112_1 4:7c488c059498 475 }
lilac0112_1 4:7c488c059498 476
lilac0112_1 5:c8e4d5cdc353 477
lilac0112_1 4:7c488c059498 478
lilac0112_1 4:7c488c059498 479 }else{
lilac0112_1 4:7c488c059498 480 if(State[2]){
lilac0112_1 5:c8e4d5cdc353 481 moving(9);
lilac0112_1 4:7c488c059498 482 return ;
lilac0112_1 4:7c488c059498 483 }
lilac0112_1 4:7c488c059498 484 else{
lilac0112_1 5:c8e4d5cdc353 485 moving(3);
lilac0112_1 4:7c488c059498 486 return ;
lilac0112_1 4:7c488c059498 487 }
lilac0112_1 4:7c488c059498 488 }
lilac0112_1 4:7c488c059498 489
lilac0112_1 2:39135c67083d 490 }
lilac0112_1 0:d35efbf4d62e 491
lilac0112_1 0:d35efbf4d62e 492
lilac0112_1 2:39135c67083d 493 void PidUpdate()
lilac0112_1 2:39135c67083d 494 {
lilac0112_1 0:d35efbf4d62e 495
lilac0112_1 3:2f74791564c9 496 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
lilac0112_1 3:2f74791564c9 497 InputPID = ((Compass/10) + 540 - CompassDef) % 360;
lilac0112_1 3:2f74791564c9 498
lilac0112_1 3:2f74791564c9 499 pid.setProcessValue(InputPID);
lilac0112_1 3:2f74791564c9 500 CompassPID = (pid.compute());
lilac0112_1 0:d35efbf4d62e 501 }
lilac0112_1 2:39135c67083d 502
lilac0112_1 0:d35efbf4d62e 503 uint8_t SwRead(){
lilac0112_1 0:d35efbf4d62e 504 /******
lilac0112_1 2:39135c67083d 505 *retrun : sw_state
lilac0112_1 2:39135c67083d 506 *StartS = 0x01;
lilac0112_1 2:39135c67083d 507 *Debug2 = 0x02;
lilac0112_1 0:d35efbf4d62e 508 *Debug1 = 0x04;
lilac0112_1 2:39135c67083d 509 *Debug3 = 0x06;
lilac0112_1 2:39135c67083d 510 *Kicker = 0x08;
lilac0112_1 2:39135c67083d 511 *Calibration = 0x10;
lilac0112_1 2:39135c67083d 512
lilac0112_1 0:d35efbf4d62e 513 *
lilac0112_1 0:d35efbf4d62e 514 *****/
lilac0112_1 0:d35efbf4d62e 515 uint8_t i,temp,temp2;
lilac0112_1 2:39135c67083d 516 temp = ~Sw - 224;
lilac0112_1 2:39135c67083d 517 if(!(temp == Kicker
lilac0112_1 0:d35efbf4d62e 518 ||temp == Debug1
lilac0112_1 0:d35efbf4d62e 519 ||temp == Debug2
lilac0112_1 2:39135c67083d 520 ||temp == Debug3
lilac0112_1 3:2f74791564c9 521 ||temp == Debug12
lilac0112_1 3:2f74791564c9 522 ||temp == Debug23
lilac0112_1 2:39135c67083d 523 ||temp == StartS)) return 0;/*スイッチが押されていない*/
lilac0112_1 2:39135c67083d 524 if(!(temp == 0x00)){
lilac0112_1 0:d35efbf4d62e 525 for(i = 0; i < 50; i++);
lilac0112_1 2:39135c67083d 526 temp2 = ~Sw - 224;
lilac0112_1 0:d35efbf4d62e 527 if(temp == temp2){
lilac0112_1 0:d35efbf4d62e 528 return temp;
lilac0112_1 0:d35efbf4d62e 529 }
lilac0112_1 0:d35efbf4d62e 530 }
lilac0112_1 0:d35efbf4d62e 531 return 0;
lilac0112_1 0:d35efbf4d62e 532 }
lilac0112_1 0:d35efbf4d62e 533
lilac0112_1 0:d35efbf4d62e 534 //通信（モータ用）
lilac0112_1 0:d35efbf4d62e 535 void tx_motor(){
lilac0112_1 2:39135c67083d 536
lilac0112_1 3:2f74791564c9 537 /*
lilac0112_1 3:2f74791564c9 538 speed[0]=0;
lilac0112_1 3:2f74791564c9 539 speed[1]=0;
lilac0112_1 3:2f74791564c9 540 speed[2]=0;
lilac0112_1 3:2f74791564c9 541 speed[3]=0;
lilac0112_1 3:2f74791564c9 542 */
lilac0112_1 0:d35efbf4d62e 543 array(speed[0],speed[1],speed[3],speed[2]);
lilac0112_1 0:d35efbf4d62e 544 Motor.printf("%s",StringFIN.c_str());
lilac0112_1 0:d35efbf4d62e 545 }
lilac0112_1 0:d35efbf4d62e 546
lilac0112_1 2:39135c67083d 547 void SetUp(){
lilac0112_1 0:d35efbf4d62e 548 /*初期化*/
lilac0112_1 3:2f74791564c9 549
lilac0112_1 3:2f74791564c9 550
lilac0112_1 0:d35efbf4d62e 551 Motor.baud(115200); //ボーレート設定
lilac0112_1 0:d35efbf4d62e 552 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:d35efbf4d62e 553 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 2:39135c67083d 554
lilac0112_1 3:2f74791564c9 555 Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
lilac0112_1 3:2f74791564c9 556
lilac0112_1 3:2f74791564c9 557 S555.calibrate(0.0006, 120.0);
lilac0112_1 3:2f74791564c9 558 //S555.position(0.0); //初期位置にセット
lilac0112_1 2:39135c67083d 559 move(0,0,0,0);//停止
lilac0112_1 0:d35efbf4d62e 560
lilac0112_1 2:39135c67083d 561 Kick = 0;
lilac0112_1 2:39135c67083d 562 Sw.mode(PullUp);
lilac0112_1 0:d35efbf4d62e 563
lilac0112_1 2:39135c67083d 564 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 2:39135c67083d 565 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 2:39135c67083d 566 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 2:39135c67083d 567 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 3:2f74791564c9 568 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:d35efbf4d62e 569
lilac0112_1 2:39135c67083d 570 for(int i=0; i<15; i++){
lilac0112_1 3:2f74791564c9 571 CompassDef = (Compass / 10);
lilac0112_1 3:2f74791564c9 572 wait_ms(10);
lilac0112_1 2:39135c67083d 573 }
lilac0112_1 4:7c488c059498 574 //pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 3:2f74791564c9 575
lilac0112_1 0:d35efbf4d62e 576
lilac0112_1 0:d35efbf4d62e 577 }
lilac0112_1 0:d35efbf4d62e 578 void StartLoop(){
lilac0112_1 2:39135c67083d 579
lilac0112_1 0:d35efbf4d62e 580 uint8_t State = 0;
lilac0112_1 2:39135c67083d 581 uint8_t LineData = 0;
lilac0112_1 0:d35efbf4d62e 582 while(1){
lilac0112_1 2:39135c67083d 583 Led[0] = Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 2:39135c67083d 584 //Lcd.cls();
lilac0112_1 3:2f74791564c9 585
lilac0112_1 0:d35efbf4d62e 586 State = SwRead();
lilac0112_1 0:d35efbf4d62e 587 if(State == 0) continue;
lilac0112_1 0:d35efbf4d62e 588
lilac0112_1 2:39135c67083d 589 if(State == StartS){
lilac0112_1 2:39135c67083d 590 /*loop end & start*/
lilac0112_1 3:2f74791564c9 591 return;
lilac0112_1 0:d35efbf4d62e 592 }
lilac0112_1 2:39135c67083d 593
lilac0112_1 2:39135c67083d 594 if(State == Debug1){
lilac0112_1 2:39135c67083d 595 while((State == Debug1)){
lilac0112_1 2:39135c67083d 596 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 2:39135c67083d 597 Lcd.printf("%d\n",LineData);
lilac0112_1 2:39135c67083d 598
lilac0112_1 2:39135c67083d 599 wait_ms(100);
lilac0112_1 2:39135c67083d 600 State = SwRead();
lilac0112_1 2:39135c67083d 601 }
lilac0112_1 2:39135c67083d 602 Lcd.cls();
lilac0112_1 0:d35efbf4d62e 603 continue;
lilac0112_1 0:d35efbf4d62e 604
lilac0112_1 0:d35efbf4d62e 605 }
lilac0112_1 0:d35efbf4d62e 606 if(State == Debug2){
lilac0112_1 2:39135c67083d 607 while((State == Debug2)){
lilac0112_1 3:2f74791564c9 608 //Receive();
lilac0112_1 2:39135c67083d 609 Lcd.printf("%d\n",IrNum);
lilac0112_1 3:2f74791564c9 610 //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 611 wait_ms(100);
lilac0112_1 2:39135c67083d 612 State = SwRead();
lilac0112_1 2:39135c67083d 613 }
lilac0112_1 2:39135c67083d 614 Lcd.cls();
lilac0112_1 2:39135c67083d 615 continue;
lilac0112_1 0:d35efbf4d62e 616 }
lilac0112_1 2:39135c67083d 617
lilac0112_1 2:39135c67083d 618 if(State == Debug3){
lilac0112_1 2:39135c67083d 619 while((State == Debug3)){
lilac0112_1 2:39135c67083d 620 Lcd.printf("%d\n",((Compass / 10) + 540 - CompassDef) % 360);
lilac0112_1 3:2f74791564c9 621 wait_ms(100);
lilac0112_1 3:2f74791564c9 622 State = SwRead();
lilac0112_1 3:2f74791564c9 623 }
lilac0112_1 3:2f74791564c9 624 Lcd.cls();
lilac0112_1 3:2f74791564c9 625 continue;
lilac0112_1 3:2f74791564c9 626 }
lilac0112_1 3:2f74791564c9 627 if(State == Debug12){
lilac0112_1 3:2f74791564c9 628 while((State == Debug12)){
lilac0112_1 3:2f74791564c9 629 Lcd.printf("%d\n", PingData[0]);//Left
lilac0112_1 3:2f74791564c9 630 Lcd.printf("%d\n", PingData[1]);//Right
lilac0112_1 3:2f74791564c9 631 wait_ms(100);
lilac0112_1 3:2f74791564c9 632 State = SwRead();
lilac0112_1 3:2f74791564c9 633 }
lilac0112_1 3:2f74791564c9 634 Lcd.cls();
lilac0112_1 3:2f74791564c9 635 continue;
lilac0112_1 3:2f74791564c9 636 }
lilac0112_1 3:2f74791564c9 637 if(State == Debug23){
lilac0112_1 3:2f74791564c9 638 while((State == Debug23)){
lilac0112_1 3:2f74791564c9 639 Lcd.printf("%d\n", PingData[2]);//Back
lilac0112_1 3:2f74791564c9 640 Lcd.printf("%d\n", PingData[3]);//Front
lilac0112_1 2:39135c67083d 641 wait_ms(100);
lilac0112_1 2:39135c67083d 642 State = SwRead();
lilac0112_1 2:39135c67083d 643 }
lilac0112_1 2:39135c67083d 644 Lcd.cls();
lilac0112_1 2:39135c67083d 645 continue;
lilac0112_1 0:d35efbf4d62e 646 }
lilac0112_1 0:d35efbf4d62e 647
lilac0112_1 2:39135c67083d 648 if(State == Kicker){
lilac0112_1 3:2f74791564c9 649 while((State == Kicker)){
lilac0112_1 3:2f74791564c9 650 Lcd.printf("%f\n",CompassPID);
lilac0112_1 3:2f74791564c9 651 wait_ms(100);
lilac0112_1 3:2f74791564c9 652 State = SwRead();
lilac0112_1 3:2f74791564c9 653 }
lilac0112_1 3:2f74791564c9 654 Lcd.cls();
lilac0112_1 3:2f74791564c9 655 continue;
lilac0112_1 3:2f74791564c9 656
lilac0112_1 3:2f74791564c9 657 /*Led[0] = Led[1] = Led[2] = 0;
lilac0112_1 3:2f74791564c9 658 Kick = 1;
lilac0112_1 3:2f74791564c9 659 wait_ms(500);
lilac0112_1 3:2f74791564c9 660 Kick = 0;
lilac0112_1 2:39135c67083d 661 while((State == Kicker)){
lilac0112_1 2:39135c67083d 662 wait_ms(100);
lilac0112_1 2:39135c67083d 663 State = SwRead();
lilac0112_1 2:39135c67083d 664 }
lilac0112_1 3:2f74791564c9 665 continue;*/
lilac0112_1 2:39135c67083d 666 }
lilac0112_1 3:2f74791564c9 667 }
lilac0112_1 0:d35efbf4d62e 668
lilac0112_1 0:d35efbf4d62e 669 }
lilac0112_1 4:7c488c059498 670
lilac0112_1 4:7c488c059498 671 uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
lilac0112_1 4:7c488c059498 672 return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one;
lilac0112_1 4:7c488c059498 673 }
lilac0112_1 4:7c488c059498 674
lilac0112_1 0:d35efbf4d62e 675 int main() {
lilac0112_1 0:d35efbf4d62e 676
lilac0112_1 3:2f74791564c9 677 //uint8_t IrNumOld = 0;//過去値
lilac0112_1 0:d35efbf4d62e 678 /*Line*/
lilac0112_1 2:39135c67083d 679 uint8_t LineData = 0;
lilac0112_1 4:7c488c059498 680 //uint8_t LinePing = 0;
lilac0112_1 3:2f74791564c9 681
lilac0112_1 2:39135c67083d 682
lilac0112_1 0:d35efbf4d62e 683 /*State */
lilac0112_1 4:7c488c059498 684 /*uint8_t LineIr = 0;
lilac0112_1 3:2f74791564c9 685 uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06,
lilac0112_1 3:2f74791564c9 686
lilac0112_1 3:2f74791564c9 687 0x04,0x04,0x0B,0x08,0x08,0x09,0x00};
lilac0112_1 4:7c488c059498 688 */
lilac0112_1 2:39135c67083d 689 /*行動設定*/
lilac0112_1 4:7c488c059498 690 //int Power = 0;
lilac0112_1 4:7c488c059498 691 //int Degree = 0;
lilac0112_1 2:39135c67083d 692
lilac0112_1 2:39135c67083d 693 /*楽しい変数達*/
lilac0112_1 4:7c488c059498 694 //int nDegree =0;//基礎角
lilac0112_1 4:7c488c059498 695 //int addDegree = 0;//追加角
lilac0112_1 0:d35efbf4d62e 696
lilac0112_1 4:7c488c059498 697 /**/
lilac0112_1 5:c8e4d5cdc353 698 int Gap=2;
lilac0112_1 4:7c488c059498 699 bool LineState[10]={0};
lilac0112_1 0:d35efbf4d62e 700
lilac0112_1 0:d35efbf4d62e 701
lilac0112_1 0:d35efbf4d62e 702 /*関数ポインタ*/
lilac0112_1 3:2f74791564c9 703
lilac0112_1 3:2f74791564c9 704
lilac0112_1 3:2f74791564c9 705 //void (*AnotherAction[3])(uint8_t [],double);
lilac0112_1 4:7c488c059498 706 /*void (*AnotherAction[3])();
lilac0112_1 2:39135c67083d 707 AnotherAction[0] = IrFrontAction;
lilac0112_1 2:39135c67083d 708 AnotherAction[1] = IrBackAction;
lilac0112_1 4:7c488c059498 709 AnotherAction[2] = GoHome;*/
lilac0112_1 2:39135c67083d 710
lilac0112_1 3:2f74791564c9 711
lilac0112_1 2:39135c67083d 712 SetUp();/*set up routine*/
lilac0112_1 3:2f74791564c9 713
lilac0112_1 2:39135c67083d 714 StartLoop(); /*loop strat, switch push end*/
lilac0112_1 3:2f74791564c9 715 Led[0] = Led[1] = Led[2] = Led[3] = 0;
lilac0112_1 3:2f74791564c9 716 wait_ms(100);
lilac0112_1 2:39135c67083d 717
lilac0112_1 3:2f74791564c9 718 Loop = 0;
lilac0112_1 2:39135c67083d 719
lilac0112_1 3:2f74791564c9 720 while(1){
lilac0112_1 3:2f74791564c9 721
lilac0112_1 5:c8e4d5cdc353 722 /*if(
lilac0112_1 4:7c488c059498 723 (
lilac0112_1 4:7c488c059498 724 (
lilac0112_1 4:7c488c059498 725 (
lilac0112_1 4:7c488c059498 726 (Compass / 10) + 540 - CompassDef
lilac0112_1 4:7c488c059498 727 ) % 360
lilac0112_1 4:7c488c059498 728 ) - REFERENCE
lilac0112_1 5:c8e4d5cdc353 729 ) >=15
lilac0112_1 4:7c488c059498 730 ){
lilac0112_1 3:2f74791564c9 731
lilac0112_1 4:7c488c059498 732 move(0,0,0,0);
lilac0112_1 2:39135c67083d 733 continue;
lilac0112_1 5:c8e4d5cdc353 734 }*/
lilac0112_1 3:2f74791564c9 735
lilac0112_1 4:7c488c059498 736 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 4:7c488c059498 737
lilac0112_1 4:7c488c059498 738 for(int i=0; i<10; i++) LineState[i]=0;
lilac0112_1 3:2f74791564c9 739
lilac0112_1 4:7c488c059498 740 if(LineData!=0){
lilac0112_1 4:7c488c059498 741
lilac0112_1 4:7c488c059498 742 if(PingData[0]>PingData[1]) LineState[0]=1;
lilac0112_1 5:c8e4d5cdc353 743 if(PingData[2]<=30) LineState[1]=1;
lilac0112_1 4:7c488c059498 744
lilac0112_1 4:7c488c059498 745 switch(LinkBit(0,0,0,0,0,0,LineState[1],LineState[0])&0x03){
lilac0112_1 4:7c488c059498 746
lilac0112_1 4:7c488c059498 747 case 0:
lilac0112_1 4:7c488c059498 748 moving(3);
lilac0112_1 4:7c488c059498 749 break;
lilac0112_1 4:7c488c059498 750 case 1:
lilac0112_1 4:7c488c059498 751 moving(9);
lilac0112_1 4:7c488c059498 752 break;
lilac0112_1 4:7c488c059498 753 case 2:
lilac0112_1 4:7c488c059498 754 moving(1);
lilac0112_1 4:7c488c059498 755 break;
lilac0112_1 4:7c488c059498 756 case 3:
lilac0112_1 4:7c488c059498 757 moving(11);
lilac0112_1 4:7c488c059498 758 break;
lilac0112_1 4:7c488c059498 759 default :
lilac0112_1 4:7c488c059498 760 break;
lilac0112_1 4:7c488c059498 761
lilac0112_1 4:7c488c059498 762 }
lilac0112_1 4:7c488c059498 763 /*if(LastPwm[0]==0){
lilac0112_1 4:7c488c059498 764
lilac0112_1 4:7c488c059498 765 }
lilac0112_1 4:7c488c059498 766 move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg);*/
lilac0112_1 4:7c488c059498 767 wait(0.5);
lilac0112_1 4:7c488c059498 768 continue;
lilac0112_1 4:7c488c059498 769
lilac0112_1 4:7c488c059498 770 }else{
lilac0112_1 5:c8e4d5cdc353 771 if(PingData[0]<=10) LineState[2]=1;
lilac0112_1 5:c8e4d5cdc353 772 if(PingData[1]<=10) LineState[3]=1;
lilac0112_1 5:c8e4d5cdc353 773 if(PingData[2]<=10) LineState[4]=1;
lilac0112_1 4:7c488c059498 774
lilac0112_1 4:7c488c059498 775 switch(LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07){
lilac0112_1 4:7c488c059498 776
lilac0112_1 4:7c488c059498 777 case 0:
lilac0112_1 4:7c488c059498 778 break;
lilac0112_1 4:7c488c059498 779 case 1:
lilac0112_1 4:7c488c059498 780 moving(3);
lilac0112_1 4:7c488c059498 781 break;
lilac0112_1 4:7c488c059498 782 case 2:
lilac0112_1 4:7c488c059498 783 moving(9);
lilac0112_1 4:7c488c059498 784 break;
lilac0112_1 4:7c488c059498 785 case 3:
lilac0112_1 4:7c488c059498 786 moving(6);
lilac0112_1 4:7c488c059498 787 break;
lilac0112_1 4:7c488c059498 788 case 4:
lilac0112_1 4:7c488c059498 789 moving(0);
lilac0112_1 4:7c488c059498 790 break;
lilac0112_1 4:7c488c059498 791 case 5:
lilac0112_1 4:7c488c059498 792 moving(1);
lilac0112_1 4:7c488c059498 793 break;
lilac0112_1 4:7c488c059498 794 case 6:
lilac0112_1 4:7c488c059498 795 moving(11);
lilac0112_1 4:7c488c059498 796 break;
lilac0112_1 4:7c488c059498 797 case 7:
lilac0112_1 4:7c488c059498 798 moving(0);
lilac0112_1 4:7c488c059498 799 break;
lilac0112_1 4:7c488c059498 800 default :
lilac0112_1 4:7c488c059498 801 break;
lilac0112_1 4:7c488c059498 802
lilac0112_1 4:7c488c059498 803 }
lilac0112_1 4:7c488c059498 804
lilac0112_1 4:7c488c059498 805 if((LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07)!=0){
lilac0112_1 4:7c488c059498 806 wait(0.2);
lilac0112_1 4:7c488c059498 807 continue;
lilac0112_1 4:7c488c059498 808 }
lilac0112_1 4:7c488c059498 809 }
lilac0112_1 3:2f74791564c9 810
lilac0112_1 0:d35efbf4d62e 811
lilac0112_1 4:7c488c059498 812
lilac0112_1 4:7c488c059498 813 if(PingData[2]>=90){
lilac0112_1 4:7c488c059498 814
lilac0112_1 4:7c488c059498 815 moving(6);
lilac0112_1 4:7c488c059498 816 wait(0.2);
lilac0112_1 4:7c488c059498 817 continue;
lilac0112_1 3:2f74791564c9 818 }
lilac0112_1 5:c8e4d5cdc353 819
lilac0112_1 5:c8e4d5cdc353 820
lilac0112_1 5:c8e4d5cdc353 821 switch(/*IrNum*/12){
lilac0112_1 5:c8e4d5cdc353 822 case 0:
lilac0112_1 5:c8e4d5cdc353 823 case 1:
lilac0112_1 5:c8e4d5cdc353 824 case 2:
lilac0112_1 5:c8e4d5cdc353 825 case 3:
lilac0112_1 5:c8e4d5cdc353 826 moving(IrNum);
lilac0112_1 5:c8e4d5cdc353 827 break;
lilac0112_1 5:c8e4d5cdc353 828 case 4:
lilac0112_1 5:c8e4d5cdc353 829 case 5:
lilac0112_1 5:c8e4d5cdc353 830 moving(IrNum+Gap);
lilac0112_1 5:c8e4d5cdc353 831 break;
lilac0112_1 5:c8e4d5cdc353 832 case 6:
lilac0112_1 5:c8e4d5cdc353 833 if(PingData[0]>PingData[1]) moving(IrNum+Gap);
lilac0112_1 5:c8e4d5cdc353 834 else moving(IrNum-Gap);
lilac0112_1 5:c8e4d5cdc353 835
lilac0112_1 5:c8e4d5cdc353 836 break;
lilac0112_1 5:c8e4d5cdc353 837 case 7:
lilac0112_1 5:c8e4d5cdc353 838 case 8:
lilac0112_1 5:c8e4d5cdc353 839 moving(IrNum-Gap);
lilac0112_1 5:c8e4d5cdc353 840 break;
lilac0112_1 5:c8e4d5cdc353 841 case 9:
lilac0112_1 5:c8e4d5cdc353 842 case 10:
lilac0112_1 5:c8e4d5cdc353 843 case 11:
lilac0112_1 5:c8e4d5cdc353 844 moving(IrNum);
lilac0112_1 5:c8e4d5cdc353 845 break;
lilac0112_1 5:c8e4d5cdc353 846 case 12:
lilac0112_1 5:c8e4d5cdc353 847 GoHome();
lilac0112_1 5:c8e4d5cdc353 848 break;
lilac0112_1 5:c8e4d5cdc353 849 default:
lilac0112_1 5:c8e4d5cdc353 850 break;
lilac0112_1 4:7c488c059498 851 }
lilac0112_1 2:39135c67083d 852
lilac0112_1 5:c8e4d5cdc353 853
lilac0112_1 0:d35efbf4d62e 854 }
lilac0112_1 0:d35efbf4d62e 855 }