CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver3

version 3 通信方式，マイコン等に変更あり

Dependencies:   AQM0802A PID Servo mbed

main.cpp@4:2857f273a7f4, 2015-03-11 (annotated)

Committer:
ryuna
Date:
Wed Mar 11 01:03:00 2015 +0000
Revision:
4:2857f273a7f4
Parent:
3:4a39486ff238
Child:
5:09afcbe0c18f
without multiserial

Who changed what in which revision?

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