CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver2

2

Dependencies:   AQM0802A HMC6352 PID Servo mbed

main.cpp@4:f7946508daa8, 2015-03-03 (annotated)

Committer:
ryuna
Date:
Tue Mar 03 02:36:28 2015 +0000
Revision:
4:f7946508daa8
Parent:
3:0c994b3a882e
Child:
5:dace4f3b6e4a
??

Who changed what in which revision?

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