Hajime Muraki / Mbed 2 deprecated chronograph

main

Dependencies:   TextLCD mbed PID

main.cpp@5:6604ec9044a0, 2014-03-17 (annotated)

Committer:
com3
Date:
Mon Mar 17 06:37:37 2014 +0000
Revision:
5:6604ec9044a0
Parent:
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 4:536cd493a337 8 #define MOTOR_P 35
com3 4:536cd493a337 9 #define HOME_P 30
com3 4:536cd493a337 10 #define TURN_P 20
com3 4:536cd493a337 11 #define BALL_P 25
com3 3:440e774cc24b 12 #define NO_BALL 45
com3 4:536cd493a337 13 #define LINE_P 30
com3 4:536cd493a337 14 #define LINE_PLR 17
com3 4:536cd493a337 15 #define LINE_ON 0x2710 //10000
com3 3:440e774cc24b 16 #define LINE_TIME 0.6
com3 3:440e774cc24b 17 #define LINE_PING 40
com3 0:e6d14fec4954 18 #define R 1.0
com3 1:fb4277ce4d93 19 #define S_MAX 5
com3 1:fb4277ce4d93 20 #define S1 15
com3 1:fb4277ce4d93 21 #define S2 10
com3 1:fb4277ce4d93 22 #define S3 5
com3 2:59edff92b599 23 #define RATE 0.05
com3 4:536cd493a337 24 #define P_GAIN 1.7
com3 2:59edff92b599 25 #define I_GAIN 0.0
com3 4:536cd493a337 26 #define D_GAIN 0.013
com3 3:440e774cc24b 27 #define HOME_BACK 30
com3 3:440e774cc24b 28 #define HOME 1
com3 4:536cd493a337 29 #define KICK 25
com3 3:440e774cc24b 30 #define GOAL 30.0
com3 3:440e774cc24b 31 #define PING_LR 100
com3 4:536cd493a337 32 #define BALL_FAR 43
com3 4:536cd493a337 33 #define BALL_NEAR 40
com3 0:e6d14fec4954 34
com3 4:536cd493a337 35 DigitalIn sw_wh(p5);
com3 2:59edff92b599 36 DigitalIn start(p7);
com3 0:e6d14fec4954 37 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
com3 0:e6d14fec4954 38 Serial motor(p9,p10);
com3 0:e6d14fec4954 39 Serial sensor(p13,p14);
com3 4:536cd493a337 40 Serial xbee(p28,p27);
com3 0:e6d14fec4954 41 Serial pc(USBTX, USBRX);
com3 0:e6d14fec4954 42 TextLCD lcd(p26, p25, p24, p23, p22, p21);
com3 1:fb4277ce4d93 43 AnalogIn adcline[4] = {p16, p17, p19, p20};
com3 2:59edff92b599 44 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
com3 4:536cd493a337 45 Timeout liner_F;
com3 4:536cd493a337 46 Timeout liner_L;
com3 4:536cd493a337 47 Timeout liner_B;
com3 4:536cd493a337 48 Timeout liner_R;
com3 2:59edff92b599 49 Ticker pidupdata;
com3 4:536cd493a337 50 Ticker xbeetx;
com3 5:6604ec9044a0 51 Ticker xbeerx;
com3 0:e6d14fec4954 52 //HMC6352 dcompass(p9,p10);
com3 0:e6d14fec4954 53
com3 0:e6d14fec4954 54 extern string StringFIN;
com3 0:e6d14fec4954 55 extern void array(int,int,int,int);
com3 4:536cd493a337 56 extern void xbee_tx(void);
com3 4:536cd493a337 57 extern void xbee_rx(void);
com3 0:e6d14fec4954 58 extern void micon_rx(void);
com3 0:e6d14fec4954 59
com3 0:e6d14fec4954 60 //uint16_t analogHex[4] = {0};
com3 0:e6d14fec4954 61 int speed[4] = {0};
com3 3:440e774cc24b 62 uint8_t value_ir = 0, ir_num = 0, ir_dis = 0, ball_on = 0;
com3 0:e6d14fec4954 63 uint8_t ping[4] = {0};
com3 3:440e774cc24b 64 uint8_t line[4] = {0}, line_stop[4] = {0}, line_ping[4] = {0};
com3 3:440e774cc24b 65 uint8_t kick = 0;
com3 3:440e774cc24b 66 //uint8_t robot_state = 0;
com3 0:e6d14fec4954 67 int compass = 0;
com3 0:e6d14fec4954 68 int x = 0, y = 0, s = 0, i = 0, line_on = 0;
com3 0:e6d14fec4954 69 int compassdef = 0, data = 0;
com3 1:fb4277ce4d93 70 uint8_t pingdef[4] = {0};
com3 1:fb4277ce4d93 71
com3 2:59edff92b599 72 double pidinput = 180.0;
com3 3:440e774cc24b 73 double compasspid = 0.0;
com3 3:440e774cc24b 74 double goal = 0.0;
com3 2:59edff92b599 75
com3 3:440e774cc24b 76 double way[10][2] = {
com3 3:440e774cc24b 77 { 0 , 1 }, //FF
com3 3:440e774cc24b 78 {-0.707, 0.707}, //FL
com3 3:440e774cc24b 79 {-1 , 0 }, //LL
com3 3:440e774cc24b 80 {-0.707,-0.707}, //BL
com3 3:440e774cc24b 81 { 0 ,-1 }, //BB
com3 3:440e774cc24b 82 { 0.707,-0.707}, //BR
com3 3:440e774cc24b 83 { 1 , 0 }, //RR
com3 3:440e774cc24b 84 { 0.707, 0.707}, //FR
com3 3:440e774cc24b 85 {-0.500, 0.866}, //FFL
com3 3:440e774cc24b 86 { 0.500, 0.866} //FFR
com3 3:440e774cc24b 87 };
com3 3:440e774cc24b 88 double ball_way[12][2] = {
com3 3:440e774cc24b 89 { 0.0 , 1.0 }, //FF
com3 4:536cd493a337 90 {-0.866,-0.500}, //FL
com3 3:440e774cc24b 91 {-0.342,-0.940}, //LL
com3 3:440e774cc24b 92 { 0.342,-0.940}, //BL
com3 3:440e774cc24b 93 { 0.966, 0.259}, //BB
com3 3:440e774cc24b 94 {-0.342,-0.940}, //BR
com3 3:440e774cc24b 95 { 0.342,-0.940}, //RR
com3 3:440e774cc24b 96 { 0.985,-0.173}, //FR
com3 3:440e774cc24b 97 {-0.985, 0.174}, //FFL
com3 3:440e774cc24b 98 { 0.985, 0.174}, //FFR
com3 3:440e774cc24b 99 {-0.173, 0.985}, //FFFL
com3 3:440e774cc24b 100 { 0.173, 0.985} //FFFR
com3 1:fb4277ce4d93 101 };
com3 0:e6d14fec4954 102
com3 0:e6d14fec4954 103 void move(int vx, int vy, int vs){
com3 0:e6d14fec4954 104 double pwm[4] = {0};
com3 0:e6d14fec4954 105
com3 0:e6d14fec4954 106 pwm[0] = (double)((vx) + vs);
com3 0:e6d14fec4954 107 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
com3 0:e6d14fec4954 108 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
com3 3:440e774cc24b 109 pwm[3] = kick;
com3 0:e6d14fec4954 110
com3 0:e6d14fec4954 111 for(i = 0; i < 4; i++){
com3 0:e6d14fec4954 112 if(pwm[i] > 100){
com3 0:e6d14fec4954 113 pwm[i] = 100;
com3 0:e6d14fec4954 114 } else if(pwm[i] < -100){
com3 0:e6d14fec4954 115 pwm[i] = -100;
com3 0:e6d14fec4954 116 }
com3 0:e6d14fec4954 117 speed[i] = pwm[i];
com3 0:e6d14fec4954 118 }
com3 0:e6d14fec4954 119 }
com3 0:e6d14fec4954 120
com3 0:e6d14fec4954 121 //通信（モータ用）
com3 0:e6d14fec4954 122 void tx_motor(){
com3 0:e6d14fec4954 123 array(speed[0],speed[1],speed[3],speed[2]);
com3 0:e6d14fec4954 124 motor.printf("%s",StringFIN.c_str());
com3 0:e6d14fec4954 125 }
com3 0:e6d14fec4954 126
com3 0:e6d14fec4954 127 //ライン判断
com3 3:440e774cc24b 128 void line_state(){
com3 3:440e774cc24b 129 if(line[LEFT]){
com3 4:536cd493a337 130 x = LINE_P;
com3 4:536cd493a337 131 } else if(line[RIGHT]){
com3 4:536cd493a337 132 x = -LINE_P;
com3 0:e6d14fec4954 133 }
com3 4:536cd493a337 134
com3 4:536cd493a337 135 if(line[FRONT]){
com3 4:536cd493a337 136 if(line[LEFT] | line[RIGHT]){
com3 4:536cd493a337 137 y = -LINE_PLR;
com3 3:440e774cc24b 138 } else {
com3 4:536cd493a337 139 y = -LINE_P;
com3 3:440e774cc24b 140 }
com3 4:536cd493a337 141 } else if(line[BACK]){
com3 4:536cd493a337 142 if(line[LEFT] | line[RIGHT]){
com3 4:536cd493a337 143 y = LINE_PLR;
com3 3:440e774cc24b 144 } else {
com3 4:536cd493a337 145 y = LINE_P;
com3 3:440e774cc24b 146 }
com3 0:e6d14fec4954 147 }
com3 1:fb4277ce4d93 148 }
com3 1:fb4277ce4d93 149
com3 4:536cd493a337 150 void line_stop_F(){
com3 3:440e774cc24b 151 line_stop[FRONT] = 0;
com3 1:fb4277ce4d93 152 }
com3 4:536cd493a337 153 void line_stop_L(){
com3 3:440e774cc24b 154 line_stop[LEFT] = 0;
com3 1:fb4277ce4d93 155 }
com3 4:536cd493a337 156 void line_stop_B(){
com3 3:440e774cc24b 157 line_stop[BACK] = 0;
com3 1:fb4277ce4d93 158 }
com3 4:536cd493a337 159 void line_stop_R(){
com3 3:440e774cc24b 160 line_stop[RIGHT] = 0;
com3 2:59edff92b599 161 }
com3 2:59edff92b599 162
com3 2:59edff92b599 163 void line_check()
com3 2:59edff92b599 164 {
com3 3:440e774cc24b 165 if(!line_stop[FRONT]){
com3 3:440e774cc24b 166 if(adcline[FRONT].read_u16() > LINE_ON){
com3 3:440e774cc24b 167 line[FRONT] = 1;
com3 3:440e774cc24b 168 line_stop[FRONT] = 1;
com3 4:536cd493a337 169 line_stop[BACK] = 1;
com3 4:536cd493a337 170 //line_ping[FRONT] = 1;
com3 2:59edff92b599 171 myled[0] = 1;
com3 4:536cd493a337 172 liner_F.attach(&line_stop_F,LINE_TIME);
com3 4:536cd493a337 173 liner_B.attach(&line_stop_B,LINE_TIME);
com3 2:59edff92b599 174 } else {
com3 3:440e774cc24b 175 line[FRONT] = 0;
com3 4:536cd493a337 176 myled[0] = 0;
com3 2:59edff92b599 177 }
com3 2:59edff92b599 178 }
com3 3:440e774cc24b 179 if(!line_stop[LEFT]){
com3 3:440e774cc24b 180 if(adcline[LEFT].read_u16() > LINE_ON){
com3 3:440e774cc24b 181 line[LEFT] = 1;
com3 3:440e774cc24b 182 line_stop[LEFT] = 1;
com3 4:536cd493a337 183 line_stop[RIGHT] = 1;
com3 4:536cd493a337 184 //line_ping[LEFT] = 1;
com3 2:59edff92b599 185 myled[1] = 1;
com3 4:536cd493a337 186 liner_L.attach(&line_stop_L,LINE_TIME);
com3 4:536cd493a337 187 liner_R.attach(&line_stop_R,LINE_TIME);
com3 2:59edff92b599 188 } else {
com3 3:440e774cc24b 189 line[LEFT] = 0;
com3 4:536cd493a337 190 myled[1] = 0;
com3 2:59edff92b599 191 }
com3 2:59edff92b599 192 }
com3 3:440e774cc24b 193 if(!line_stop[BACK]){
com3 3:440e774cc24b 194 if(adcline[BACK].read_u16() > LINE_ON){
com3 3:440e774cc24b 195 line[BACK] = 1;
com3 3:440e774cc24b 196 line_stop[BACK] = 1;
com3 3:440e774cc24b 197 line_stop[FRONT] = 1;
com3 4:536cd493a337 198 //line_ping[BACK] = 1;
com3 2:59edff92b599 199 myled[2] = 1;
com3 4:536cd493a337 200 liner_B.attach(&line_stop_B,LINE_TIME);
com3 4:536cd493a337 201 liner_F.attach(&line_stop_F,LINE_TIME);
com3 2:59edff92b599 202 } else {
com3 3:440e774cc24b 203 line[BACK] = 0;
com3 4:536cd493a337 204 myled[2] = 0;
com3 2:59edff92b599 205 }
com3 2:59edff92b599 206 }
com3 3:440e774cc24b 207 if(!line_stop[RIGHT]){
com3 3:440e774cc24b 208 if(adcline[RIGHT].read_u16() > LINE_ON){
com3 3:440e774cc24b 209 line[RIGHT] = 1;
com3 3:440e774cc24b 210 line_stop[RIGHT] = 1;
com3 4:536cd493a337 211 line_stop[LEFT] = 1;
com3 4:536cd493a337 212 //line_ping[RIGHT] = 1;
com3 2:59edff92b599 213 myled[3] = 1;
com3 4:536cd493a337 214 liner_R.attach(&line_stop_R,LINE_TIME);
com3 4:536cd493a337 215 liner_L.attach(&line_stop_L,LINE_TIME);
com3 2:59edff92b599 216 } else {
com3 3:440e774cc24b 217 line[RIGHT] = 0;
com3 4:536cd493a337 218 myled[3] = 0;
com3 2:59edff92b599 219 }
com3 2:59edff92b599 220 }
com3 2:59edff92b599 221 }
com3 2:59edff92b599 222
com3 2:59edff92b599 223
com3 2:59edff92b599 224 void pidupdate()
com3 2:59edff92b599 225 {
com3 4:536cd493a337 226 //pid.setSetPoint(180.0 + goal);
com3 2:59edff92b599 227 pidinput = compass;
com3 2:59edff92b599 228 pid.setProcessValue(pidinput);
com3 2:59edff92b599 229
com3 2:59edff92b599 230 compasspid = -pid.compute();
com3 2:59edff92b599 231 }
com3 1:fb4277ce4d93 232
com3 3:440e774cc24b 233 void home()
com3 3:440e774cc24b 234 {
com3 3:440e774cc24b 235 x = 0;
com3 3:440e774cc24b 236 y = 0;
com3 3:440e774cc24b 237 kick = 0;
com3 3:440e774cc24b 238 goal = 0.0;
com3 3:440e774cc24b 239
com3 3:440e774cc24b 240 // if((145 < ping[LEFT]+ping[RIGHT]) && (ping[LEFT]+ping[RIGHT] < 155)){
com3 3:440e774cc24b 241 if(ping[LEFT] > PING_LR){
com3 3:440e774cc24b 242 x = -HOME_P;
com3 3:440e774cc24b 243 y = -5;
com3 3:440e774cc24b 244 } else if(ping[RIGHT] > PING_LR){
com3 3:440e774cc24b 245 x = HOME_P;
com3 3:440e774cc24b 246 y = -5;
com3 3:440e774cc24b 247 }
com3 3:440e774cc24b 248 // }
com3 3:440e774cc24b 249
com3 3:440e774cc24b 250 if(ping[BACK] > HOME_BACK){
com3 3:440e774cc24b 251 y = -HOME_P;
com3 3:440e774cc24b 252 } else if(ping[BACK] < 5){
com3 3:440e774cc24b 253 y = HOME_P;
com3 3:440e774cc24b 254 }
com3 3:440e774cc24b 255 }
com3 3:440e774cc24b 256
com3 0:e6d14fec4954 257 int main() {
com3 0:e6d14fec4954 258
com3 0:e6d14fec4954 259 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
com3 3:440e774cc24b 260 //uint8_t num = 0;
com3 4:536cd493a337 261 uint8_t dir = 0, power = 0, lcd_count = 0;
com3 4:536cd493a337 262 int x_ball = 0, x_turn = 0, y_ball = 0, y_turn = 0;
com3 0:e6d14fec4954 263
com3 0:e6d14fec4954 264 wait(1);
com3 0:e6d14fec4954 265
com3 0:e6d14fec4954 266 motor.baud(115200); //ボーレート設定
com3 0:e6d14fec4954 267 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
com3 0:e6d14fec4954 268 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
com3 0:e6d14fec4954 269 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
com3 5:6604ec9044a0 270 xbeerx.attach(&xbee_rx,5);
com3 5:6604ec9044a0 271 xbeetx.attach(&xbee_tx,5);
com3 0:e6d14fec4954 272 //compassdef = (compass / 10); //コンパス初期値保存
com3 0:e6d14fec4954 273 //compassdef = (dcompass.sample() / 10);
com3 2:59edff92b599 274 pid.setInputLimits(0.0,360.0);
com3 2:59edff92b599 275 pid.setOutputLimits(-30.0,30.0);
com3 2:59edff92b599 276 pid.setBias(0.0);
com3 2:59edff92b599 277 pid.setMode(AUTO_MODE);
com3 2:59edff92b599 278 pid.setSetPoint(180.0);
com3 2:59edff92b599 279 pidupdata.attach(&pidupdate, 0.05);
com3 0:e6d14fec4954 280
com3 4:536cd493a337 281 sw_wh.mode(PullUp);
com3 4:536cd493a337 282 start.mode(PullDown);
com3 0:e6d14fec4954 283
com3 3:440e774cc24b 284 lcd.cls();
com3 3:440e774cc24b 285 lcd.locate(0,0);
com3 3:440e774cc24b 286 lcd.printf("Chronograph");
com3 2:59edff92b599 287
com3 4:536cd493a337 288
com3 4:536cd493a337 289 //move(x,y,s);
com3 4:536cd493a337 290
com3 3:440e774cc24b 291 myled[0] = 1;
com3 4:536cd493a337 292
com3 4:536cd493a337 293 //スタート前チェック
com3 3:440e774cc24b 294 while(!start){
com3 4:536cd493a337 295 if(lcd_count == 0){
com3 4:536cd493a337 296 lcd.locate(6,1);
com3 4:536cd493a337 297 lcd.printf("%3d", compass);
com3 4:536cd493a337 298 } else if(lcd_count == 1){
com3 4:536cd493a337 299 lcd.locate(0,1);
com3 4:536cd493a337 300 lcd.printf("ir_dis = %3d", ir_dis);
com3 4:536cd493a337 301 } else if(lcd_count == 2){
com3 4:536cd493a337 302 lcd.locate(0,0);
com3 4:536cd493a337 303 lcd.printf("ir_num = %3d\nir_val = %3d", ir_num, value_ir);
com3 4:536cd493a337 304 } else if(lcd_count == 3){
com3 4:536cd493a337 305 lcd.locate(0,0);
com3 4:536cd493a337 306 lcd.printf("F:%3d B:%3d\nL:%3d R:%3d", ping[FRONT], ping[BACK], ping[LEFT], ping[RIGHT]);
com3 4:536cd493a337 307 } else if(lcd_count == 4){
com3 4:536cd493a337 308 lcd.locate(0,0);
com3 4:536cd493a337 309 lcd.printf("F:%5d B:%5d\nL:%5d R:%5d", adcline[FRONT].read_u16(), adcline[BACK].read_u16(), adcline[LEFT].read_u16(), adcline[RIGHT].read_u16());
com3 4:536cd493a337 310 }
com3 4:536cd493a337 311 if(!sw_wh){
com3 4:536cd493a337 312 lcd_count++;
com3 4:536cd493a337 313 if(lcd_count == 5){
com3 4:536cd493a337 314 lcd_count = 0;
com3 4:536cd493a337 315 lcd.cls();
com3 4:536cd493a337 316 lcd.locate(0,0);
com3 4:536cd493a337 317 lcd.printf("Chronograph");
com3 4:536cd493a337 318 }
com3 4:536cd493a337 319 wait(0.5);
com3 4:536cd493a337 320 }
com3 2:59edff92b599 321 wait(0.1);
com3 2:59edff92b599 322 }
com3 4:536cd493a337 323
com3 4:536cd493a337 324
com3 4:536cd493a337 325
com3 4:536cd493a337 326
com3 3:440e774cc24b 327 myled[0] = 0;
com3 3:440e774cc24b 328 lcd.cls();
com3 3:440e774cc24b 329 lcd.locate(0,0);
com3 3:440e774cc24b 330 lcd.printf("Chronograph");
com3 3:440e774cc24b 331 lcd.locate(7,1);
com3 3:440e774cc24b 332 lcd.printf("ABURASOBA");
com3 3:440e774cc24b 333
com3 3:440e774cc24b 334
com3 4:536cd493a337 335
com3 4:536cd493a337 336 while(1){
com3 4:536cd493a337 337 x = 0;
com3 4:536cd493a337 338 y = 0;
com3 4:536cd493a337 339 s = compasspid;
com3 4:536cd493a337 340 power = MOTOR_P;
com3 4:536cd493a337 341
com3 4:536cd493a337 342 if((ir_dis < 35)/* && ( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )*/){
com3 4:536cd493a337 343 kick = KICK;
com3 4:536cd493a337 344 } else {
com3 4:536cd493a337 345 kick = 0;
com3 4:536cd493a337 346 //goal = 0.0;
com3 4:536cd493a337 347 }
com3 4:536cd493a337 348
com3 4:536cd493a337 349 if((ir_num != FF) && (ir_num != FFL) && (ir_num != FFR)){
com3 4:536cd493a337 350 if(ir_dis > BALL_FAR){
com3 4:536cd493a337 351 x_turn = TURN_P * way[ir_num][0];
com3 4:536cd493a337 352 y_turn = TURN_P * way[ir_num][1];
com3 4:536cd493a337 353
com3 4:536cd493a337 354 x_ball = BALL_P * ball_way[ir_num][0];
com3 4:536cd493a337 355 y_ball = BALL_P * ball_way[ir_num][1];
com3 4:536cd493a337 356
com3 4:536cd493a337 357 if(ir_num == BB){
com3 4:536cd493a337 358 if(ping[LEFT] > ping[RIGHT]){
com3 4:536cd493a337 359 x_ball = -x_ball;
com3 4:536cd493a337 360 }
com3 4:536cd493a337 361 }
com3 4:536cd493a337 362
com3 4:536cd493a337 363 x = x_turn + x_ball;
com3 4:536cd493a337 364 y = y_turn + y_ball;
com3 4:536cd493a337 365
com3 4:536cd493a337 366 } else if(ir_dis < BALL_NEAR){
com3 4:536cd493a337 367 x_turn = -(TURN_P - 5) * way[ir_num][0];
com3 4:536cd493a337 368 y_turn = -(TURN_P - 5) * way[ir_num][1];
com3 4:536cd493a337 369
com3 4:536cd493a337 370 x_ball = BALL_P * ball_way[ir_num][0];
com3 4:536cd493a337 371 y_ball = BALL_P * ball_way[ir_num][1];
com3 4:536cd493a337 372
com3 4:536cd493a337 373 if(ir_num == BB){
com3 4:536cd493a337 374 if(ping[LEFT] > ping[RIGHT]){
com3 4:536cd493a337 375 x_ball = -x_ball;
com3 4:536cd493a337 376 }
com3 4:536cd493a337 377 }
com3 4:536cd493a337 378
com3 4:536cd493a337 379 x = x_turn + x_ball;
com3 4:536cd493a337 380 y = y_turn + y_ball;
com3 4:536cd493a337 381
com3 4:536cd493a337 382 } else {
com3 4:536cd493a337 383 if((ir_num == FL) || (ir_num == FR)){
com3 4:536cd493a337 384 power = 20;
com3 4:536cd493a337 385 }
com3 4:536cd493a337 386 x = power * ball_way[ir_num][0];
com3 4:536cd493a337 387 y = power * ball_way[ir_num][1];
com3 4:536cd493a337 388 if(ir_num == BB){
com3 4:536cd493a337 389 if(ping[LEFT] > ping[RIGHT]){
com3 4:536cd493a337 390 x = -x;
com3 4:536cd493a337 391 }
com3 4:536cd493a337 392 }
com3 4:536cd493a337 393 }
com3 4:536cd493a337 394
com3 4:536cd493a337 395 } else {
com3 4:536cd493a337 396 if(ir_num == FF){
com3 4:536cd493a337 397 power = MOTOR_P + 10;
com3 4:536cd493a337 398 }
com3 4:536cd493a337 399 x = power * ball_way[ir_num][0];
com3 4:536cd493a337 400 y = power * ball_way[ir_num][1];
com3 4:536cd493a337 401
com3 4:536cd493a337 402 }
com3 4:536cd493a337 403
com3 4:536cd493a337 404 line_check();
com3 4:536cd493a337 405
com3 4:536cd493a337 406 if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
com3 4:536cd493a337 407 home();
com3 4:536cd493a337 408 }
com3 4:536cd493a337 409
com3 4:536cd493a337 410 //x = 30; y = 10;
com3 4:536cd493a337 411 line_state();
com3 4:536cd493a337 412
com3 4:536cd493a337 413 if((x == 0) && (y == 0)){
com3 4:536cd493a337 414 if(ping[BACK] > 25){
com3 4:536cd493a337 415 y = -HOME_P;
com3 4:536cd493a337 416 }
com3 4:536cd493a337 417 }
com3 4:536cd493a337 418
com3 4:536cd493a337 419 //x = 0; y = 0;
com3 4:536cd493a337 420
com3 4:536cd493a337 421 move(x,y,s);
com3 4:536cd493a337 422 }
com3 4:536cd493a337 423
com3 4:536cd493a337 424 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 425
com3 4:536cd493a337 426 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 427
com3 4:536cd493a337 428 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 429
com3 4:536cd493a337 430 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 431
com3 4:536cd493a337 432 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 433
com3 4:536cd493a337 434 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 435
com3 4:536cd493a337 436 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 437
com3 4:536cd493a337 438 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 439
com3 4:536cd493a337 440 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
com3 4:536cd493a337 441
com3 2:59edff92b599 442 while(1){
com3 3:440e774cc24b 443
com3 3:440e774cc24b 444 x = 0;
com3 3:440e774cc24b 445 y = 0;
com3 3:440e774cc24b 446 s = compasspid;
com3 3:440e774cc24b 447 power = MOTOR_P;
com3 3:440e774cc24b 448
com3 3:440e774cc24b 449 if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){
com3 4:536cd493a337 450 //kick = 25;
com3 3:440e774cc24b 451 //if(130 < ping[LEFT]+ping[RIGHT]){
com3 3:440e774cc24b 452 //if(goal == 0){
com3 3:440e774cc24b 453 /* if(ping[LEFT] > 100){
com3 3:440e774cc24b 454 goal = -GOAL;
com3 3:440e774cc24b 455 }*//* else if(ping[RIGHT] > 100){
com3 3:440e774cc24b 456 //goal = GOAL;
com3 3:440e774cc24b 457 }*/
com3 3:440e774cc24b 458 //}
com3 3:440e774cc24b 459 //}
com3 3:440e774cc24b 460
com3 3:440e774cc24b 461 } else {
com3 3:440e774cc24b 462 kick = 0;
com3 3:440e774cc24b 463 goal = 0.0;
com3 3:440e774cc24b 464 }
com3 3:440e774cc24b 465
com3 4:536cd493a337 466 if((ir_dis > 130) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){
com3 3:440e774cc24b 467 x = power * way[ir_num][0];
com3 3:440e774cc24b 468 y = power * way[ir_num][1];
com3 3:440e774cc24b 469 } else {
com3 3:440e774cc24b 470 if((ir_num == FF) || (ir_num == FFFL) || (ir_num == FFFR)){
com3 4:536cd493a337 471 power = MOTOR_P + 10;
com3 3:440e774cc24b 472 } else if((ir_num == FFL) || (ir_num == FFR)){
com3 3:440e774cc24b 473 power = MOTOR_P;
com3 3:440e774cc24b 474 } else {
com3 3:440e774cc24b 475 power = MOTOR_P + 5;
com3 3:440e774cc24b 476 }
com3 3:440e774cc24b 477 x = power * ball_way[ir_num][0];
com3 3:440e774cc24b 478 y = power * ball_way[ir_num][1];
com3 3:440e774cc24b 479 if(ir_num == BB){
com3 3:440e774cc24b 480 if(ping[LEFT] > ping[RIGHT]){
com3 3:440e774cc24b 481 x = -x;
com3 3:440e774cc24b 482 }
com3 3:440e774cc24b 483 }
com3 3:440e774cc24b 484 }
com3 3:440e774cc24b 485
com3 2:59edff92b599 486 line_check();
com3 2:59edff92b599 487
com3 3:440e774cc24b 488 if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
com3 2:59edff92b599 489 home();
com3 2:59edff92b599 490 }
com3 3:440e774cc24b 491
com3 4:536cd493a337 492 //x = 30; y = 10;
com3 3:440e774cc24b 493 line_state();
com3 2:59edff92b599 494
com3 3:440e774cc24b 495 if((x == 0) && (y == 0)){
com3 3:440e774cc24b 496 if(ping[BACK] > 25){
com3 3:440e774cc24b 497 y = -HOME_P;
com3 3:440e774cc24b 498 }
com3 3:440e774cc24b 499 }
com3 3:440e774cc24b 500
com3 3:440e774cc24b 501 //x = 0; y = 0;
com3 2:59edff92b599 502
com3 2:59edff92b599 503 move(x,y,s);
com3 2:59edff92b599 504 }
com3 0:e6d14fec4954 505 }