CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver3

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

Dependencies:   AQM0802A PID Servo mbed

main.cpp@2:e84bb87eea71, 2015-03-10 (annotated)

Committer:
ryuna
Date:
Tue Mar 10 08:28:15 2015 +0000
Revision:
2:e84bb87eea71
Parent:
1:f91d53098d57
Child:
3:4a39486ff238
???

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