CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver2

2

Dependencies:   AQM0802A HMC6352 PID Servo mbed

main.cpp@3:0c994b3a882e, 2015-03-02 (annotated)

Committer:
ryuna
Date:
Mon Mar 02 05:55:33 2015 +0000
Revision:
3:0c994b3a882e
Parent:
0:dfe81bdcb486
Child:
4:f7946508daa8
?????????

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