Hajime Muraki / Mbed 2 deprecated chronograph

main

Dependencies:   TextLCD mbed PID

main.cpp@3:440e774cc24b, 2014-03-12 (annotated)

Committer:
com3
Date:
Wed Mar 12 04:51:30 2014 +0000
Revision:
3:440e774cc24b
Parent:
2:59edff92b599
Child:
4:536cd493a337
main

Who changed what in which revision?

UserRevisionLine numberNew contents of line
com3 0:e6d14fec4954 1 #include "mbed.h"
com3 0:e6d14fec4954 2 #include "TextLCD.h"
com3 2:59edff92b599 3 #include "PID.h"
com3 0:e6d14fec4954 4 #include "common.h"
com3 0:e6d14fec4954 5 #include <math.h>
com3 0:e6d14fec4954 6 #include <sstream>
com3 0:e6d14fec4954 7
com3 3:440e774cc24b 8 #define MOTOR_P 26
com3 3:440e774cc24b 9 #define HOME_P 25
com3 3:440e774cc24b 10 #define NO_BALL 45
com3 1:fb4277ce4d93 11 #define LINE_LP 30
com3 3:440e774cc24b 12 #define LINE_FP 35
com3 1:fb4277ce4d93 13 #define LINE_ON 0xFFF0
com3 3:440e774cc24b 14 #define LINE_TIME 0.6
com3 3:440e774cc24b 15 #define LINE_PING 40
com3 0:e6d14fec4954 16 #define R 1.0
com3 1:fb4277ce4d93 17 #define S_MAX 5
com3 1:fb4277ce4d93 18 #define S1 15
com3 1:fb4277ce4d93 19 #define S2 10
com3 1:fb4277ce4d93 20 #define S3 5
com3 2:59edff92b599 21 #define RATE 0.05
com3 3:440e774cc24b 22 #define P_GAIN 1.2
com3 2:59edff92b599 23 #define I_GAIN 0.0
com3 3:440e774cc24b 24 #define D_GAIN 0.015
com3 3:440e774cc24b 25 #define HOME_BACK 30
com3 3:440e774cc24b 26 #define HOME 1
com3 3:440e774cc24b 27 #define GOAL 30.0
com3 3:440e774cc24b 28 #define PING_LR 100
com3 1:fb4277ce4d93 29 //誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
com3 1:fb4277ce4d93 30 #define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
com3 0:e6d14fec4954 31
com3 1:fb4277ce4d93 32 DigitalIn sw(p5);
com3 2:59edff92b599 33 DigitalIn start(p7);
com3 0:e6d14fec4954 34 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
com3 0:e6d14fec4954 35 Serial motor(p9,p10);
com3 0:e6d14fec4954 36 Serial sensor(p13,p14);
com3 0:e6d14fec4954 37 Serial pc(USBTX, USBRX);
com3 0:e6d14fec4954 38 TextLCD lcd(p26, p25, p24, p23, p22, p21);
com3 1:fb4277ce4d93 39 AnalogIn adcline[4] = {p16, p17, p19, p20};
com3 2:59edff92b599 40 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
com3 1:fb4277ce4d93 41 Timeout liner0;
com3 1:fb4277ce4d93 42 Timeout liner1;
com3 1:fb4277ce4d93 43 Timeout liner2;
com3 1:fb4277ce4d93 44 Timeout liner3;
com3 2:59edff92b599 45 Ticker pidupdata;
com3 0:e6d14fec4954 46 //HMC6352 dcompass(p9,p10);
com3 0:e6d14fec4954 47
com3 0:e6d14fec4954 48 extern string StringFIN;
com3 0:e6d14fec4954 49 extern void array(int,int,int,int);
com3 0:e6d14fec4954 50 extern void micon_rx(void);
com3 0:e6d14fec4954 51
com3 0:e6d14fec4954 52 //uint16_t analogHex[4] = {0};
com3 0:e6d14fec4954 53 int speed[4] = {0};
com3 3:440e774cc24b 54 uint8_t value_ir = 0, ir_num = 0, ir_dis = 0, ball_on = 0;
com3 0:e6d14fec4954 55 uint8_t ping[4] = {0};
com3 3:440e774cc24b 56 uint8_t line[4] = {0}, line_stop[4] = {0}, line_ping[4] = {0};
com3 3:440e774cc24b 57 uint8_t kick = 0;
com3 3:440e774cc24b 58 //uint8_t robot_state = 0;
com3 0:e6d14fec4954 59 int compass = 0;
com3 0:e6d14fec4954 60 int x = 0, y = 0, s = 0, i = 0, line_on = 0;
com3 0:e6d14fec4954 61 int compassdef = 0, data = 0;
com3 1:fb4277ce4d93 62 uint8_t pingdef[4] = {0};
com3 1:fb4277ce4d93 63
com3 2:59edff92b599 64 double pidinput = 180.0;
com3 3:440e774cc24b 65 double compasspid = 0.0;
com3 3:440e774cc24b 66 double goal = 0.0;
com3 2:59edff92b599 67
com3 3:440e774cc24b 68 double way[10][2] = {
com3 3:440e774cc24b 69 { 0 , 1 }, //FF
com3 3:440e774cc24b 70 {-0.707, 0.707}, //FL
com3 3:440e774cc24b 71 {-1 , 0 }, //LL
com3 3:440e774cc24b 72 {-0.707,-0.707}, //BL
com3 3:440e774cc24b 73 { 0 ,-1 }, //BB
com3 3:440e774cc24b 74 { 0.707,-0.707}, //BR
com3 3:440e774cc24b 75 { 1 , 0 }, //RR
com3 3:440e774cc24b 76 { 0.707, 0.707}, //FR
com3 3:440e774cc24b 77 {-0.500, 0.866}, //FFL
com3 3:440e774cc24b 78 { 0.500, 0.866} //FFR
com3 3:440e774cc24b 79 };
com3 3:440e774cc24b 80 double ball_way[12][2] = {
com3 3:440e774cc24b 81 { 0.0 , 1.0 }, //FF
com3 3:440e774cc24b 82 {-0.985,-0.173}, //FL
com3 3:440e774cc24b 83 {-0.342,-0.940}, //LL
com3 3:440e774cc24b 84 { 0.342,-0.940}, //BL
com3 3:440e774cc24b 85 { 0.966, 0.259}, //BB
com3 3:440e774cc24b 86 {-0.342,-0.940}, //BR
com3 3:440e774cc24b 87 { 0.342,-0.940}, //RR
com3 3:440e774cc24b 88 { 0.985,-0.173}, //FR
com3 3:440e774cc24b 89 {-0.985, 0.174}, //FFL
com3 3:440e774cc24b 90 { 0.985, 0.174}, //FFR
com3 3:440e774cc24b 91 {-0.173, 0.985}, //FFFL
com3 3:440e774cc24b 92 { 0.173, 0.985} //FFFR
com3 1:fb4277ce4d93 93 };
com3 0:e6d14fec4954 94
com3 3:440e774cc24b 95 double all_way[10][2] = {
com3 3:440e774cc24b 96 { 0 , 1 }, //FF
com3 3:440e774cc24b 97 {-0.707, 0.707}, //FL
com3 3:440e774cc24b 98 {-0.939,-0.342}, //LL {-1 , 0 },
com3 3:440e774cc24b 99 {-0.707,-0.707}, //BL
com3 3:440e774cc24b 100 { 0 ,-1 }, //BB
com3 3:440e774cc24b 101 { 0.707,-0.707}, //BR
com3 3:440e774cc24b 102 { 0.939,-0.342}, //RR { 1 , 0 },
com3 3:440e774cc24b 103 { 0.707, 0.707}, //FR
com3 3:440e774cc24b 104 {-0.500, 0.866}, //FFL
com3 3:440e774cc24b 105 { 0.500, 0.866} //FFR
com3 3:440e774cc24b 106 };
com3 0:e6d14fec4954 107
com3 0:e6d14fec4954 108 void move(int vx, int vy, int vs){
com3 0:e6d14fec4954 109 double pwm[4] = {0};
com3 0:e6d14fec4954 110
com3 0:e6d14fec4954 111 pwm[0] = (double)((vx) + vs);
com3 0:e6d14fec4954 112 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 113 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
com3 3:440e774cc24b 114 pwm[3] = kick;
com3 0:e6d14fec4954 115
com3 0:e6d14fec4954 116 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 117 if(pwm[i] > 100){
com3 0:e6d14fec4954 118 pwm[i] = 100;
com3 0:e6d14fec4954 119 } else if(pwm[i] < -100){
com3 0:e6d14fec4954 120 pwm[i] = -100;
com3 0:e6d14fec4954 121 }
com3 0:e6d14fec4954 122 speed[i] = pwm[i];
com3 0:e6d14fec4954 123 }
com3 0:e6d14fec4954 124 }
com3 0:e6d14fec4954 125
com3 0:e6d14fec4954 126 //通信（モータ用）
com3 0:e6d14fec4954 127 void tx_motor(){
com3 0:e6d14fec4954 128 array(speed[0],speed[1],speed[3],speed[2]);
com3 0:e6d14fec4954 129 motor.printf("%s",StringFIN.c_str());
com3 0:e6d14fec4954 130 }
com3 0:e6d14fec4954 131
com3 0:e6d14fec4954 132 //ライン判断
com3 3:440e774cc24b 133 void line_state(){
com3 3:440e774cc24b 134 if(line[LEFT]){
com3 3:440e774cc24b 135 x = LINE_LP;
com3 3:440e774cc24b 136 } else if(line_ping[LEFT]){
com3 3:440e774cc24b 137 if(ping[LEFT] < LINE_PING){
com3 3:440e774cc24b 138 if(x < 10){
com3 3:440e774cc24b 139 x = 10;
com3 3:440e774cc24b 140 }
com3 3:440e774cc24b 141 } else {
com3 3:440e774cc24b 142 line_ping[LEFT] = 0;
com3 3:440e774cc24b 143 myled[1] = 0;
com3 3:440e774cc24b 144 }
com3 0:e6d14fec4954 145 }
com3 3:440e774cc24b 146
com3 3:440e774cc24b 147 if(line[BACK]){
com3 1:fb4277ce4d93 148 y = LINE_FP;
com3 3:440e774cc24b 149 } else if(line_ping[BACK]){
com3 3:440e774cc24b 150 if(ping[BACK] < LINE_PING){
com3 3:440e774cc24b 151 if(y < 10){
com3 3:440e774cc24b 152 y = 10;
com3 3:440e774cc24b 153 }
com3 3:440e774cc24b 154 } else {
com3 3:440e774cc24b 155 line_ping[BACK] = 0;
com3 3:440e774cc24b 156 myled[2] = 0;
com3 3:440e774cc24b 157 }
com3 0:e6d14fec4954 158 }
com3 3:440e774cc24b 159
com3 3:440e774cc24b 160 if(line[RIGHT]){
com3 1:fb4277ce4d93 161 x = -LINE_LP;
com3 3:440e774cc24b 162 } else if(line_ping[RIGHT]){
com3 3:440e774cc24b 163 if(ping[RIGHT] < LINE_PING){
com3 3:440e774cc24b 164 if(x > 10){
com3 3:440e774cc24b 165 x = -10;
com3 3:440e774cc24b 166 }
com3 3:440e774cc24b 167 } else {
com3 3:440e774cc24b 168 line_ping[RIGHT] = 0;
com3 3:440e774cc24b 169 myled[3] = 0;
com3 3:440e774cc24b 170 }
com3 0:e6d14fec4954 171 }
com3 3:440e774cc24b 172
com3 3:440e774cc24b 173 if(line[FRONT]){
com3 3:440e774cc24b 174 y = -LINE_FP;
com3 3:440e774cc24b 175 } else if(line_ping[FRONT]){
com3 3:440e774cc24b 176 if(ping[FRONT] < LINE_PING){
com3 3:440e774cc24b 177 if(y > 10){
com3 3:440e774cc24b 178 y = -10;
com3 3:440e774cc24b 179 }
com3 3:440e774cc24b 180 } else {
com3 3:440e774cc24b 181 line_ping[FRONT] = 0;
com3 3:440e774cc24b 182 myled[0] = 0;
com3 3:440e774cc24b 183 }
com3 3:440e774cc24b 184 }
com3 3:440e774cc24b 185
com3 1:fb4277ce4d93 186 }
com3 1:fb4277ce4d93 187
com3 1:fb4277ce4d93 188 void lcd_ping(){
com3 1:fb4277ce4d93 189 lcd.cls();
com3 1:fb4277ce4d93 190 lcd.locate(0,0);
com3 1:fb4277ce4d93 191 lcd.printf("%03d %03d\n%03d %03d", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]);
com3 1:fb4277ce4d93 192 }
com3 1:fb4277ce4d93 193
com3 1:fb4277ce4d93 194 void lcd_line(){
com3 1:fb4277ce4d93 195 lcd.cls();
com3 1:fb4277ce4d93 196 lcd.locate(0,0);
com3 1:fb4277ce4d93 197 lcd.printf("%03d %03d\n%03d %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]);
com3 1:fb4277ce4d93 198 }
com3 1:fb4277ce4d93 199
com3 1:fb4277ce4d93 200 void line_stop0(){
com3 3:440e774cc24b 201 line_stop[FRONT] = 0;
com3 1:fb4277ce4d93 202 }
com3 1:fb4277ce4d93 203 void line_stop1(){
com3 3:440e774cc24b 204 line_stop[LEFT] = 0;
com3 1:fb4277ce4d93 205 }
com3 1:fb4277ce4d93 206 void line_stop2(){
com3 3:440e774cc24b 207 line_stop[BACK] = 0;
com3 1:fb4277ce4d93 208 }
com3 1:fb4277ce4d93 209 void line_stop3(){
com3 3:440e774cc24b 210 line_stop[RIGHT] = 0;
com3 2:59edff92b599 211 }
com3 2:59edff92b599 212
com3 2:59edff92b599 213 void line_check()
com3 2:59edff92b599 214 {
com3 3:440e774cc24b 215 if(!line_stop[FRONT]){
com3 3:440e774cc24b 216 if(adcline[FRONT].read_u16() > LINE_ON){
com3 3:440e774cc24b 217 line[FRONT] = 1;
com3 3:440e774cc24b 218 line_stop[FRONT] = 1;
com3 3:440e774cc24b 219 line_ping[FRONT] = 1;
com3 2:59edff92b599 220 myled[0] = 1;
com3 3:440e774cc24b 221 liner0.attach(&line_stop0,1);
com3 2:59edff92b599 222 } else {
com3 3:440e774cc24b 223 line[FRONT] = 0;
com3 3:440e774cc24b 224 ////myled[0] = 0;
com3 2:59edff92b599 225 }
com3 2:59edff92b599 226 }
com3 3:440e774cc24b 227 if(!line_stop[LEFT]){
com3 3:440e774cc24b 228 if(adcline[LEFT].read_u16() > LINE_ON){
com3 3:440e774cc24b 229 line[LEFT] = 1;
com3 3:440e774cc24b 230 line_stop[LEFT] = 1;
com3 3:440e774cc24b 231 line_ping[LEFT] = 1;
com3 2:59edff92b599 232 myled[1] = 1;
com3 2:59edff92b599 233 liner1.attach(&line_stop1,LINE_TIME);
com3 2:59edff92b599 234 } else {
com3 3:440e774cc24b 235 line[LEFT] = 0;
com3 3:440e774cc24b 236 ////myled[1] = 0;
com3 2:59edff92b599 237 }
com3 2:59edff92b599 238 }
com3 3:440e774cc24b 239 if(!line_stop[BACK]){
com3 3:440e774cc24b 240 if(adcline[BACK].read_u16() > LINE_ON){
com3 3:440e774cc24b 241 line[BACK] = 1;
com3 3:440e774cc24b 242 line_stop[BACK] = 1;
com3 3:440e774cc24b 243 line_stop[FRONT] = 1;
com3 3:440e774cc24b 244 line_ping[BACK] = 1;
com3 2:59edff92b599 245 myled[2] = 1;
com3 3:440e774cc24b 246 liner0.attach(&line_stop0,LINE_TIME);
com3 2:59edff92b599 247 liner2.attach(&line_stop2,LINE_TIME);
com3 2:59edff92b599 248 } else {
com3 3:440e774cc24b 249 line[BACK] = 0;
com3 3:440e774cc24b 250 ////myled[2] = 0;
com3 2:59edff92b599 251 }
com3 2:59edff92b599 252 }
com3 3:440e774cc24b 253 if(!line_stop[RIGHT]){
com3 3:440e774cc24b 254 if(adcline[RIGHT].read_u16() > LINE_ON){
com3 3:440e774cc24b 255 line[RIGHT] = 1;
com3 3:440e774cc24b 256 line_stop[RIGHT] = 1;
com3 3:440e774cc24b 257 line_ping[RIGHT] = 1;
com3 2:59edff92b599 258 myled[3] = 1;
com3 2:59edff92b599 259 liner3.attach(&line_stop3,LINE_TIME);
com3 2:59edff92b599 260 } else {
com3 3:440e774cc24b 261 line[RIGHT] = 0;
com3 3:440e774cc24b 262 ////myled[3] = 0;
com3 2:59edff92b599 263 }
com3 2:59edff92b599 264 }
com3 2:59edff92b599 265 }
com3 2:59edff92b599 266
com3 2:59edff92b599 267
com3 2:59edff92b599 268 void pidupdate()
com3 2:59edff92b599 269 {
com3 3:440e774cc24b 270 pid.setSetPoint(180.0 + goal);
com3 2:59edff92b599 271 pidinput = compass;
com3 2:59edff92b599 272 pid.setProcessValue(pidinput);
com3 2:59edff92b599 273
com3 2:59edff92b599 274 compasspid = -pid.compute();
com3 2:59edff92b599 275 }
com3 1:fb4277ce4d93 276
com3 3:440e774cc24b 277 void home()
com3 3:440e774cc24b 278 {
com3 3:440e774cc24b 279 x = 0;
com3 3:440e774cc24b 280 y = 0;
com3 3:440e774cc24b 281 kick = 0;
com3 3:440e774cc24b 282 goal = 0.0;
com3 3:440e774cc24b 283
com3 3:440e774cc24b 284 // if((145 < ping[LEFT]+ping[RIGHT]) && (ping[LEFT]+ping[RIGHT] < 155)){
com3 3:440e774cc24b 285 if(ping[LEFT] > PING_LR){
com3 3:440e774cc24b 286 x = -HOME_P;
com3 3:440e774cc24b 287 y = -5;
com3 3:440e774cc24b 288 } else if(ping[RIGHT] > PING_LR){
com3 3:440e774cc24b 289 x = HOME_P;
com3 3:440e774cc24b 290 y = -5;
com3 3:440e774cc24b 291 }
com3 3:440e774cc24b 292 // }
com3 3:440e774cc24b 293
com3 3:440e774cc24b 294 if(ping[BACK] > HOME_BACK){
com3 3:440e774cc24b 295 y = -HOME_P;
com3 3:440e774cc24b 296 } else if(ping[BACK] < 5){
com3 3:440e774cc24b 297 y = HOME_P;
com3 3:440e774cc24b 298 }
com3 3:440e774cc24b 299 }
com3 3:440e774cc24b 300
com3 0:e6d14fec4954 301 int main() {
com3 0:e6d14fec4954 302
com3 0:e6d14fec4954 303 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
com3 3:440e774cc24b 304 //uint8_t num = 0;
com3 3:440e774cc24b 305 uint8_t dir = 0, power = 0;
com3 0:e6d14fec4954 306
com3 0:e6d14fec4954 307 wait(1);
com3 0:e6d14fec4954 308
com3 0:e6d14fec4954 309 motor.baud(115200); //ボーレート設定
com3 0:e6d14fec4954 310 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
com3 0:e6d14fec4954 311 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
com3 0:e6d14fec4954 312 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
com3 0:e6d14fec4954 313 //compassdef = (compass / 10); //コンパス初期値保存
com3 0:e6d14fec4954 314 //compassdef = (dcompass.sample() / 10);
com3 2:59edff92b599 315 pid.setInputLimits(0.0,360.0);
com3 2:59edff92b599 316 pid.setOutputLimits(-30.0,30.0);
com3 2:59edff92b599 317 pid.setBias(0.0);
com3 2:59edff92b599 318 pid.setMode(AUTO_MODE);
com3 2:59edff92b599 319 pid.setSetPoint(180.0);
com3 2:59edff92b599 320 pidupdata.attach(&pidupdate, 0.05);
com3 0:e6d14fec4954 321
com3 1:fb4277ce4d93 322 sw.mode(PullUp);
com3 1:fb4277ce4d93 323 start.mode(PullUp);
com3 0:e6d14fec4954 324
com3 3:440e774cc24b 325 lcd.cls();
com3 3:440e774cc24b 326 lcd.locate(0,0);
com3 3:440e774cc24b 327 lcd.printf("Chronograph");
com3 2:59edff92b599 328
com3 3:440e774cc24b 329
com3 3:440e774cc24b 330 myled[0] = 1;
com3 3:440e774cc24b 331 while(!start){
com3 3:440e774cc24b 332 lcd.locate(6,1);
com3 3:440e774cc24b 333 lcd.printf("%d", compass);
com3 2:59edff92b599 334 wait(0.1);
com3 2:59edff92b599 335 }
com3 3:440e774cc24b 336 myled[0] = 0;
com3 3:440e774cc24b 337 lcd.cls();
com3 3:440e774cc24b 338 lcd.locate(0,0);
com3 3:440e774cc24b 339 lcd.printf("Chronograph");
com3 3:440e774cc24b 340 lcd.locate(7,1);
com3 3:440e774cc24b 341 lcd.printf("ABURASOBA");
com3 3:440e774cc24b 342
com3 3:440e774cc24b 343
com3 2:59edff92b599 344 while(1){
com3 3:440e774cc24b 345
com3 3:440e774cc24b 346 x = 0;
com3 3:440e774cc24b 347 y = 0;
com3 3:440e774cc24b 348 s = compasspid;
com3 3:440e774cc24b 349 power = MOTOR_P;
com3 3:440e774cc24b 350
com3 3:440e774cc24b 351 if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){
com3 3:440e774cc24b 352 kick = 25;
com3 3:440e774cc24b 353 //if(130 < ping[LEFT]+ping[RIGHT]){
com3 3:440e774cc24b 354 //if(goal == 0){
com3 3:440e774cc24b 355 /* if(ping[LEFT] > 100){
com3 3:440e774cc24b 356 goal = -GOAL;
com3 3:440e774cc24b 357 }*//* else if(ping[RIGHT] > 100){
com3 3:440e774cc24b 358 //goal = GOAL;
com3 3:440e774cc24b 359 }*/
com3 3:440e774cc24b 360 //}
com3 3:440e774cc24b 361 //}
com3 3:440e774cc24b 362
com3 3:440e774cc24b 363 } else {
com3 3:440e774cc24b 364 kick = 0;
com3 3:440e774cc24b 365 goal = 0.0;
com3 3:440e774cc24b 366 }
com3 3:440e774cc24b 367
com3 3:440e774cc24b 368 if((ir_dis > 50) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){
com3 3:440e774cc24b 369 x = power * way[ir_num][0];
com3 3:440e774cc24b 370 y = power * way[ir_num][1];
com3 3:440e774cc24b 371 } else {
com3 3:440e774cc24b 372 if((ir_num == FF) || (ir_num == FFFL) || (ir_num == FFFR)){
com3 3:440e774cc24b 373 power = MOTOR_P + 5;
com3 3:440e774cc24b 374 } else if((ir_num == FFL) || (ir_num == FFR)){
com3 3:440e774cc24b 375 power = MOTOR_P;
com3 3:440e774cc24b 376 } else {
com3 3:440e774cc24b 377 power = MOTOR_P + 5;
com3 3:440e774cc24b 378 }
com3 3:440e774cc24b 379 x = power * ball_way[ir_num][0];
com3 3:440e774cc24b 380 y = power * ball_way[ir_num][1];
com3 3:440e774cc24b 381 if(ir_num == BB){
com3 3:440e774cc24b 382 if(ping[LEFT] > ping[RIGHT]){
com3 3:440e774cc24b 383 x = -x;
com3 3:440e774cc24b 384 }
com3 3:440e774cc24b 385 }
com3 3:440e774cc24b 386 }
com3 3:440e774cc24b 387
com3 2:59edff92b599 388 line_check();
com3 2:59edff92b599 389
com3 3:440e774cc24b 390 if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
com3 2:59edff92b599 391 home();
com3 2:59edff92b599 392 }
com3 3:440e774cc24b 393
com3 3:440e774cc24b 394 line_state();
com3 2:59edff92b599 395
com3 3:440e774cc24b 396 if((x == 0) && (y == 0)){
com3 3:440e774cc24b 397 if(ping[BACK] > 25){
com3 3:440e774cc24b 398 y = -HOME_P;
com3 3:440e774cc24b 399 }
com3 3:440e774cc24b 400 }
com3 3:440e774cc24b 401
com3 3:440e774cc24b 402 //x = 0; y = 0;
com3 2:59edff92b599 403
com3 2:59edff92b599 404 move(x,y,s);
com3 2:59edff92b599 405 }
com3 0:e6d14fec4954 406 }