CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver2

2

Dependencies:   AQM0802A HMC6352 PID Servo mbed

main.cpp@5:dace4f3b6e4a, 2015-03-04 (annotated)

Committer:
ryuna
Date:
Wed Mar 04 07:01:19 2015 +0000
Revision:
5:dace4f3b6e4a
Parent:
4:f7946508daa8 
add one function _fool

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 5:dace4f3b6e4a 56 wait_ms(1);
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 5:dace4f3b6e4a 62 wait_ms(1);
ryuna 3:0c994b3a882e 63 }
ryuna 0:dfe81bdcb486 64
ryuna 3:0c994b3a882e 65 void ReceiveFast(uint8_t Address, char kind, char output[],int num){
ryuna 3:0c994b3a882e 66 /*初めに相手に何が欲しいのか言わない
ryuna 3:0c994b3a882e 67 *要は全部よこせということ
ryuna 3:0c994b3a882e 68 *上位3つだとirのみ
ryuna 3:0c994b3a882e 69 *上位5つだとirとping
ryuna 3:0c994b3a882e 70 *
ryuna 3:0c994b3a882e 71 *[0]:ir num num
ryuna 3:0c994b3a882e 72 *[1]:ir max_data
ryuna 3:0c994b3a882e 73 *[2]:ir second_max_data
ryuna 3:0c994b3a882e 74 *[3]:ping
ryuna 3:0c994b3a882e 75 *[4]:ping
ryuna 3:0c994b3a882e 76 **/
ryuna 3:0c994b3a882e 77
ryuna 3:0c994b3a882e 78 bool val;
ryuna 5:dace4f3b6e4a 79 Led[0] = 0;
ryuna 3:0c994b3a882e 80 val = Sensor.read(Address|1,output,num);
ryuna 5:dace4f3b6e4a 81 wait_ms(1);
ryuna 3:0c994b3a882e 82 Led[0] = !val;
ryuna 3:0c994b3a882e 83
ryuna 3:0c994b3a882e 84 }
ryuna 0:dfe81bdcb486 85
ryuna 0:dfe81bdcb486 86
ryuna 0:dfe81bdcb486 87 uint8_t IrReceive(uint8_t IrData[]){
ryuna 0:dfe81bdcb486 88 /*
ryuna 0:dfe81bdcb486 89 *Irdata[0] : 1位,2位の場所(1位*13+2位)
ryuna 0:dfe81bdcb486 90 *Irdata[1] : 1位の値
ryuna 0:dfe81bdcb486 91 *return : 1位の場所
ryuna 0:dfe81bdcb486 92 */
ryuna 3:0c994b3a882e 93 char data_r[3] = {0},data_l[3] = {0};
ryuna 3:0c994b3a882e 94
ryuna 5:dace4f3b6e4a 95 Receive(ADDRESS_R,BALL,data_r,3);
ryuna 5:dace4f3b6e4a 96 Receive(ADDRESS_L,BALL,data_l,3);
ryuna 0:dfe81bdcb486 97
ryuna 3:0c994b3a882e 98 if((data_r[0] == 125)||(data_l[0] == 125)){/*ボールを検知しているかチェック*/
ryuna 3:0c994b3a882e 99 if((data_r[0] == 125)&&(data_l[0] == 125)){
ryuna 0:dfe81bdcb486 100 IrData[0] = 12*13 + 12;
ryuna 0:dfe81bdcb486 101 return 12;
ryuna 0:dfe81bdcb486 102 }
ryuna 3:0c994b3a882e 103 if(data_r[0] == 125){
ryuna 0:dfe81bdcb486 104 IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6);
ryuna 0:dfe81bdcb486 105 IrData[1] = data_l[1];
ryuna 3:0c994b3a882e 106 return (data_l[0]/6)+6;
ryuna 0:dfe81bdcb486 107
ryuna 0:dfe81bdcb486 108 }
ryuna 0:dfe81bdcb486 109
ryuna 0:dfe81bdcb486 110 IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6);
ryuna 0:dfe81bdcb486 111 IrData[1] = data_r[1];
ryuna 0:dfe81bdcb486 112 return data_r[0]/6;
ryuna 0:dfe81bdcb486 113 }
ryuna 0:dfe81bdcb486 114
ryuna 0:dfe81bdcb486 115 if(data_r[2]<data_l[2]){
ryuna 0:dfe81bdcb486 116 if(data_r[2]<data_l[1]){
ryuna 0:dfe81bdcb486 117 IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6);
ryuna 0:dfe81bdcb486 118 IrData[1] = data_r[1];
ryuna 0:dfe81bdcb486 119 return data_r[0]/6;
ryuna 0:dfe81bdcb486 120 }
ryuna 0:dfe81bdcb486 121
ryuna 0:dfe81bdcb486 122 IrData[0] = (data_r[0]/6)*13 + (data_l[0]/6+6);
ryuna 0:dfe81bdcb486 123 IrData[1] = data_r[1];
ryuna 0:dfe81bdcb486 124 return data_r[0]/6;
ryuna 0:dfe81bdcb486 125
ryuna 0:dfe81bdcb486 126 }else{
ryuna 0:dfe81bdcb486 127 if(data_l[2]<data_r[1]){
ryuna 0:dfe81bdcb486 128 IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6);
ryuna 0:dfe81bdcb486 129 IrData[1] = data_l[1];
ryuna 0:dfe81bdcb486 130 return (data_l[0]/6+6);
ryuna 0:dfe81bdcb486 131 }
ryuna 0:dfe81bdcb486 132
ryuna 0:dfe81bdcb486 133 IrData[0] = (data_l[0]/6+6)*13 + (data_r[0]/6);
ryuna 0:dfe81bdcb486 134 IrData[1] = data_l[1];
ryuna 0:dfe81bdcb486 135 return (data_l[0]/6+6);
ryuna 0:dfe81bdcb486 136 }
ryuna 0:dfe81bdcb486 137
ryuna 0:dfe81bdcb486 138
ryuna 0:dfe81bdcb486 139 }
ryuna 3:0c994b3a882e 140 uint8_t IrReceiveFast(){
ryuna 3:0c994b3a882e 141 /*
ryuna 3:0c994b3a882e 142 *return : 1位の場所
ryuna 3:0c994b3a882e 143 */
ryuna 3:0c994b3a882e 144 char data_r[3] = {0},data_l[3] = {0};
ryuna 3:0c994b3a882e 145
ryuna 3:0c994b3a882e 146 ReceiveFast(ADDRESS_R,BALL,data_r,3);
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 5:dace4f3b6e4a 194 void move(int vr,int vl, double vs ,int Rad){
ryuna 0:dfe81bdcb486 195 double pwm[4] = {0};
ryuna 0:dfe81bdcb486 196 uint8_t i = 0;
ryuna 5:dace4f3b6e4a 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 5:dace4f3b6e4a 210 SetRad = Rad;
ryuna 0:dfe81bdcb486 211 }
ryuna 5:dace4f3b6e4a 212 void fool (int *Rad, int *Power){
ryuna 5:dace4f3b6e4a 213 static int Last_Rad = 0;
ryuna 5:dace4f3b6e4a 214 static int Last_Vector = 1;
ryuna 5:dace4f3b6e4a 215 int rad = *Rad;
ryuna 5:dace4f3b6e4a 216 int Temp;
ryuna 5:dace4f3b6e4a 217 Temp = Last_Rad % 180;
ryuna 0:dfe81bdcb486 218
ryuna 5:dace4f3b6e4a 219 if((Temp>70) &&(Temp<110)){
ryuna 5:dace4f3b6e4a 220 Temp = *Rad % 180;
ryuna 5:dace4f3b6e4a 221 if((Temp>70) &&(Temp<110)){
ryuna 5:dace4f3b6e4a 222 Temp = abs(*Rad - Last_Rad);
ryuna 5:dace4f3b6e4a 223 if(Temp>160){
ryuna 5:dace4f3b6e4a 224 Last_Vector = -1 * Last_Vector;//正転逆転切り替え
ryuna 5:dace4f3b6e4a 225 if(*Rad/180){
ryuna 5:dace4f3b6e4a 226 *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180);
ryuna 5:dace4f3b6e4a 227 *Power = *Power * Last_Vector;
ryuna 5:dace4f3b6e4a 228 }else{
ryuna 5:dace4f3b6e4a 229 *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad);
ryuna 5:dace4f3b6e4a 230 *Power = *Power * Last_Vector;
ryuna 0:dfe81bdcb486 231 }
ryuna 5:dace4f3b6e4a 232 Last_Rad = rad;
ryuna 5:dace4f3b6e4a 233 return;
ryuna 5:dace4f3b6e4a 234 }else if((Last_Vector+2) == 1){
ryuna 5:dace4f3b6e4a 235 /*逆転のまま角度拡張*/
ryuna 5:dace4f3b6e4a 236 if(*Rad/180){
ryuna 5:dace4f3b6e4a 237 *Rad = -360 + *Rad ;
ryuna 5:dace4f3b6e4a 238 *Power = *Power * Last_Vector;
ryuna 5:dace4f3b6e4a 239 }else{
ryuna 5:dace4f3b6e4a 240 *Power = *Power * Last_Vector;
ryuna 5:dace4f3b6e4a 241 }
ryuna 5:dace4f3b6e4a 242 Last_Rad = rad;
ryuna 5:dace4f3b6e4a 243 return;
ryuna 5:dace4f3b6e4a 244
ryuna 5:dace4f3b6e4a 245 }else if((Last_Vector+2) == 3){
ryuna 5:dace4f3b6e4a 246 /*正転のまま*/
ryuna 5:dace4f3b6e4a 247 if(*Rad/180){
ryuna 5:dace4f3b6e4a 248 *Rad = -360 + *Rad ;
ryuna 5:dace4f3b6e4a 249 *Power = *Power * Last_Vector;
ryuna 5:dace4f3b6e4a 250 }else{
ryuna 5:dace4f3b6e4a 251 *Power = *Power * Last_Vector;
ryuna 5:dace4f3b6e4a 252 Last_Rad = rad;
ryuna 5:dace4f3b6e4a 253 }
ryuna 5:dace4f3b6e4a 254 Last_Rad = rad;
ryuna 5:dace4f3b6e4a 255 return;
ryuna 3:0c994b3a882e 256 }
ryuna 0:dfe81bdcb486 257 }
ryuna 0:dfe81bdcb486 258 }
ryuna 5:dace4f3b6e4a 259 /*通常動作*/
ryuna 5:dace4f3b6e4a 260 Last_Vector = RadToVector[(*Rad-1)/90];
ryuna 5:dace4f3b6e4a 261 *Rad = Angle[*Rad%180];
ryuna 5:dace4f3b6e4a 262 *Power = *Power * Last_Vector;
ryuna 5:dace4f3b6e4a 263 Last_Rad = rad;
ryuna 5:dace4f3b6e4a 264
ryuna 0:dfe81bdcb486 265 }
ryuna 0:dfe81bdcb486 266
ryuna 3:0c994b3a882e 267 void IrFrontAction(int *CompassDef , uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
ryuna 3:0c994b3a882e 268 {
ryuna 3:0c994b3a882e 269
ryuna 3:0c994b3a882e 270 }
ryuna 3:0c994b3a882e 271
ryuna 3:0c994b3a882e 272 void IrBackAction(int *CompassDef , uint8_t IrMemo[],double ReV)//ball found six o-clock
ryuna 0:dfe81bdcb486 273 {
ryuna 0:dfe81bdcb486 274 /***
ryuna 0:dfe81bdcb486 275 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
ryuna 0:dfe81bdcb486 276 *
ryuna 0:dfe81bdcb486 277 **/
ryuna 0:dfe81bdcb486 278 char Ping[2];
ryuna 0:dfe81bdcb486 279
ryuna 0:dfe81bdcb486 280 PingReceiveRL(Ping);
ryuna 0:dfe81bdcb486 281
ryuna 0:dfe81bdcb486 282
ryuna 0:dfe81bdcb486 283 if(IrMemo[0] == 6){//check ir number
ryuna 0:dfe81bdcb486 284 return ;
ryuna 0:dfe81bdcb486 285 }
ryuna 0:dfe81bdcb486 286 if(Ping[0]<Ping[1]){
ryuna 0:dfe81bdcb486 287 /*右が大きい*/
ryuna 0:dfe81bdcb486 288
ryuna 0:dfe81bdcb486 289
ryuna 0:dfe81bdcb486 290 return;
ryuna 0:dfe81bdcb486 291 }
ryuna 0:dfe81bdcb486 292 /*左が大きい*/
ryuna 0:dfe81bdcb486 293
ryuna 0:dfe81bdcb486 294
ryuna 3:0c994b3a882e 295
ryuna 0:dfe81bdcb486 296 }
ryuna 3:0c994b3a882e 297
ryuna 3:0c994b3a882e 298 void GoHome(int *CompassDef , uint8_t IrMemo[],double ReV)//Ball is not found.
ryuna 0:dfe81bdcb486 299 {
ryuna 3:0c994b3a882e 300 //止まっとく
ryuna 3:0c994b3a882e 301 S555.position(0.0);
ryuna 5:dace4f3b6e4a 302 move(0,0,ReV,0);
ryuna 3:0c994b3a882e 303
ryuna 0:dfe81bdcb486 304 /*line検知をしないバージョン*/
ryuna 3:0c994b3a882e 305 /*
ryuna 0:dfe81bdcb486 306 char Ping[2] = {0};
ryuna 0:dfe81bdcb486 307
ryuna 0:dfe81bdcb486 308
ryuna 0:dfe81bdcb486 309 PingReceiveFB(Ping);
ryuna 0:dfe81bdcb486 310 if(Ping[1] >=60){//後ろからの距離60cm
ryuna 3:0c994b3a882e 311 move(-20,-20,ReV);
ryuna 0:dfe81bdcb486 312 PingReceiveFB(Ping);
ryuna 0:dfe81bdcb486 313 return ;
ryuna 0:dfe81bdcb486 314 }
ryuna 0:dfe81bdcb486 315
ryuna 3:0c994b3a882e 316 move(0,0,ReV);//stop
ryuna 3:0c994b3a882e 317 */
ryuna 3:0c994b3a882e 318 }
ryuna 3:0c994b3a882e 319
ryuna 5:dace4f3b6e4a 320 void PidUpdate(int CompassDef , double SetAngle, double *Pid_up)
ryuna 3:0c994b3a882e 321 {
ryuna 3:0c994b3a882e 322 int Compass;
ryuna 3:0c994b3a882e 323 double inputPID = 0.0;
ryuna 3:0c994b3a882e 324
ryuna 3:0c994b3a882e 325 Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
ryuna 5:dace4f3b6e4a 326 wait_ms(10);
ryuna 3:0c994b3a882e 327 pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0));
ryuna 3:0c994b3a882e 328 inputPID = Compass;
ryuna 3:0c994b3a882e 329
ryuna 3:0c994b3a882e 330 pid.setProcessValue(inputPID);
ryuna 5:dace4f3b6e4a 331 *Pid_up = -(pid.compute());
ryuna 0:dfe81bdcb486 332
ryuna 0:dfe81bdcb486 333 }
ryuna 0:dfe81bdcb486 334 uint8_t SwRead(){
ryuna 0:dfe81bdcb486 335 /******
ryuna 0:dfe81bdcb486 336 *retrun : sw_state
ryuna 3:0c994b3a882e 337 *StartS = 0x01;
ryuna 3:0c994b3a882e 338 *Debug2 = 0x02;
ryuna 0:dfe81bdcb486 339 *Debug1 = 0x04;
ryuna 3:0c994b3a882e 340 *Debug3 = 0x06;
ryuna 3:0c994b3a882e 341 *Kicker = 0x08;
ryuna 3:0c994b3a882e 342 *Calibration = 0x10;
ryuna 3:0c994b3a882e 343
ryuna 0:dfe81bdcb486 344 *
ryuna 0:dfe81bdcb486 345 *****/
ryuna 0:dfe81bdcb486 346 uint8_t i,temp,temp2;
ryuna 3:0c994b3a882e 347 temp = ~Sw - 224;
ryuna 0:dfe81bdcb486 348 if(!(temp == Calibration
ryuna 0:dfe81bdcb486 349 ||temp == Kicker
ryuna 0:dfe81bdcb486 350 ||temp == Debug1
ryuna 0:dfe81bdcb486 351 ||temp == Debug2
ryuna 3:0c994b3a882e 352 //||temp == Debug3
ryuna 0:dfe81bdcb486 353 ||temp == StartS)) return 0;/*スイッチが押されていない*/
ryuna 3:0c994b3a882e 354 if(!(temp == 0x00)){
ryuna 0:dfe81bdcb486 355 for(i = 0; i < 50; i++);
ryuna 3:0c994b3a882e 356 temp2 = ~Sw - 224;
ryuna 0:dfe81bdcb486 357 if(temp == temp2){
ryuna 0:dfe81bdcb486 358 return temp;
ryuna 0:dfe81bdcb486 359 }
ryuna 0:dfe81bdcb486 360 }
ryuna 0:dfe81bdcb486 361 return 0;
ryuna 0:dfe81bdcb486 362 }
ryuna 0:dfe81bdcb486 363
ryuna 0:dfe81bdcb486 364 //通信（モータ用）
ryuna 0:dfe81bdcb486 365 void tx_motor(){
ryuna 0:dfe81bdcb486 366 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:dfe81bdcb486 367 Motor.printf("%s",StringFIN.c_str());
ryuna 5:dace4f3b6e4a 368 S555.position(SetRad);
ryuna 0:dfe81bdcb486 369 }
ryuna 0:dfe81bdcb486 370
ryuna 3:0c994b3a882e 371 void SetUp(int *CompassDef){
ryuna 0:dfe81bdcb486 372 /*初期化*/
ryuna 3:0c994b3a882e 373 double ReV;
ryuna 3:0c994b3a882e 374 int i;
ryuna 0:dfe81bdcb486 375 Kick = 0;
ryuna 0:dfe81bdcb486 376 Sw.mode(PullUp);
ryuna 0:dfe81bdcb486 377
ryuna 3:0c994b3a882e 378 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
ryuna 3:0c994b3a882e 379 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
ryuna 3:0c994b3a882e 380 pid.setBias(PID_BIAS); //pid sed def
ryuna 3:0c994b3a882e 381 pid.setMode(AUTO_MODE); //pid sed def
ryuna 3:0c994b3a882e 382 pid.setSetPoint(REFERENCE); //pid sed def
ryuna 3:0c994b3a882e 383
ryuna 0:dfe81bdcb486 384 Motor.baud(115200); //ボーレート設定
ryuna 0:dfe81bdcb486 385 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:dfe81bdcb486 386 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 5:dace4f3b6e4a 387
ryuna 0:dfe81bdcb486 388
ryuna 0:dfe81bdcb486 389 S555.calibrate(0.0005, 120.0);
ryuna 0:dfe81bdcb486 390 S555.position(0.0); //初期位置にセット
ryuna 5:dace4f3b6e4a 391 move(0,0,0,0);//停止
ryuna 5:dace4f3b6e4a 392 Sensor.frequency(100000);
ryuna 0:dfe81bdcb486 393 hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ryuna 3:0c994b3a882e 394 *CompassDef = (hmc6352.sample() / 10);
ryuna 5:dace4f3b6e4a 395 for(i = 0;i<15;i++){
ryuna 5:dace4f3b6e4a 396 PidUpdate(*CompassDef,0,&ReV);
ryuna 3:0c994b3a882e 397 }
ryuna 3:0c994b3a882e 398
ryuna 3:0c994b3a882e 399 Lcd.printf("%f\n",ReV);
ryuna 3:0c994b3a882e 400 wait_ms(100);
ryuna 0:dfe81bdcb486 401
ryuna 0:dfe81bdcb486 402 }
ryuna 3:0c994b3a882e 403 void StartLoop(int *CompassDef){
ryuna 0:dfe81bdcb486 404 /*
ryuna 0:dfe81bdcb486 405 *スイッチが押されるまでロボットはスタートしない．
ryuna 0:dfe81bdcb486 406 *
ryuna 0:dfe81bdcb486 407 *switch割り当て
ryuna 0:dfe81bdcb486 408 *1.コンパスのキャリブレーション実行スイッチ
ryuna 0:dfe81bdcb486 409 *2.キッカーのキック(check用)
ryuna 0:dfe81bdcb486 410 *3,4.自由
ryuna 0:dfe81bdcb486 411 *5.StartSw
ryuna 0:dfe81bdcb486 412 */
ryuna 0:dfe81bdcb486 413 uint8_t State = 0;
ryuna 3:0c994b3a882e 414 int Compass = 0;
ryuna 3:0c994b3a882e 415 uint8_t IrData[3] = {0};
ryuna 3:0c994b3a882e 416 uint8_t Irnum = 0;
ryuna 3:0c994b3a882e 417 uint8_t LineData = 0;
ryuna 0:dfe81bdcb486 418 while(1){
ryuna 3:0c994b3a882e 419 Led[0] = Led[1] = Led[2] = Led[3] = 1;
ryuna 3:0c994b3a882e 420 //Lcd.cls();
ryuna 0:dfe81bdcb486 421 State = SwRead();
ryuna 0:dfe81bdcb486 422 if(State == 0) continue;
ryuna 0:dfe81bdcb486 423
ryuna 3:0c994b3a882e 424 if(State == StartS){
ryuna 3:0c994b3a882e 425 /*loop end & start*/
ryuna 3:0c994b3a882e 426 return;
ryuna 0:dfe81bdcb486 427 }
ryuna 3:0c994b3a882e 428
ryuna 3:0c994b3a882e 429
ryuna 0:dfe81bdcb486 430 if(State == Debug1){
ryuna 3:0c994b3a882e 431 while((State == Debug1)){
ryuna 3:0c994b3a882e 432 /*Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:0c994b3a882e 433 Lcd.printf("%d\n",Compass);
ryuna 3:0c994b3a882e 434 */
ryuna 3:0c994b3a882e 435
ryuna 3:0c994b3a882e 436
ryuna 3:0c994b3a882e 437 LineData = (~Line+0x00) & 0x0F;
ryuna 3:0c994b3a882e 438 Lcd.printf("%d\n",LineData);
ryuna 3:0c994b3a882e 439
ryuna 3:0c994b3a882e 440 wait_ms(100);
ryuna 3:0c994b3a882e 441 State = SwRead();
ryuna 3:0c994b3a882e 442
ryuna 3:0c994b3a882e 443 }
ryuna 0:dfe81bdcb486 444
ryuna 3:0c994b3a882e 445 Lcd.cls();
ryuna 3:0c994b3a882e 446 continue;
ryuna 0:dfe81bdcb486 447 /* debug command free
ryuna 0:dfe81bdcb486 448 *
ryuna 0:dfe81bdcb486 449 * decide movement of the beginning.
ryuna 0:dfe81bdcb486 450 *
ryuna 3:0c994b3a882e 451 **/
ryuna 3:0c994b3a882e 452 }
ryuna 3:0c994b3a882e 453 if(State == Debug2){
ryuna 3:0c994b3a882e 454 while((State == Debug2)){
ryuna 3:0c994b3a882e 455 Irnum = IrReceive(IrData);
ryuna 3:0c994b3a882e 456 Lcd.printf("%d\n",Irnum);
ryuna 3:0c994b3a882e 457 wait_ms(100);
ryuna 3:0c994b3a882e 458 State = SwRead();
ryuna 3:0c994b3a882e 459 }
ryuna 3:0c994b3a882e 460 Lcd.cls();
ryuna 3:0c994b3a882e 461 continue;
ryuna 3:0c994b3a882e 462
ryuna 3:0c994b3a882e 463 /* debug command free
ryuna 0:dfe81bdcb486 464 *
ryuna 3:0c994b3a882e 465 *display out to selected 3 menus. compass, ir, ping, line,etc
ryuna 3:0c994b3a882e 466 *
ryuna 3:0c994b3a882e 467 **/
ryuna 0:dfe81bdcb486 468 }
ryuna 3:0c994b3a882e 469 /*
ryuna 3:0c994b3a882e 470 if(State == Debug3){
ryuna 3:0c994b3a882e 471 while((State == Debug3)){
ryuna 3:0c994b3a882e 472 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:0c994b3a882e 473 Lcd.printf("%d\n",Compass);
ryuna 3:0c994b3a882e 474 State = SwRead();
ryuna 3:0c994b3a882e 475 wait_ms(100);
ryuna 3:0c994b3a882e 476 }
ryuna 3:0c994b3a882e 477 Lcd.cls();
ryuna 3:0c994b3a882e 478 */
ryuna 3:0c994b3a882e 479 /* debug command free
ryuna 3:0c994b3a882e 480 *
ryuna 3:0c994b3a882e 481 *display out to selected 3 menus. compass, ir, ping, line,etc
ryuna 3:0c994b3a882e 482 *
ryuna 3:0c994b3a882e 483 **/
ryuna 3:0c994b3a882e 484 //}
ryuna 3:0c994b3a882e 485
ryuna 3:0c994b3a882e 486 if(State == Kicker){
ryuna 3:0c994b3a882e 487 while((State == Kicker)){
ryuna 3:0c994b3a882e 488 Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
ryuna 3:0c994b3a882e 489 Lcd.printf("%d\n",Compass);
ryuna 3:0c994b3a882e 490 wait_ms(100);
ryuna 3:0c994b3a882e 491 State = SwRead();
ryuna 3:0c994b3a882e 492
ryuna 3:0c994b3a882e 493 }
ryuna 3:0c994b3a882e 494 Lcd.cls();
ryuna 3:0c994b3a882e 495 /*kicker check */
ryuna 3:0c994b3a882e 496 /*
ryuna 3:0c994b3a882e 497 Led[4] = 1;
ryuna 3:0c994b3a882e 498 Kick = 1;
ryuna 3:0c994b3a882e 499 wait(0.5);
ryuna 3:0c994b3a882e 500 Kick = 0;
ryuna 3:0c994b3a882e 501 Led[4] = 0;
ryuna 3:0c994b3a882e 502 wait(1);
ryuna 3:0c994b3a882e 503 */
ryuna 3:0c994b3a882e 504 continue;
ryuna 0:dfe81bdcb486 505 }
ryuna 3:0c994b3a882e 506 if(State == Calibration){
ryuna 3:0c994b3a882e 507 Led[0] = Led[1] = Led[2] = 0;
ryuna 5:dace4f3b6e4a 508 move(20,20,0,0);
ryuna 5:dace4f3b6e4a 509 while((State == Calibration)){
ryuna 5:dace4f3b6e4a 510 wait_ms(100);
ryuna 5:dace4f3b6e4a 511 State = SwRead();
ryuna 5:dace4f3b6e4a 512
ryuna 5:dace4f3b6e4a 513 }
ryuna 5:dace4f3b6e4a 514 move(0,0,0,0);
ryuna 5:dace4f3b6e4a 515 /*
ryuna 3:0c994b3a882e 516 hmc6352.setCalibrationMode(ENTER);
ryuna 3:0c994b3a882e 517 while((State == Calibration)){
ryuna 3:0c994b3a882e 518 State = SwRead();
ryuna 3:0c994b3a882e 519 }
ryuna 3:0c994b3a882e 520 hmc6352.setCalibrationMode(EXIT);
ryuna 3:0c994b3a882e 521 wait(0.3);//必要
ryuna 3:0c994b3a882e 522 Led[3] = 0;
ryuna 5:dace4f3b6e4a 523 */
ryuna 3:0c994b3a882e 524 /*calibration command enter*/
ryuna 3:0c994b3a882e 525 continue;
ryuna 3:0c994b3a882e 526 }
ryuna 0:dfe81bdcb486 527 }
ryuna 0:dfe81bdcb486 528
ryuna 0:dfe81bdcb486 529 }
ryuna 0:dfe81bdcb486 530 int main() {
ryuna 0:dfe81bdcb486 531
ryuna 0:dfe81bdcb486 532 /*Ir*/
ryuna 3:0c994b3a882e 533 uint8_t IrNum = 12;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
ryuna 3:0c994b3a882e 534 //uint8_t IrNumOld = 0;//過去値
ryuna 0:dfe81bdcb486 535 uint8_t IrData[2];//0:1位*13+2位,1:1位の値
ryuna 3:0c994b3a882e 536
ryuna 0:dfe81bdcb486 537 /*Line*/
ryuna 0:dfe81bdcb486 538 uint8_t LineData = 0;
ryuna 0:dfe81bdcb486 539
ryuna 0:dfe81bdcb486 540 /*Compass*/
ryuna 3:0c994b3a882e 541 int CompassDef = 0;
ryuna 3:0c994b3a882e 542 //int Compass = 0;
ryuna 3:0c994b3a882e 543 double SetAngle = 0;
ryuna 3:0c994b3a882e 544
ryuna 3:0c994b3a882e 545 /*PID補正move加算値 Revise */
ryuna 3:0c994b3a882e 546 double ReV = 0.0;
ryuna 5:dace4f3b6e4a 547 int Rad = 0;
ryuna 0:dfe81bdcb486 548
ryuna 0:dfe81bdcb486 549 /*State */
ryuna 0:dfe81bdcb486 550 //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
ryuna 3:0c994b3a882e 551 uint8_t LineIr = 0;
ryuna 3:0c994b3a882e 552 uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
ryuna 3:0c994b3a882e 553 0x04,0x04,0x08,0x08,0x08,0x01,0x00};
ryuna 3:0c994b3a882e 554
ryuna 5:dace4f3b6e4a 555 int Power = 0;
ryuna 0:dfe81bdcb486 556
ryuna 0:dfe81bdcb486 557 /*関数ポインタ*/
ryuna 3:0c994b3a882e 558 void (*AnotherAction[3])(int *, uint8_t [],double);
ryuna 0:dfe81bdcb486 559 AnotherAction[0] = IrFrontAction;
ryuna 0:dfe81bdcb486 560 AnotherAction[1] = IrBackAction;
ryuna 0:dfe81bdcb486 561 AnotherAction[2] = GoHome;
ryuna 0:dfe81bdcb486 562
ryuna 3:0c994b3a882e 563 /*check 変数*/
ryuna 3:0c994b3a882e 564 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 565
ryuna 3:0c994b3a882e 566 int vr[13] = { 20,20,20,20,-20,-20,-20,-20,-20,20,20,20,0};
ryuna 3:0c994b3a882e 567
ryuna 3:0c994b3a882e 568 int vl[13] = { 20,20,20,20,-20,-20,-20,-20,-20,20,20,20,0};
ryuna 3:0c994b3a882e 569
ryuna 0:dfe81bdcb486 570
ryuna 0:dfe81bdcb486 571 SetUp(&CompassDef);/*set up routine*/
ryuna 3:0c994b3a882e 572 StartLoop(&CompassDef); /*loop strat, switch push end*/
ryuna 0:dfe81bdcb486 573
ryuna 3:0c994b3a882e 574 while(1){
ryuna 4:f7946508daa8 575
ryuna 4:f7946508daa8 576 Led[0] = 1;
ryuna 4:f7946508daa8 577
ryuna 4:f7946508daa8 578 /*白線を読んでいないか確認する*/
ryuna 5:dace4f3b6e4a 579
ryuna 4:f7946508daa8 580 LineData = (~Line+0x00) & 0x0F;
ryuna 4:f7946508daa8 581 if(LineData){
ryuna 5:dace4f3b6e4a 582 //IrNum = IrReceiveFast();
ryuna 5:dace4f3b6e4a 583 //LineIr = LineData & IrChange[IrNum]; //一箇所でも一致すればlineを検知している．
ryuna 5:dace4f3b6e4a 584 while(LineData){
ryuna 5:dace4f3b6e4a 585 move(0,0,0,0);//
ryuna 4:f7946508daa8 586 Led[1] = Led[2] = Led[3] = 1;
ryuna 4:f7946508daa8 587 LineData = (~Line+0x00) & 0x0F;
ryuna 5:dace4f3b6e4a 588 //IrNum = IrReceiveFast();
ryuna 5:dace4f3b6e4a 589 wait_ms(10);
ryuna 5:dace4f3b6e4a 590 //LineIr = LineData & IrChange[IrNum];//一箇所でも一致すればlineを検知している．
ryuna 5:dace4f3b6e4a 591
ryuna 4:f7946508daa8 592 }
ryuna 4:f7946508daa8 593 Led[1] = Led[2] = Led[3] = 0;
ryuna 5:dace4f3b6e4a 594 //wait(0.02);
ryuna 5:dace4f3b6e4a 595
ryuna 4:f7946508daa8 596 }
ryuna 5:dace4f3b6e4a 597
ryuna 5:dace4f3b6e4a 598 Led[2] = 1;
ryuna 4:f7946508daa8 599 IrNum = IrReceive(IrData);
ryuna 5:dace4f3b6e4a 600 wait_ms(100);
ryuna 5:dace4f3b6e4a 601 Led[2] = 0;
ryuna 5:dace4f3b6e4a 602 //PidUpdate(CompassDef ,SetAngle,&ReV);
ryuna 5:dace4f3b6e4a 603 Led[3] = 1;
ryuna 5:dace4f3b6e4a 604 Power = 20;
ryuna 5:dace4f3b6e4a 605 Rad = 359-IrNum*30;
ryuna 5:dace4f3b6e4a 606 fool(&Rad,&Power);
ryuna 5:dace4f3b6e4a 607 move(Power,Power,ReV,Rad);
ryuna 5:dace4f3b6e4a 608 wait_ms(100);
ryuna 5:dace4f3b6e4a 609 Led[3] = 0;
ryuna 4:f7946508daa8 610
ryuna 5:dace4f3b6e4a 611 Led[2] = 1;
ryuna 5:dace4f3b6e4a 612 wait_ms(200);
ryuna 4:f7946508daa8 613
ryuna 5:dace4f3b6e4a 614 Led[0] = Led[1] = Led[2] = Led[3] = 0;
ryuna 5:dace4f3b6e4a 615
ryuna 0:dfe81bdcb486 616 }
ryuna 0:dfe81bdcb486 617
ryuna 3:0c994b3a882e 618 /*
ryuna 3:0c994b3a882e 619 while(0){
ryuna 3:0c994b3a882e 620 //デモプログラム
ryuna 3:0c994b3a882e 621 S555.position(0.0);
ryuna 3:0c994b3a882e 622
ryuna 3:0c994b3a882e 623 wait(1.5);
ryuna 3:0c994b3a882e 624 S555.position(90.0);
ryuna 0:dfe81bdcb486 625
ryuna 3:0c994b3a882e 626 }
ryuna 3:0c994b3a882e 627
ryuna 3:0c994b3a882e 628 */
ryuna 0:dfe81bdcb486 629
ryuna 0:dfe81bdcb486 630
ryuna 0:dfe81bdcb486 631 }