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@7:81f57b67dff8, 2015-03-14 (annotated)

Committer:
lilac0112_1
Date:
Sat Mar 14 07:16:46 2015 +0000
Revision:
7:81f57b67dff8
Parent:
6:2c9cb40ada59 
The end...

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