Hajime Muraki / Mbed 2 deprecated chronograph

main

Dependencies:   TextLCD mbed PID

main.cpp@4:536cd493a337, 2014-03-17 (annotated)

Committer:
com3
Date:
Mon Mar 17 05:41:07 2014 +0000
Revision:
4:536cd493a337
Parent:
3:440e774cc24b
Child:
5:6604ec9044a0
chrono

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