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

Committer:
lilac0112_1
Date:
Fri Mar 13 02:10:37 2015 +0000
Revision:
4:7c488c059498
Parent:
3:2f74791564c9
Child:
5:c8e4d5cdc353
haven't written completely.

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 4:7c488c059498 84 if(abs(CompassDif)<CompassVias){
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 if(
lilac0112_1 4:7c488c059498 419 (
lilac0112_1 4:7c488c059498 420 (
lilac0112_1 4:7c488c059498 421 (
lilac0112_1 4:7c488c059498 422 (Compass / 10) + 540 - CompassDef
lilac0112_1 4:7c488c059498 423 ) % 360
lilac0112_1 4:7c488c059498 424 ) - REFERENCE
lilac0112_1 4:7c488c059498 425 ) >10.0
lilac0112_1 4:7c488c059498 426 ){
lilac0112_1 4:7c488c059498 427
lilac0112_1 4:7c488c059498 428 move(0,0,0,0);
lilac0112_1 4:7c488c059498 429 return;
lilac0112_1 4:7c488c059498 430 }
lilac0112_1 4:7c488c059498 431
lilac0112_1 4:7c488c059498 432
lilac0112_1 4:7c488c059498 433 for(int i=0; i<20; i++) State[i]=0;
lilac0112_1 4:7c488c059498 434
lilac0112_1 4:7c488c059498 435
lilac0112_1 4:7c488c059498 436
lilac0112_1 4:7c488c059498 437 if(PingData[2] >=60) State[0]=1;
lilac0112_1 4:7c488c059498 438
lilac0112_1 4:7c488c059498 439 if(PingData[2] <=40) State[1]=1;
lilac0112_1 4:7c488c059498 440
lilac0112_1 4:7c488c059498 441 if(PingData[2] >=90-10) State[5]=1;
lilac0112_1 4:7c488c059498 442
lilac0112_1 4:7c488c059498 443 if(PingData[2] <=40+10) State[6]=1;
lilac0112_1 4:7c488c059498 444
lilac0112_1 4:7c488c059498 445 if(PingData[2] >=10) State[7]=1;
lilac0112_1 4:7c488c059498 446
lilac0112_1 4:7c488c059498 447 if(PingData[2] <=5) State[8]=1;
lilac0112_1 4:7c488c059498 448
lilac0112_1 4:7c488c059498 449 if(PingData[2] >=25-5) State[9]=1;
lilac0112_1 4:7c488c059498 450
lilac0112_1 4:7c488c059498 451 if(PingData[2] <=5+5) State[10]=1;
lilac0112_1 4:7c488c059498 452
lilac0112_1 4:7c488c059498 453
lilac0112_1 4:7c488c059498 454
lilac0112_1 4:7c488c059498 455 if(PingData[0]>PingData[1]) State[2]=1;
lilac0112_1 4:7c488c059498 456
lilac0112_1 4:7c488c059498 457 if(
lilac0112_1 4:7c488c059498 458 (PingData[0]+PingData[1])
lilac0112_1 4:7c488c059498 459 >= 120
lilac0112_1 4:7c488c059498 460 ) State[3]=1;
lilac0112_1 4:7c488c059498 461
lilac0112_1 4:7c488c059498 462 if(
lilac0112_1 4:7c488c059498 463 (abs( int( int(PingData[0]) - int(PingData[1]) ) ) )
lilac0112_1 4:7c488c059498 464 <= 10
lilac0112_1 4:7c488c059498 465 ) State[4]=1;
lilac0112_1 4:7c488c059498 466
lilac0112_1 4:7c488c059498 467
lilac0112_1 4:7c488c059498 468 /*if(State[0]){
lilac0112_1 4:7c488c059498 469
lilac0112_1 4:7c488c059498 470 move(vrl[6],vrl[6],CompassPID,IrServo[6]);
lilac0112_1 4:7c488c059498 471 return ;
lilac0112_1 4:7c488c059498 472 }*/
lilac0112_1 4:7c488c059498 473
lilac0112_1 4:7c488c059498 474 if(State[4]){
lilac0112_1 4:7c488c059498 475
lilac0112_1 4:7c488c059498 476 if(State[3]){
lilac0112_1 4:7c488c059498 477
lilac0112_1 4:7c488c059498 478 if((State[7])==(State[8])){
lilac0112_1 4:7c488c059498 479
lilac0112_1 4:7c488c059498 480 move(0,0,CompassPID,LastDegree);
lilac0112_1 4:7c488c059498 481 return ;
lilac0112_1 4:7c488c059498 482 }
lilac0112_1 4:7c488c059498 483 if(State[7]){
lilac0112_1 4:7c488c059498 484
lilac0112_1 4:7c488c059498 485 move(vrl[6],vrl[6],CompassPID,IrServo[6]);
lilac0112_1 4:7c488c059498 486 return ;
lilac0112_1 4:7c488c059498 487 }
lilac0112_1 4:7c488c059498 488 if(State[8]){
lilac0112_1 4:7c488c059498 489
lilac0112_1 4:7c488c059498 490 move(vrl[0],vrl[0],CompassPID,IrServo[0]);
lilac0112_1 4:7c488c059498 491 return ;
lilac0112_1 4:7c488c059498 492 }
lilac0112_1 4:7c488c059498 493 }
lilac0112_1 4:7c488c059498 494
lilac0112_1 4:7c488c059498 495
lilac0112_1 4:7c488c059498 496 if((State[0])==(State[1])){
lilac0112_1 4:7c488c059498 497
lilac0112_1 4:7c488c059498 498 move(0,0,CompassPID,LastDegree);
lilac0112_1 4:7c488c059498 499 return ;
lilac0112_1 4:7c488c059498 500 }
lilac0112_1 4:7c488c059498 501 if(State[0]){
lilac0112_1 4:7c488c059498 502
lilac0112_1 4:7c488c059498 503 move(vrl[6],vrl[6],CompassPID,IrServo[6]);
lilac0112_1 4:7c488c059498 504 return ;
lilac0112_1 4:7c488c059498 505 }
lilac0112_1 4:7c488c059498 506 if(State[1]){
lilac0112_1 4:7c488c059498 507
lilac0112_1 4:7c488c059498 508 move(vrl[0],vrl[0],CompassPID,IrServo[0]);
lilac0112_1 4:7c488c059498 509 return ;
lilac0112_1 4:7c488c059498 510 }
lilac0112_1 4:7c488c059498 511
lilac0112_1 4:7c488c059498 512 }else{
lilac0112_1 4:7c488c059498 513 if(State[2]){
lilac0112_1 4:7c488c059498 514 move(vrl[9],vrl[9],CompassPID,IrServo[9]);
lilac0112_1 4:7c488c059498 515 return ;
lilac0112_1 4:7c488c059498 516 }
lilac0112_1 4:7c488c059498 517 else{
lilac0112_1 4:7c488c059498 518 move(vrl[3],vrl[3],CompassPID,IrServo[3]);
lilac0112_1 4:7c488c059498 519 return ;
lilac0112_1 4:7c488c059498 520 }
lilac0112_1 4:7c488c059498 521 }
lilac0112_1 4:7c488c059498 522
lilac0112_1 2:39135c67083d 523 }
lilac0112_1 0:d35efbf4d62e 524
lilac0112_1 0:d35efbf4d62e 525
lilac0112_1 2:39135c67083d 526 void PidUpdate()
lilac0112_1 2:39135c67083d 527 {
lilac0112_1 0:d35efbf4d62e 528
lilac0112_1 3:2f74791564c9 529 pid.setSetPoint((int)((REFERENCE + SetC) / 1.0));
lilac0112_1 3:2f74791564c9 530 InputPID = ((Compass/10) + 540 - CompassDef) % 360;
lilac0112_1 3:2f74791564c9 531
lilac0112_1 3:2f74791564c9 532 pid.setProcessValue(InputPID);
lilac0112_1 3:2f74791564c9 533 CompassPID = (pid.compute());
lilac0112_1 0:d35efbf4d62e 534 }
lilac0112_1 2:39135c67083d 535
lilac0112_1 0:d35efbf4d62e 536 uint8_t SwRead(){
lilac0112_1 0:d35efbf4d62e 537 /******
lilac0112_1 2:39135c67083d 538 *retrun : sw_state
lilac0112_1 2:39135c67083d 539 *StartS = 0x01;
lilac0112_1 2:39135c67083d 540 *Debug2 = 0x02;
lilac0112_1 0:d35efbf4d62e 541 *Debug1 = 0x04;
lilac0112_1 2:39135c67083d 542 *Debug3 = 0x06;
lilac0112_1 2:39135c67083d 543 *Kicker = 0x08;
lilac0112_1 2:39135c67083d 544 *Calibration = 0x10;
lilac0112_1 2:39135c67083d 545
lilac0112_1 0:d35efbf4d62e 546 *
lilac0112_1 0:d35efbf4d62e 547 *****/
lilac0112_1 0:d35efbf4d62e 548 uint8_t i,temp,temp2;
lilac0112_1 2:39135c67083d 549 temp = ~Sw - 224;
lilac0112_1 2:39135c67083d 550 if(!(temp == Kicker
lilac0112_1 0:d35efbf4d62e 551 ||temp == Debug1
lilac0112_1 0:d35efbf4d62e 552 ||temp == Debug2
lilac0112_1 2:39135c67083d 553 ||temp == Debug3
lilac0112_1 3:2f74791564c9 554 ||temp == Debug12
lilac0112_1 3:2f74791564c9 555 ||temp == Debug23
lilac0112_1 2:39135c67083d 556 ||temp == StartS)) return 0;/*スイッチが押されていない*/
lilac0112_1 2:39135c67083d 557 if(!(temp == 0x00)){
lilac0112_1 0:d35efbf4d62e 558 for(i = 0; i < 50; i++);
lilac0112_1 2:39135c67083d 559 temp2 = ~Sw - 224;
lilac0112_1 0:d35efbf4d62e 560 if(temp == temp2){
lilac0112_1 0:d35efbf4d62e 561 return temp;
lilac0112_1 0:d35efbf4d62e 562 }
lilac0112_1 0:d35efbf4d62e 563 }
lilac0112_1 0:d35efbf4d62e 564 return 0;
lilac0112_1 0:d35efbf4d62e 565 }
lilac0112_1 0:d35efbf4d62e 566
lilac0112_1 0:d35efbf4d62e 567 //通信（モータ用）
lilac0112_1 0:d35efbf4d62e 568 void tx_motor(){
lilac0112_1 2:39135c67083d 569
lilac0112_1 3:2f74791564c9 570 /*
lilac0112_1 3:2f74791564c9 571 speed[0]=0;
lilac0112_1 3:2f74791564c9 572 speed[1]=0;
lilac0112_1 3:2f74791564c9 573 speed[2]=0;
lilac0112_1 3:2f74791564c9 574 speed[3]=0;
lilac0112_1 3:2f74791564c9 575 */
lilac0112_1 0:d35efbf4d62e 576 array(speed[0],speed[1],speed[3],speed[2]);
lilac0112_1 0:d35efbf4d62e 577 Motor.printf("%s",StringFIN.c_str());
lilac0112_1 0:d35efbf4d62e 578 }
lilac0112_1 0:d35efbf4d62e 579
lilac0112_1 2:39135c67083d 580 void SetUp(){
lilac0112_1 0:d35efbf4d62e 581 /*初期化*/
lilac0112_1 3:2f74791564c9 582
lilac0112_1 3:2f74791564c9 583
lilac0112_1 0:d35efbf4d62e 584 Motor.baud(115200); //ボーレート設定
lilac0112_1 0:d35efbf4d62e 585 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:d35efbf4d62e 586 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 2:39135c67083d 587
lilac0112_1 3:2f74791564c9 588 Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
lilac0112_1 3:2f74791564c9 589
lilac0112_1 3:2f74791564c9 590 S555.calibrate(0.0006, 120.0);
lilac0112_1 3:2f74791564c9 591 //S555.position(0.0); //初期位置にセット
lilac0112_1 2:39135c67083d 592 move(0,0,0,0);//停止
lilac0112_1 0:d35efbf4d62e 593
lilac0112_1 2:39135c67083d 594 Kick = 0;
lilac0112_1 2:39135c67083d 595 Sw.mode(PullUp);
lilac0112_1 0:d35efbf4d62e 596
lilac0112_1 2:39135c67083d 597 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 2:39135c67083d 598 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 2:39135c67083d 599 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 2:39135c67083d 600 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 3:2f74791564c9 601 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:d35efbf4d62e 602
lilac0112_1 2:39135c67083d 603 for(int i=0; i<15; i++){
lilac0112_1 3:2f74791564c9 604 CompassDef = (Compass / 10);
lilac0112_1 3:2f74791564c9 605 wait_ms(10);
lilac0112_1 2:39135c67083d 606 }
lilac0112_1 4:7c488c059498 607 //pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 3:2f74791564c9 608
lilac0112_1 0:d35efbf4d62e 609
lilac0112_1 0:d35efbf4d62e 610 }
lilac0112_1 0:d35efbf4d62e 611 void StartLoop(){
lilac0112_1 2:39135c67083d 612
lilac0112_1 0:d35efbf4d62e 613 uint8_t State = 0;
lilac0112_1 2:39135c67083d 614 uint8_t LineData = 0;
lilac0112_1 0:d35efbf4d62e 615 while(1){
lilac0112_1 2:39135c67083d 616 Led[0] = Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 2:39135c67083d 617 //Lcd.cls();
lilac0112_1 3:2f74791564c9 618
lilac0112_1 0:d35efbf4d62e 619 State = SwRead();
lilac0112_1 0:d35efbf4d62e 620 if(State == 0) continue;
lilac0112_1 0:d35efbf4d62e 621
lilac0112_1 2:39135c67083d 622 if(State == StartS){
lilac0112_1 2:39135c67083d 623 /*loop end & start*/
lilac0112_1 3:2f74791564c9 624 return;
lilac0112_1 0:d35efbf4d62e 625 }
lilac0112_1 2:39135c67083d 626
lilac0112_1 2:39135c67083d 627 if(State == Debug1){
lilac0112_1 2:39135c67083d 628 while((State == Debug1)){
lilac0112_1 2:39135c67083d 629 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 2:39135c67083d 630 Lcd.printf("%d\n",LineData);
lilac0112_1 2:39135c67083d 631
lilac0112_1 2:39135c67083d 632 wait_ms(100);
lilac0112_1 2:39135c67083d 633 State = SwRead();
lilac0112_1 2:39135c67083d 634 }
lilac0112_1 2:39135c67083d 635 Lcd.cls();
lilac0112_1 0:d35efbf4d62e 636 continue;
lilac0112_1 0:d35efbf4d62e 637
lilac0112_1 0:d35efbf4d62e 638 }
lilac0112_1 0:d35efbf4d62e 639 if(State == Debug2){
lilac0112_1 2:39135c67083d 640 while((State == Debug2)){
lilac0112_1 3:2f74791564c9 641 //Receive();
lilac0112_1 2:39135c67083d 642 Lcd.printf("%d\n",IrNum);
lilac0112_1 3:2f74791564c9 643 //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 644 wait_ms(100);
lilac0112_1 2:39135c67083d 645 State = SwRead();
lilac0112_1 2:39135c67083d 646 }
lilac0112_1 2:39135c67083d 647 Lcd.cls();
lilac0112_1 2:39135c67083d 648 continue;
lilac0112_1 0:d35efbf4d62e 649 }
lilac0112_1 2:39135c67083d 650
lilac0112_1 2:39135c67083d 651 if(State == Debug3){
lilac0112_1 2:39135c67083d 652 while((State == Debug3)){
lilac0112_1 2:39135c67083d 653 Lcd.printf("%d\n",((Compass / 10) + 540 - CompassDef) % 360);
lilac0112_1 3:2f74791564c9 654 wait_ms(100);
lilac0112_1 3:2f74791564c9 655 State = SwRead();
lilac0112_1 3:2f74791564c9 656 }
lilac0112_1 3:2f74791564c9 657 Lcd.cls();
lilac0112_1 3:2f74791564c9 658 continue;
lilac0112_1 3:2f74791564c9 659 }
lilac0112_1 3:2f74791564c9 660 if(State == Debug12){
lilac0112_1 3:2f74791564c9 661 while((State == Debug12)){
lilac0112_1 3:2f74791564c9 662 Lcd.printf("%d\n", PingData[0]);//Left
lilac0112_1 3:2f74791564c9 663 Lcd.printf("%d\n", PingData[1]);//Right
lilac0112_1 3:2f74791564c9 664 wait_ms(100);
lilac0112_1 3:2f74791564c9 665 State = SwRead();
lilac0112_1 3:2f74791564c9 666 }
lilac0112_1 3:2f74791564c9 667 Lcd.cls();
lilac0112_1 3:2f74791564c9 668 continue;
lilac0112_1 3:2f74791564c9 669 }
lilac0112_1 3:2f74791564c9 670 if(State == Debug23){
lilac0112_1 3:2f74791564c9 671 while((State == Debug23)){
lilac0112_1 3:2f74791564c9 672 Lcd.printf("%d\n", PingData[2]);//Back
lilac0112_1 3:2f74791564c9 673 Lcd.printf("%d\n", PingData[3]);//Front
lilac0112_1 2:39135c67083d 674 wait_ms(100);
lilac0112_1 2:39135c67083d 675 State = SwRead();
lilac0112_1 2:39135c67083d 676 }
lilac0112_1 2:39135c67083d 677 Lcd.cls();
lilac0112_1 2:39135c67083d 678 continue;
lilac0112_1 0:d35efbf4d62e 679 }
lilac0112_1 0:d35efbf4d62e 680
lilac0112_1 2:39135c67083d 681 if(State == Kicker){
lilac0112_1 3:2f74791564c9 682 while((State == Kicker)){
lilac0112_1 3:2f74791564c9 683 Lcd.printf("%f\n",CompassPID);
lilac0112_1 3:2f74791564c9 684 wait_ms(100);
lilac0112_1 3:2f74791564c9 685 State = SwRead();
lilac0112_1 3:2f74791564c9 686 }
lilac0112_1 3:2f74791564c9 687 Lcd.cls();
lilac0112_1 3:2f74791564c9 688 continue;
lilac0112_1 3:2f74791564c9 689
lilac0112_1 3:2f74791564c9 690 /*Led[0] = Led[1] = Led[2] = 0;
lilac0112_1 3:2f74791564c9 691 Kick = 1;
lilac0112_1 3:2f74791564c9 692 wait_ms(500);
lilac0112_1 3:2f74791564c9 693 Kick = 0;
lilac0112_1 2:39135c67083d 694 while((State == Kicker)){
lilac0112_1 2:39135c67083d 695 wait_ms(100);
lilac0112_1 2:39135c67083d 696 State = SwRead();
lilac0112_1 2:39135c67083d 697 }
lilac0112_1 3:2f74791564c9 698 continue;*/
lilac0112_1 2:39135c67083d 699 }
lilac0112_1 3:2f74791564c9 700 }
lilac0112_1 0:d35efbf4d62e 701
lilac0112_1 0:d35efbf4d62e 702 }
lilac0112_1 4:7c488c059498 703
lilac0112_1 4:7c488c059498 704 uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
lilac0112_1 4:7c488c059498 705 return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one;
lilac0112_1 4:7c488c059498 706 }
lilac0112_1 4:7c488c059498 707
lilac0112_1 0:d35efbf4d62e 708 int main() {
lilac0112_1 0:d35efbf4d62e 709
lilac0112_1 3:2f74791564c9 710 //uint8_t IrNumOld = 0;//過去値
lilac0112_1 0:d35efbf4d62e 711 /*Line*/
lilac0112_1 2:39135c67083d 712 uint8_t LineData = 0;
lilac0112_1 4:7c488c059498 713 //uint8_t LinePing = 0;
lilac0112_1 3:2f74791564c9 714
lilac0112_1 2:39135c67083d 715
lilac0112_1 0:d35efbf4d62e 716 /*State */
lilac0112_1 4:7c488c059498 717 /*uint8_t LineIr = 0;
lilac0112_1 3:2f74791564c9 718 uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06,
lilac0112_1 3:2f74791564c9 719
lilac0112_1 3:2f74791564c9 720 0x04,0x04,0x0B,0x08,0x08,0x09,0x00};
lilac0112_1 4:7c488c059498 721 */
lilac0112_1 2:39135c67083d 722 /*行動設定*/
lilac0112_1 4:7c488c059498 723 //int Power = 0;
lilac0112_1 4:7c488c059498 724 //int Degree = 0;
lilac0112_1 2:39135c67083d 725
lilac0112_1 2:39135c67083d 726 /*楽しい変数達*/
lilac0112_1 4:7c488c059498 727 //int nDegree =0;//基礎角
lilac0112_1 4:7c488c059498 728 //int addDegree = 0;//追加角
lilac0112_1 0:d35efbf4d62e 729
lilac0112_1 4:7c488c059498 730 /**/
lilac0112_1 4:7c488c059498 731 //int Gap=2;
lilac0112_1 4:7c488c059498 732 bool LineState[10]={0};
lilac0112_1 0:d35efbf4d62e 733
lilac0112_1 0:d35efbf4d62e 734
lilac0112_1 0:d35efbf4d62e 735 /*関数ポインタ*/
lilac0112_1 3:2f74791564c9 736
lilac0112_1 3:2f74791564c9 737
lilac0112_1 3:2f74791564c9 738 //void (*AnotherAction[3])(uint8_t [],double);
lilac0112_1 4:7c488c059498 739 /*void (*AnotherAction[3])();
lilac0112_1 2:39135c67083d 740 AnotherAction[0] = IrFrontAction;
lilac0112_1 2:39135c67083d 741 AnotherAction[1] = IrBackAction;
lilac0112_1 4:7c488c059498 742 AnotherAction[2] = GoHome;*/
lilac0112_1 2:39135c67083d 743
lilac0112_1 3:2f74791564c9 744
lilac0112_1 2:39135c67083d 745 SetUp();/*set up routine*/
lilac0112_1 3:2f74791564c9 746
lilac0112_1 2:39135c67083d 747 StartLoop(); /*loop strat, switch push end*/
lilac0112_1 3:2f74791564c9 748 Led[0] = Led[1] = Led[2] = Led[3] = 0;
lilac0112_1 3:2f74791564c9 749 wait_ms(100);
lilac0112_1 2:39135c67083d 750
lilac0112_1 3:2f74791564c9 751 Loop = 0;
lilac0112_1 2:39135c67083d 752
lilac0112_1 3:2f74791564c9 753 while(1){
lilac0112_1 3:2f74791564c9 754
lilac0112_1 4:7c488c059498 755 if(
lilac0112_1 4:7c488c059498 756 (
lilac0112_1 4:7c488c059498 757 (
lilac0112_1 4:7c488c059498 758 (
lilac0112_1 4:7c488c059498 759 (Compass / 10) + 540 - CompassDef
lilac0112_1 4:7c488c059498 760 ) % 360
lilac0112_1 4:7c488c059498 761 ) - REFERENCE
lilac0112_1 4:7c488c059498 762 ) >15.0
lilac0112_1 4:7c488c059498 763 ){
lilac0112_1 3:2f74791564c9 764
lilac0112_1 4:7c488c059498 765 move(0,0,0,0);
lilac0112_1 2:39135c67083d 766 continue;
lilac0112_1 3:2f74791564c9 767 }
lilac0112_1 3:2f74791564c9 768
lilac0112_1 4:7c488c059498 769 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 4:7c488c059498 770
lilac0112_1 4:7c488c059498 771 for(int i=0; i<10; i++) LineState[i]=0;
lilac0112_1 3:2f74791564c9 772
lilac0112_1 4:7c488c059498 773 if(LineData!=0){
lilac0112_1 4:7c488c059498 774
lilac0112_1 4:7c488c059498 775 if(PingData[0]>PingData[1]) LineState[0]=1;
lilac0112_1 4:7c488c059498 776 if(PingData[2]<=20) LineState[1]=1;
lilac0112_1 4:7c488c059498 777
lilac0112_1 4:7c488c059498 778 switch(LinkBit(0,0,0,0,0,0,LineState[1],LineState[0])&0x03){
lilac0112_1 4:7c488c059498 779
lilac0112_1 4:7c488c059498 780 case 0:
lilac0112_1 4:7c488c059498 781 moving(3);
lilac0112_1 4:7c488c059498 782 break;
lilac0112_1 4:7c488c059498 783 case 1:
lilac0112_1 4:7c488c059498 784 moving(9);
lilac0112_1 4:7c488c059498 785 break;
lilac0112_1 4:7c488c059498 786 case 2:
lilac0112_1 4:7c488c059498 787 moving(1);
lilac0112_1 4:7c488c059498 788 break;
lilac0112_1 4:7c488c059498 789 case 3:
lilac0112_1 4:7c488c059498 790 moving(11);
lilac0112_1 4:7c488c059498 791 break;
lilac0112_1 4:7c488c059498 792 default :
lilac0112_1 4:7c488c059498 793 break;
lilac0112_1 4:7c488c059498 794
lilac0112_1 4:7c488c059498 795 }
lilac0112_1 4:7c488c059498 796 /*if(LastPwm[0]==0){
lilac0112_1 4:7c488c059498 797
lilac0112_1 4:7c488c059498 798 }
lilac0112_1 4:7c488c059498 799 move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg);*/
lilac0112_1 4:7c488c059498 800 wait(0.5);
lilac0112_1 4:7c488c059498 801 continue;
lilac0112_1 4:7c488c059498 802
lilac0112_1 4:7c488c059498 803 }else{
lilac0112_1 4:7c488c059498 804 if(PingData[0]<=5) LineState[2]=1;
lilac0112_1 4:7c488c059498 805 if(PingData[1]<=5) LineState[3]=1;
lilac0112_1 4:7c488c059498 806 if(PingData[2]<=5) LineState[4]=1;
lilac0112_1 4:7c488c059498 807
lilac0112_1 4:7c488c059498 808 switch(LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07){
lilac0112_1 4:7c488c059498 809
lilac0112_1 4:7c488c059498 810 case 0:
lilac0112_1 4:7c488c059498 811 break;
lilac0112_1 4:7c488c059498 812 case 1:
lilac0112_1 4:7c488c059498 813 moving(3);
lilac0112_1 4:7c488c059498 814 break;
lilac0112_1 4:7c488c059498 815 case 2:
lilac0112_1 4:7c488c059498 816 moving(9);
lilac0112_1 4:7c488c059498 817 break;
lilac0112_1 4:7c488c059498 818 case 3:
lilac0112_1 4:7c488c059498 819 moving(6);
lilac0112_1 4:7c488c059498 820 break;
lilac0112_1 4:7c488c059498 821 case 4:
lilac0112_1 4:7c488c059498 822 moving(0);
lilac0112_1 4:7c488c059498 823 break;
lilac0112_1 4:7c488c059498 824 case 5:
lilac0112_1 4:7c488c059498 825 moving(1);
lilac0112_1 4:7c488c059498 826 break;
lilac0112_1 4:7c488c059498 827 case 6:
lilac0112_1 4:7c488c059498 828 moving(11);
lilac0112_1 4:7c488c059498 829 break;
lilac0112_1 4:7c488c059498 830 case 7:
lilac0112_1 4:7c488c059498 831 moving(0);
lilac0112_1 4:7c488c059498 832 break;
lilac0112_1 4:7c488c059498 833 default :
lilac0112_1 4:7c488c059498 834 break;
lilac0112_1 4:7c488c059498 835
lilac0112_1 4:7c488c059498 836 }
lilac0112_1 4:7c488c059498 837
lilac0112_1 4:7c488c059498 838 if((LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07)!=0){
lilac0112_1 4:7c488c059498 839 wait(0.2);
lilac0112_1 4:7c488c059498 840 continue;
lilac0112_1 4:7c488c059498 841 }
lilac0112_1 4:7c488c059498 842 }
lilac0112_1 3:2f74791564c9 843
lilac0112_1 0:d35efbf4d62e 844
lilac0112_1 4:7c488c059498 845
lilac0112_1 4:7c488c059498 846 if(PingData[2]>=90){
lilac0112_1 4:7c488c059498 847
lilac0112_1 4:7c488c059498 848 moving(6);
lilac0112_1 4:7c488c059498 849 wait(0.2);
lilac0112_1 4:7c488c059498 850 continue;
lilac0112_1 3:2f74791564c9 851 }
lilac0112_1 4:7c488c059498 852 else{
lilac0112_1 4:7c488c059498 853
lilac0112_1 4:7c488c059498 854 moving(12);
lilac0112_1 4:7c488c059498 855 continue;
lilac0112_1 4:7c488c059498 856 }
lilac0112_1 2:39135c67083d 857
lilac0112_1 0:d35efbf4d62e 858 }
lilac0112_1 4:7c488c059498 859 /*
lilac0112_1 4:7c488c059498 860 Gap=2;
lilac0112_1 0:d35efbf4d62e 861
lilac0112_1 4:7c488c059498 862 switch(IrNum){
lilac0112_1 4:7c488c059498 863 case 0:
lilac0112_1 4:7c488c059498 864 case 1:
lilac0112_1 4:7c488c059498 865 case 2:
lilac0112_1 4:7c488c059498 866 case 3:
lilac0112_1 4:7c488c059498 867 moving(IrNum);
lilac0112_1 4:7c488c059498 868 break;
lilac0112_1 4:7c488c059498 869 case 4:
lilac0112_1 4:7c488c059498 870 case 5:
lilac0112_1 4:7c488c059498 871 moving(IrNum+Gap);
lilac0112_1 4:7c488c059498 872 break;
lilac0112_1 4:7c488c059498 873 case 6:
lilac0112_1 4:7c488c059498 874 if(PingData[0]>PingData[1]) moving(IrNum+Gap);
lilac0112_1 4:7c488c059498 875 else moving(IrNum-Gap);
lilac0112_1 4:7c488c059498 876
lilac0112_1 4:7c488c059498 877 break;
lilac0112_1 4:7c488c059498 878 case 7:
lilac0112_1 4:7c488c059498 879 case 8:
lilac0112_1 4:7c488c059498 880 moving(IrNum-Gap);
lilac0112_1 4:7c488c059498 881 break;
lilac0112_1 4:7c488c059498 882 case 9:
lilac0112_1 4:7c488c059498 883 case 10:
lilac0112_1 4:7c488c059498 884 case 11:
lilac0112_1 4:7c488c059498 885 moving(IrNum);
lilac0112_1 4:7c488c059498 886 break;
lilac0112_1 4:7c488c059498 887 case 12:
lilac0112_1 4:7c488c059498 888 GoHome();
lilac0112_1 4:7c488c059498 889 break;
lilac0112_1 4:7c488c059498 890
lilac0112_1 0:d35efbf4d62e 891 }
lilac0112_1 4:7c488c059498 892 */
lilac0112_1 2:39135c67083d 893
lilac0112_1 2:39135c67083d 894
lilac0112_1 0:d35efbf4d62e 895 }