Fumiya Fujisawa / Mbed 2 deprecated Chronograph_Main_ver2

ロボカップ2013のジャパンオープンメインプログラム

Dependencies:   PID TextLCD mbed

main.cpp@0:fb4c529248d7, 2014-04-27 (annotated)

Committer:
ryuna
Date:
Sun Apr 27 02:37:13 2014 +0000
Revision:
0:fb4c529248d7
update_4/27

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:fb4c529248d7 1 #include "mbed.h"
ryuna 0:fb4c529248d7 2 //#include "HMC6352.h"
ryuna 0:fb4c529248d7 3 #include "TextLCD.h"
ryuna 0:fb4c529248d7 4 #include "common.h"
ryuna 0:fb4c529248d7 5 #include "PID.h"
ryuna 0:fb4c529248d7 6 #include <math.h>
ryuna 0:fb4c529248d7 7 #include <sstream>
ryuna 0:fb4c529248d7 8
ryuna 0:fb4c529248d7 9 #define LINE_LP 30/*line def@line_state*/
ryuna 0:fb4c529248d7 10 #define LINE_FP 40/*line def@line_State*/
ryuna 0:fb4c529248d7 11 #define LINE_ON 0x2710/*line underline*///40000
ryuna 0:fb4c529248d7 12 #define LINE_TIME 0.5/*used lineAttach*/
ryuna 0:fb4c529248d7 13
ryuna 0:fb4c529248d7 14 /****pid config *************/
ryuna 0:fb4c529248d7 15 #define RATE 0.08//52
ryuna 0:fb4c529248d7 16 #define PID_BIAS 0.2
ryuna 0:fb4c529248d7 17 #define REFERENCE 180.0
ryuna 0:fb4c529248d7 18 #define MINIMUM 0.1
ryuna 0:fb4c529248d7 19 #define MAXIMUM 360.0
ryuna 0:fb4c529248d7 20 #define PID_CYCLE 0.05 //s
ryuna 0:fb4c529248d7 21 #define P_GAIN 1.4 //0.78
ryuna 0:fb4c529248d7 22 #define I_GAIN 0.0 //0.0
ryuna 0:fb4c529248d7 23 #define D_GAIN 0.015 //0.009
ryuna 0:fb4c529248d7 24 #define OUT_LIMIT 30.0
ryuna 0:fb4c529248d7 25 #define MAX_POW 100
ryuna 0:fb4c529248d7 26 #define MIN_POW -100
ryuna 0:fb4c529248d7 27 /****pid config *************/
ryuna 0:fb4c529248d7 28
ryuna 0:fb4c529248d7 29 /*keep your distance!!*/
ryuna 0:fb4c529248d7 30 #define OFFENSE_POWER 31 /*moter power strength*/
ryuna 0:fb4c529248d7 31 #define DEFENSE_POWER 30
ryuna 0:fb4c529248d7 32
ryuna 0:fb4c529248d7 33 #define BACK_HOME_TIME 0.3
ryuna 0:fb4c529248d7 34
ryuna 0:fb4c529248d7 35 /*dash*/
ryuna 0:fb4c529248d7 36 #define DASH_TIME 0.7//about
ryuna 0:fb4c529248d7 37 #define CHARGE_DACH_TIME 0.25//about
ryuna 0:fb4c529248d7 38 #define DASH_STRENGTH 40
ryuna 0:fb4c529248d7 39
ryuna 0:fb4c529248d7 40 Serial motor(p9,p10);
ryuna 0:fb4c529248d7 41 Serial sensor(p13,p14);
ryuna 0:fb4c529248d7 42 Serial xbee(p28,p27);
ryuna 0:fb4c529248d7 43 Serial pc(USBTX, USBRX);
ryuna 0:fb4c529248d7 44
ryuna 0:fb4c529248d7 45 TextLCD lcd(p26, p25, p24, p23, p22, p21);
ryuna 0:fb4c529248d7 46 AnalogIn adcline[4] = {p17, p18, p19, p20};
ryuna 0:fb4c529248d7 47 DigitalIn start(p7);/*start switch*/
ryuna 0:fb4c529248d7 48 DigitalIn check(p8);/*Xbee chenge switch*/
ryuna 0:fb4c529248d7 49 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
ryuna 0:fb4c529248d7 50 DigitalIn kick_check(p5);
ryuna 0:fb4c529248d7 51
ryuna 0:fb4c529248d7 52 /*Compass PID SetUp*/
ryuna 0:fb4c529248d7 53 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
ryuna 0:fb4c529248d7 54 Ticker pidUpdata;
ryuna 0:fb4c529248d7 55
ryuna 0:fb4c529248d7 56 /*XBee interrupt*/
ryuna 0:fb4c529248d7 57 Ticker xbeetx;
ryuna 0:fb4c529248d7 58 Ticker xbeerx;
ryuna 0:fb4c529248d7 59
ryuna 0:fb4c529248d7 60 /*line check,linestate*/
ryuna 0:fb4c529248d7 61 Timeout liner0;
ryuna 0:fb4c529248d7 62 Timeout liner1;
ryuna 0:fb4c529248d7 63 Timeout liner2;
ryuna 0:fb4c529248d7 64 Timeout liner3;
ryuna 0:fb4c529248d7 65
ryuna 0:fb4c529248d7 66 /*back home*/
ryuna 0:fb4c529248d7 67 Timeout home;
ryuna 0:fb4c529248d7 68 Timer timer_home;
ryuna 0:fb4c529248d7 69
ryuna 0:fb4c529248d7 70 /*ofence dash timer*/
ryuna 0:fb4c529248d7 71 Timer dash_charge;
ryuna 0:fb4c529248d7 72 Ticker dash_start;
ryuna 0:fb4c529248d7 73
ryuna 0:fb4c529248d7 74
ryuna 0:fb4c529248d7 75 extern string StringFIN;
ryuna 0:fb4c529248d7 76 extern void array(int,int,int,int);
ryuna 0:fb4c529248d7 77 extern void micon_rx(void);
ryuna 0:fb4c529248d7 78 extern void xbee_tx(void);
ryuna 0:fb4c529248d7 79 extern void xbee_rx(void);
ryuna 0:fb4c529248d7 80 extern int count;
ryuna 0:fb4c529248d7 81
ryuna 0:fb4c529248d7 82 int speed[4] = {0}; /*@move,tx_moter*/
ryuna 0:fb4c529248d7 83 uint8_t line_stop[4] = {0},home_stop = 0; /*@line_stop(number),@back_home*/
ryuna 0:fb4c529248d7 84 unsigned int compass = 0; /*relevant compass*/
ryuna 0:fb4c529248d7 85 double standTu = 0, inputPID = 180.0, compassPID = 0.0; /*relevant compass*/
ryuna 0:fb4c529248d7 86 uint8_t ping[4]={0}; /*ping_data*/
ryuna 0:fb4c529248d7 87 uint8_t ir_min = 0,ir_num = 0, ir_main = 0; /*ir_min=25~50~70(value),ir_num=0~9,ir_main=undef*/
ryuna 0:fb4c529248d7 88 //double clockL = 0;//not used
ryuna 0:fb4c529248d7 89 uint8_t dash_stop = 0;
ryuna 0:fb4c529248d7 90
ryuna 0:fb4c529248d7 91
ryuna 0:fb4c529248d7 92 void move(double vx, double vy, int vs, int vk){
ryuna 0:fb4c529248d7 93 double pwm[4] = {0};
ryuna 0:fb4c529248d7 94 uint8_t i = 0;
ryuna 0:fb4c529248d7 95 pwm[0] = (double)((vx) + vs);
ryuna 0:fb4c529248d7 96 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
ryuna 0:fb4c529248d7 97 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
ryuna 0:fb4c529248d7 98 pwm[3] = vk;
ryuna 0:fb4c529248d7 99
ryuna 0:fb4c529248d7 100 for(i = 0; i < 4; i++){
ryuna 0:fb4c529248d7 101 if(pwm[i] > 100){
ryuna 0:fb4c529248d7 102 pwm[i] = 100;
ryuna 0:fb4c529248d7 103 }else if(pwm[i] < -100){
ryuna 0:fb4c529248d7 104 pwm[i] = -100;
ryuna 0:fb4c529248d7 105 }
ryuna 0:fb4c529248d7 106 speed[i] = pwm[i];
ryuna 0:fb4c529248d7 107 }
ryuna 0:fb4c529248d7 108 }
ryuna 0:fb4c529248d7 109 //通信（モータ用）
ryuna 0:fb4c529248d7 110 void tx_motor(){
ryuna 0:fb4c529248d7 111 array(speed[0],speed[1],speed[3],speed[2]);
ryuna 0:fb4c529248d7 112 motor.printf("%s",StringFIN.c_str());
ryuna 0:fb4c529248d7 113 }
ryuna 0:fb4c529248d7 114 void line_state(double *vx,double*vy,uint8_t *line){
ryuna 0:fb4c529248d7 115 /*chenge to move ,line*/
ryuna 0:fb4c529248d7 116 if(line[FRONT]){
ryuna 0:fb4c529248d7 117 *vy = -LINE_FP;
ryuna 0:fb4c529248d7 118 }
ryuna 0:fb4c529248d7 119 if(line[LEFT]){
ryuna 0:fb4c529248d7 120 *vx = LINE_LP;
ryuna 0:fb4c529248d7 121 }
ryuna 0:fb4c529248d7 122 if(line[BACK]){
ryuna 0:fb4c529248d7 123 *vy = LINE_FP;
ryuna 0:fb4c529248d7 124 }
ryuna 0:fb4c529248d7 125 if(line[RIGHT]){
ryuna 0:fb4c529248d7 126 *vx = -LINE_LP;
ryuna 0:fb4c529248d7 127 }
ryuna 0:fb4c529248d7 128 }
ryuna 0:fb4c529248d7 129 void line_stop0(){
ryuna 0:fb4c529248d7 130 line_stop[FRONT] = 0;
ryuna 0:fb4c529248d7 131 }
ryuna 0:fb4c529248d7 132 void line_stop1(){
ryuna 0:fb4c529248d7 133 line_stop[LEFT] = 0;
ryuna 0:fb4c529248d7 134 }
ryuna 0:fb4c529248d7 135 void line_stop2(){
ryuna 0:fb4c529248d7 136 line_stop[BACK] = 0;
ryuna 0:fb4c529248d7 137 }
ryuna 0:fb4c529248d7 138 void line_stop3(){
ryuna 0:fb4c529248d7 139 line_stop[RIGHT] = 0;
ryuna 0:fb4c529248d7 140 }
ryuna 0:fb4c529248d7 141 void line_check(uint8_t *line){
ryuna 0:fb4c529248d7 142 if(!line_stop[FRONT]){
ryuna 0:fb4c529248d7 143 if(adcline[FRONT].read_u16() > LINE_ON){
ryuna 0:fb4c529248d7 144 line[FRONT] = 1;
ryuna 0:fb4c529248d7 145 line_stop[FRONT] = 1;
ryuna 0:fb4c529248d7 146 line_stop[BACK] = 1;
ryuna 0:fb4c529248d7 147 myled[0] = 1;
ryuna 0:fb4c529248d7 148 liner0.attach(&line_stop0,LINE_TIME);
ryuna 0:fb4c529248d7 149 liner2.attach(&line_stop2,LINE_TIME);
ryuna 0:fb4c529248d7 150 } else {
ryuna 0:fb4c529248d7 151 line[FRONT] = 0;
ryuna 0:fb4c529248d7 152 myled[0] = 0;
ryuna 0:fb4c529248d7 153 }
ryuna 0:fb4c529248d7 154 }
ryuna 0:fb4c529248d7 155 if(!line_stop[LEFT]){
ryuna 0:fb4c529248d7 156 if(adcline[LEFT].read_u16() > LINE_ON){
ryuna 0:fb4c529248d7 157 line[LEFT] = 1;
ryuna 0:fb4c529248d7 158 line_stop[LEFT] = 1;
ryuna 0:fb4c529248d7 159 line_stop[RIGHT] = 1;
ryuna 0:fb4c529248d7 160 myled[1] = 1;
ryuna 0:fb4c529248d7 161 liner1.attach(&line_stop1,LINE_TIME);
ryuna 0:fb4c529248d7 162 liner3.attach(&line_stop3,LINE_TIME);
ryuna 0:fb4c529248d7 163 } else {
ryuna 0:fb4c529248d7 164 line[LEFT] = 0;
ryuna 0:fb4c529248d7 165 myled[1] = 0;
ryuna 0:fb4c529248d7 166 }
ryuna 0:fb4c529248d7 167 }
ryuna 0:fb4c529248d7 168 if(!line_stop[BACK]){
ryuna 0:fb4c529248d7 169 if(adcline[BACK].read_u16() > LINE_ON){
ryuna 0:fb4c529248d7 170 line[BACK] = 1;
ryuna 0:fb4c529248d7 171 line_stop[BACK] = 1;
ryuna 0:fb4c529248d7 172 line_stop[FRONT] = 1;
ryuna 0:fb4c529248d7 173 myled[2] = 1;
ryuna 0:fb4c529248d7 174 liner2.attach(&line_stop2,LINE_TIME);
ryuna 0:fb4c529248d7 175 liner0.attach(&line_stop0,LINE_TIME);
ryuna 0:fb4c529248d7 176 } else {
ryuna 0:fb4c529248d7 177 line[BACK] = 0;
ryuna 0:fb4c529248d7 178 myled[2] = 0;
ryuna 0:fb4c529248d7 179 }
ryuna 0:fb4c529248d7 180 }
ryuna 0:fb4c529248d7 181 if(!line_stop[RIGHT]){
ryuna 0:fb4c529248d7 182 if(adcline[RIGHT].read_u16() > LINE_ON){
ryuna 0:fb4c529248d7 183 line[RIGHT] = 1;
ryuna 0:fb4c529248d7 184 line_stop[RIGHT] = 1;
ryuna 0:fb4c529248d7 185 line_stop[LEFT] = 1;
ryuna 0:fb4c529248d7 186 myled[3] = 1;
ryuna 0:fb4c529248d7 187 liner3.attach(&line_stop3,LINE_TIME);
ryuna 0:fb4c529248d7 188 liner1.attach(&line_stop1,LINE_TIME);
ryuna 0:fb4c529248d7 189 } else {
ryuna 0:fb4c529248d7 190 line[RIGHT] = 0;
ryuna 0:fb4c529248d7 191 myled[3] = 0;
ryuna 0:fb4c529248d7 192 }
ryuna 0:fb4c529248d7 193 }
ryuna 0:fb4c529248d7 194 }
ryuna 0:fb4c529248d7 195 void PidUpdate()
ryuna 0:fb4c529248d7 196 {
ryuna 0:fb4c529248d7 197 pid.setSetPoint((int)((REFERENCE + standTu) / 1.0));
ryuna 0:fb4c529248d7 198 inputPID = compass;
ryuna 0:fb4c529248d7 199
ryuna 0:fb4c529248d7 200 pid.setProcessValue(inputPID);
ryuna 0:fb4c529248d7 201 compassPID = -(pid.compute());
ryuna 0:fb4c529248d7 202 }
ryuna 0:fb4c529248d7 203
ryuna 0:fb4c529248d7 204
ryuna 0:fb4c529248d7 205 void ir_defense0(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 206
ryuna 0:fb4c529248d7 207
ryuna 0:fb4c529248d7 208 }
ryuna 0:fb4c529248d7 209
ryuna 0:fb4c529248d7 210 void ir_defense1(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 211
ryuna 0:fb4c529248d7 212
ryuna 0:fb4c529248d7 213 }
ryuna 0:fb4c529248d7 214
ryuna 0:fb4c529248d7 215 void ir_defense2(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 216
ryuna 0:fb4c529248d7 217
ryuna 0:fb4c529248d7 218 }
ryuna 0:fb4c529248d7 219
ryuna 0:fb4c529248d7 220 void ir_defense3(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 221
ryuna 0:fb4c529248d7 222
ryuna 0:fb4c529248d7 223 }
ryuna 0:fb4c529248d7 224
ryuna 0:fb4c529248d7 225 void ir_defense4(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 226
ryuna 0:fb4c529248d7 227
ryuna 0:fb4c529248d7 228 }
ryuna 0:fb4c529248d7 229
ryuna 0:fb4c529248d7 230 void ir_defense5(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 231
ryuna 0:fb4c529248d7 232
ryuna 0:fb4c529248d7 233 }
ryuna 0:fb4c529248d7 234
ryuna 0:fb4c529248d7 235 void ir_defense6(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 236
ryuna 0:fb4c529248d7 237
ryuna 0:fb4c529248d7 238 }
ryuna 0:fb4c529248d7 239
ryuna 0:fb4c529248d7 240 void ir_defense7(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 241 if(ping[2]<20){
ryuna 0:fb4c529248d7 242 *vx = DEFENSE_POWER*1.2*way16[13][0];
ryuna 0:fb4c529248d7 243 *vy = DEFENSE_POWER*1.2*way16[13][1];
ryuna 0:fb4c529248d7 244 }
ryuna 0:fb4c529248d7 245 }
ryuna 0:fb4c529248d7 246
ryuna 0:fb4c529248d7 247 void ir_defense8(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 248 if(ping[LEFT]>ping[RIGHT]){
ryuna 0:fb4c529248d7 249 *vx = DEFENSE_POWER*(-0.8+near1[ir_num][VX]+go[ir_num][VX]);
ryuna 0:fb4c529248d7 250 *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]);
ryuna 0:fb4c529248d7 251 }else if(ping[RIGHT]>ping[LEFT]){
ryuna 0:fb4c529248d7 252 *vx = DEFENSE_POWER*(0.8+near1[ir_num][VX]+go[ir_num][VX]);
ryuna 0:fb4c529248d7 253 *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]);
ryuna 0:fb4c529248d7 254 }
ryuna 0:fb4c529248d7 255
ryuna 0:fb4c529248d7 256 }
ryuna 0:fb4c529248d7 257
ryuna 0:fb4c529248d7 258 void ir_defense9(double *vx, double *vy, int *k, int rateD){
ryuna 0:fb4c529248d7 259 if(ping[2]<20){
ryuna 0:fb4c529248d7 260 *vx = DEFENSE_POWER*1.2*way16[3][0];
ryuna 0:fb4c529248d7 261 *vy = DEFENSE_POWER*1.2*way16[3][1];
ryuna 0:fb4c529248d7 262 }
ryuna 0:fb4c529248d7 263
ryuna 0:fb4c529248d7 264 }
ryuna 0:fb4c529248d7 265
ryuna 0:fb4c529248d7 266 void ir_offense0(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 267 dash_charge.stop();
ryuna 0:fb4c529248d7 268 dash_charge.reset();
ryuna 0:fb4c529248d7 269 /*
ryuna 0:fb4c529248d7 270 clockL = 6.50;
ryuna 0:fb4c529248d7 271 standTu = 0;
ryuna 0:fb4c529248d7 272 *vx = (ir_min+5)*way16[9][0];
ryuna 0:fb4c529248d7 273 *vy = (ir_min+5)*way16[9][1];
ryuna 0:fb4c529248d7 274 */
ryuna 0:fb4c529248d7 275
ryuna 0:fb4c529248d7 276 }
ryuna 0:fb4c529248d7 277 void ir_offense1(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 278 dash_charge.stop();
ryuna 0:fb4c529248d7 279 dash_charge.reset();
ryuna 0:fb4c529248d7 280 /*
ryuna 0:fb4c529248d7 281 clockL = 7.00;
ryuna 0:fb4c529248d7 282 standTu = 0;
ryuna 0:fb4c529248d7 283 *vx = ir_min*way16[10][0];
ryuna 0:fb4c529248d7 284 *vy = ir_min*way16[10][1];
ryuna 0:fb4c529248d7 285 */
ryuna 0:fb4c529248d7 286 }
ryuna 0:fb4c529248d7 287 void ir_offense2(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 288 dash_charge.stop();
ryuna 0:fb4c529248d7 289 dash_charge.reset();
ryuna 0:fb4c529248d7 290
ryuna 0:fb4c529248d7 291 /*y
ryuna 0:fb4c529248d7 292 if(rateD <= 0){
ryuna 0:fb4c529248d7 293
ryuna 0:fb4c529248d7 294 *vx = STRENGTH*1.2*way16[0][0];
ryuna 0:fb4c529248d7 295 *vy = STRENGTH*1.2*way16[0][1];
ryuna 0:fb4c529248d7 296 }
ryuna 0:fb4c529248d7 297 */
ryuna 0:fb4c529248d7 298 /*
ryuna 0:fb4c529248d7 299 clockL = 10.0;
ryuna 0:fb4c529248d7 300 *vx=-ir_min;
ryuna 0:fb4c529248d7 301 *vy = ir_min+5;
ryuna 0:fb4c529248d7 302 if(ping[LEFT]>ping[RIGHT]){
ryuna 0:fb4c529248d7 303 clockL = 10.5;
ryuna 0:fb4c529248d7 304 *vx = 35*way16[14][0];
ryuna 0:fb4c529248d7 305 *vy = 35*way16[14][1];
ryuna 0:fb4c529248d7 306 *//**vx = 0;
ryuna 0:fb4c529248d7 307 *vy =35;*/
ryuna 0:fb4c529248d7 308 /*
ryuna 0:fb4c529248d7 309 }
ryuna 0:fb4c529248d7 310 */
ryuna 0:fb4c529248d7 311 }
ryuna 0:fb4c529248d7 312 void ir_offense3(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 313 /**********
ryuna 0:fb4c529248d7 314 一定時間待ち,その間変わらずir[3]が反応していたならば、ブースト直進キック
ryuna 0:fb4c529248d7 315 irの最小値の位置が移動した場合,タイマーの時間を初期化、停止
ryuna 0:fb4c529248d7 316 設定時間内に変更がなかった場合,数秒間キック直進し続ける。(ほかからの変更を拒否)
ryuna 0:fb4c529248d7 317
ryuna 0:fb4c529248d7 318 ********/
ryuna 0:fb4c529248d7 319 if(dash_charge.read() == 0){
ryuna 0:fb4c529248d7 320
ryuna 0:fb4c529248d7 321 dash_charge.start();
ryuna 0:fb4c529248d7 322 }
ryuna 0:fb4c529248d7 323 /*
ryuna 0:fb4c529248d7 324 if(rateD <=0+3){
ryuna 0:fb4c529248d7 325 *vk = 25;
ryuna 0:fb4c529248d7 326 }else {
ryuna 0:fb4c529248d7 327 *vk = 0;
ryuna 0:fb4c529248d7 328 }
ryuna 0:fb4c529248d7 329 */
ryuna 0:fb4c529248d7 330 /*
ryuna 0:fb4c529248d7 331 if(rateD <= 0){
ryuna 0:fb4c529248d7 332 *vx = STRENGTH*1.2*way16[0][0];
ryuna 0:fb4c529248d7 333 *vy = STRENGTH*1.2*way16[0][1];
ryuna 0:fb4c529248d7 334 }*/
ryuna 0:fb4c529248d7 335 /*
ryuna 0:fb4c529248d7 336 clockL = 12.0;
ryuna 0:fb4c529248d7 337 *vx = 0,*vy = 30;
ryuna 0:fb4c529248d7 338 */
ryuna 0:fb4c529248d7 339 }
ryuna 0:fb4c529248d7 340 void ir_offense4(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 341 dash_charge.stop();
ryuna 0:fb4c529248d7 342 dash_charge.reset();
ryuna 0:fb4c529248d7 343
ryuna 0:fb4c529248d7 344 /*if(rateD <= 0){
ryuna 0:fb4c529248d7 345
ryuna 0:fb4c529248d7 346 *vx = STRENGTH*1.2*way16[0][0];
ryuna 0:fb4c529248d7 347 *vy = STRENGTH*1.2*way16[0][1];
ryuna 0:fb4c529248d7 348 }*/
ryuna 0:fb4c529248d7 349 /*
ryuna 0:fb4c529248d7 350 clockL = 2.00;
ryuna 0:fb4c529248d7 351 *vx=ir_min;
ryuna 0:fb4c529248d7 352 *vy = ir_min+5;
ryuna 0:fb4c529248d7 353 if(ping[RIGHT]>ping[LEFT]){
ryuna 0:fb4c529248d7 354 clockL = 2.50;
ryuna 0:fb4c529248d7 355 *vx = 35*way16[2][0];
ryuna 0:fb4c529248d7 356 *vy = 35*way16[2][1];
ryuna 0:fb4c529248d7 357 *//**vx = 0;
ryuna 0:fb4c529248d7 358 *vy = 35;*/
ryuna 0:fb4c529248d7 359 /*
ryuna 0:fb4c529248d7 360 }
ryuna 0:fb4c529248d7 361 */
ryuna 0:fb4c529248d7 362 }
ryuna 0:fb4c529248d7 363 void ir_offense5(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 364 dash_charge.stop();
ryuna 0:fb4c529248d7 365 dash_charge.reset();
ryuna 0:fb4c529248d7 366 /*
ryuna 0:fb4c529248d7 367 clockL = 5.00;
ryuna 0:fb4c529248d7 368 standTu = 0;
ryuna 0:fb4c529248d7 369 *vx = ir_min*way16[6][0];
ryuna 0:fb4c529248d7 370 *vy = ir_min*way16[6][1];
ryuna 0:fb4c529248d7 371 */
ryuna 0:fb4c529248d7 372 }
ryuna 0:fb4c529248d7 373 void ir_offense6(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 374 dash_charge.stop();
ryuna 0:fb4c529248d7 375 dash_charge.reset();
ryuna 0:fb4c529248d7 376 /*
ryuna 0:fb4c529248d7 377 clockL = 5.50;
ryuna 0:fb4c529248d7 378 standTu = 0;
ryuna 0:fb4c529248d7 379 *vx = (ir_min+5)*way16[7][0];
ryuna 0:fb4c529248d7 380 *vy = (ir_min+5)*way16[7][1];
ryuna 0:fb4c529248d7 381 */
ryuna 0:fb4c529248d7 382 }
ryuna 0:fb4c529248d7 383 void ir_offense7(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 384 dash_charge.stop();
ryuna 0:fb4c529248d7 385 dash_charge.reset();
ryuna 0:fb4c529248d7 386 /*clockL = 6.00;
ryuna 0:fb4c529248d7 387 standTu = 0;
ryuna 0:fb4c529248d7 388 *vx = 0;
ryuna 0:fb4c529248d7 389 *vy = -(ir_min+8);
ryuna 0:fb4c529248d7 390 */
ryuna 0:fb4c529248d7 391 }
ryuna 0:fb4c529248d7 392 void ir_offense8(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 393 dash_charge.stop();
ryuna 0:fb4c529248d7 394 dash_charge.reset();
ryuna 0:fb4c529248d7 395 if(rateD<(0)){
ryuna 0:fb4c529248d7 396 if(ping[LEFT]>ping[RIGHT]){
ryuna 0:fb4c529248d7 397 *vx = OFFENSE_POWER*(-1+near1[ir_num][VX]+go[ir_num][VX]);
ryuna 0:fb4c529248d7 398 *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]);
ryuna 0:fb4c529248d7 399 }else if(ping[RIGHT]>ping[LEFT]){
ryuna 0:fb4c529248d7 400 *vx = OFFENSE_POWER*(1+near1[ir_num][VX]+go[ir_num][VX]);
ryuna 0:fb4c529248d7 401 *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]);
ryuna 0:fb4c529248d7 402 }
ryuna 0:fb4c529248d7 403 }
ryuna 0:fb4c529248d7 404 /*standTu = 0;
ryuna 0:fb4c529248d7 405 if(ping[LEFT]>ping[RIGHT]){
ryuna 0:fb4c529248d7 406 clockL = 8.00;
ryuna 0:fb4c529248d7 407 *vx = 40*way16[11][0];
ryuna 0:fb4c529248d7 408 *vy = 40*way16[11][1];
ryuna 0:fb4c529248d7 409 }else{
ryuna 0:fb4c529248d7 410 clockL = 4.00;
ryuna 0:fb4c529248d7 411 *vx = 40*way16[5][0];
ryuna 0:fb4c529248d7 412 *vy = 40*way16[5][1];
ryuna 0:fb4c529248d7 413 }*/
ryuna 0:fb4c529248d7 414 }
ryuna 0:fb4c529248d7 415 void ir_offense9(double *vx, double *vy, int *vk, int rateD){
ryuna 0:fb4c529248d7 416 dash_charge.stop();
ryuna 0:fb4c529248d7 417 dash_charge.reset();
ryuna 0:fb4c529248d7 418 /*clockL = 6.00;
ryuna 0:fb4c529248d7 419 standTu = 0;
ryuna 0:fb4c529248d7 420 *vx = 0;
ryuna 0:fb4c529248d7 421 *vy = -(ir_min+8);*/
ryuna 0:fb4c529248d7 422 }
ryuna 0:fb4c529248d7 423
ryuna 0:fb4c529248d7 424
ryuna 0:fb4c529248d7 425 /**kick and dash function**/
ryuna 0:fb4c529248d7 426 void dash_reset(){
ryuna 0:fb4c529248d7 427 dash_stop = 0;
ryuna 0:fb4c529248d7 428 lcd.cls();
ryuna 0:fb4c529248d7 429 }
ryuna 0:fb4c529248d7 430
ryuna 0:fb4c529248d7 431 void dist_fun_DM(int *rate_dm){//used defense first junction
ryuna 0:fb4c529248d7 432 *rate_dm = ir_min - keep_ball[ir_num];//zastu
ryuna 0:fb4c529248d7 433 if(*rate_dm >0){
ryuna 0:fb4c529248d7 434 *rate_dm = 1;
ryuna 0:fb4c529248d7 435 }else if(*rate_dm <-7){
ryuna 0:fb4c529248d7 436 *rate_dm =-7;
ryuna 0:fb4c529248d7 437 }
ryuna 0:fb4c529248d7 438
ryuna 0:fb4c529248d7 439 }
ryuna 0:fb4c529248d7 440
ryuna 0:fb4c529248d7 441 void dist_fun_D(int *rate_d){//ofense action support
ryuna 0:fb4c529248d7 442 *rate_d = ir_main - keep_dist[ir_num];
ryuna 0:fb4c529248d7 443 if(*rate_d < -5){
ryuna 0:fb4c529248d7 444 *rate_d = -5;
ryuna 0:fb4c529248d7 445 }else if(*rate_d >10){
ryuna 0:fb4c529248d7 446 *rate_d = 11;
ryuna 0:fb4c529248d7 447 }
ryuna 0:fb4c529248d7 448 }
ryuna 0:fb4c529248d7 449 void dist_fun_ping(int *rate_p){//defense action support
ryuna 0:fb4c529248d7 450 rate_p[1] = ping[1] - keep_ping[1];
ryuna 0:fb4c529248d7 451 if(rate_p[1] <-10){
ryuna 0:fb4c529248d7 452 rate_p[1] = -10;
ryuna 0:fb4c529248d7 453 }else if(rate_p[1]>0){
ryuna 0:fb4c529248d7 454 rate_p[1] = 1;
ryuna 0:fb4c529248d7 455 }
ryuna 0:fb4c529248d7 456 rate_p[2] = keep_ping[2] - ping[2];
ryuna 0:fb4c529248d7 457 if(rate_p[2]<-5){
ryuna 0:fb4c529248d7 458 rate_p[2] = -5;
ryuna 0:fb4c529248d7 459 }else if(rate_p[2] >6){
ryuna 0:fb4c529248d7 460 rate_p[2] = 6;
ryuna 0:fb4c529248d7 461 }
ryuna 0:fb4c529248d7 462 rate_p[3] = ping[3] - keep_ping[3];
ryuna 0:fb4c529248d7 463 if(rate_p[3] <-10){
ryuna 0:fb4c529248d7 464 rate_p[3] = -10;
ryuna 0:fb4c529248d7 465 }else if(rate_p[3]>0){
ryuna 0:fb4c529248d7 466 rate_p[3] = 1;
ryuna 0:fb4c529248d7 467 }
ryuna 0:fb4c529248d7 468 }
ryuna 0:fb4c529248d7 469
ryuna 0:fb4c529248d7 470 void dist_fun_pingSTRONG(int *rate_p){//defense action support
ryuna 0:fb4c529248d7 471 rate_p[2] = keep_pingSTRONG[2] - ping[2];
ryuna 0:fb4c529248d7 472 if(rate_p[2]<-5){
ryuna 0:fb4c529248d7 473 rate_p[2] = -5;
ryuna 0:fb4c529248d7 474 }else if(rate_p[2] >6){
ryuna 0:fb4c529248d7 475 rate_p[2] = 6;
ryuna 0:fb4c529248d7 476 }
ryuna 0:fb4c529248d7 477 if(ping[1]+ping[3] >=155){//tyousei
ryuna 0:fb4c529248d7 478 rate_p[1] = ping[1] - keep_pingSTRONG[1];//38
ryuna 0:fb4c529248d7 479 if(rate_p[1] <-10){
ryuna 0:fb4c529248d7 480 rate_p[1] = -10;
ryuna 0:fb4c529248d7 481 }else if(rate_p[1]>0){
ryuna 0:fb4c529248d7 482 rate_p[1] = 1;
ryuna 0:fb4c529248d7 483 }
ryuna 0:fb4c529248d7 484
ryuna 0:fb4c529248d7 485 rate_p[3] = ping[3] - keep_pingSTRONG[3];///37
ryuna 0:fb4c529248d7 486 if(rate_p[3] <-10){
ryuna 0:fb4c529248d7 487 rate_p[3] = -10;
ryuna 0:fb4c529248d7 488 }else if(rate_p[3]>0){
ryuna 0:fb4c529248d7 489 rate_p[3] = 1;
ryuna 0:fb4c529248d7 490 }
ryuna 0:fb4c529248d7 491
ryuna 0:fb4c529248d7 492 }else {
ryuna 0:fb4c529248d7 493 rate_p[1] = 0;
ryuna 0:fb4c529248d7 494 rate_p[3] = 0;
ryuna 0:fb4c529248d7 495 }
ryuna 0:fb4c529248d7 496 }
ryuna 0:fb4c529248d7 497 /*
ryuna 0:fb4c529248d7 498 void dist_fun(int *rate_d,int *rate_dm,int *rate_p){*//*This is compute some length.?*/
ryuna 0:fb4c529248d7 499 /* KEEP_DISTANCE is border line, ir_main is variable
ryuna 0:fb4c529248d7 500 if ir_main> keep_dist (access)
ryuna 0:fb4c529248d7 501 else ir_main< keep_dist (out)
ryuna 0:fb4c529248d7 502 @retaD
ryuna 0:fb4c529248d7 503 */
ryuna 0:fb4c529248d7 504 /*
ryuna 0:fb4c529248d7 505 *rate_d = ir_main - keep_dist[ir_num];
ryuna 0:fb4c529248d7 506 if(*rate_d < -5){
ryuna 0:fb4c529248d7 507 *rate_d = -5;
ryuna 0:fb4c529248d7 508 }else if(*rate_d >10){
ryuna 0:fb4c529248d7 509 *rate_d = 11; //+30
ryuna 0:fb4c529248d7 510 }
ryuna 0:fb4c529248d7 511 *rate_dm = ir_min - keep_ball[ir_num];//zastu
ryuna 0:fb4c529248d7 512 rate_p[1] = ping[1] - keep_ping[1];
ryuna 0:fb4c529248d7 513 if(rate_p[1] <-10){
ryuna 0:fb4c529248d7 514 rate_p[1] = -10;
ryuna 0:fb4c529248d7 515 }else if(rate_p[1]>0){
ryuna 0:fb4c529248d7 516 rate_p[1] = 1;
ryuna 0:fb4c529248d7 517 }
ryuna 0:fb4c529248d7 518 rate_p[2] = keep_ping[2] - ping[2];
ryuna 0:fb4c529248d7 519 if(rate_p[2]<-5){
ryuna 0:fb4c529248d7 520 rate_p[2] = -5;
ryuna 0:fb4c529248d7 521 }else if(rate_p[2] >6){
ryuna 0:fb4c529248d7 522 rate_p[2] = 6;
ryuna 0:fb4c529248d7 523 }
ryuna 0:fb4c529248d7 524 rate_p[3] = ping[3] - keep_ping[3];
ryuna 0:fb4c529248d7 525 if(rate_p[3] <-10){
ryuna 0:fb4c529248d7 526 rate_p[3] = -10;
ryuna 0:fb4c529248d7 527 }else if(rate_p[3]>0){
ryuna 0:fb4c529248d7 528 rate_p[3] = 1;
ryuna 0:fb4c529248d7 529 }
ryuna 0:fb4c529248d7 530
ryuna 0:fb4c529248d7 531
ryuna 0:fb4c529248d7 532 }
ryuna 0:fb4c529248d7 533 */
ryuna 0:fb4c529248d7 534 void home_stop0(){
ryuna 0:fb4c529248d7 535 home_stop = 0;
ryuna 0:fb4c529248d7 536 }
ryuna 0:fb4c529248d7 537
ryuna 0:fb4c529248d7 538 void back_home(double *vx,double*vy){
ryuna 0:fb4c529248d7 539 *vx = 0;
ryuna 0:fb4c529248d7 540 *vy = -40;
ryuna 0:fb4c529248d7 541 if(!home_stop){
ryuna 0:fb4c529248d7 542 if(timer_home.read() ==0){
ryuna 0:fb4c529248d7 543 timer_home.start();
ryuna 0:fb4c529248d7 544 }
ryuna 0:fb4c529248d7 545 }
ryuna 0:fb4c529248d7 546 if(ping[2]<24){
ryuna 0:fb4c529248d7 547 *vy = -35;
ryuna 0:fb4c529248d7 548 }else if(ping[2]<30){
ryuna 0:fb4c529248d7 549 *vy = -25;
ryuna 0:fb4c529248d7 550 }
ryuna 0:fb4c529248d7 551 if((abs(ping[1]-ping[3]))>25){
ryuna 0:fb4c529248d7 552 if(ping[1]>ping[3]){
ryuna 0:fb4c529248d7 553 //*vy = 15;
ryuna 0:fb4c529248d7 554 *vx = -25;
ryuna 0:fb4c529248d7 555 }else if(ping[1]<ping[3]){
ryuna 0:fb4c529248d7 556 //*vy = 12;
ryuna 0:fb4c529248d7 557 *vx = 25;
ryuna 0:fb4c529248d7 558 }
ryuna 0:fb4c529248d7 559 }
ryuna 0:fb4c529248d7 560
ryuna 0:fb4c529248d7 561 //左角を抜ける方法を考える
ryuna 0:fb4c529248d7 562
ryuna 0:fb4c529248d7 563
ryuna 0:fb4c529248d7 564 if((timer_home.read()>0.6)/*||home0.read()==0*/){
ryuna 0:fb4c529248d7 565 *vy = 11;
ryuna 0:fb4c529248d7 566 *vx = 0;
ryuna 0:fb4c529248d7 567 timer_home.stop();
ryuna 0:fb4c529248d7 568 timer_home.reset();
ryuna 0:fb4c529248d7 569 home_stop = 1;
ryuna 0:fb4c529248d7 570 home.attach(&home_stop0,BACK_HOME_TIME);
ryuna 0:fb4c529248d7 571 //wait(0.1);
ryuna 0:fb4c529248d7 572 }
ryuna 0:fb4c529248d7 573 if(ping[2]<20){
ryuna 0:fb4c529248d7 574 *vx = 0;
ryuna 0:fb4c529248d7 575 *vy = 5;
ryuna 0:fb4c529248d7 576 }
ryuna 0:fb4c529248d7 577
ryuna 0:fb4c529248d7 578 }
ryuna 0:fb4c529248d7 579 int main(void){
ryuna 0:fb4c529248d7 580
ryuna 0:fb4c529248d7 581 uint8_t line[4] = {0}; /*@line_state,line_check*/
ryuna 0:fb4c529248d7 582 double vx = 0,vy = 0; /*used move(moter)*/
ryuna 0:fb4c529248d7 583 int vs = 0, vk = 0; /*used move(moter)*/
ryuna 0:fb4c529248d7 584 int rateD = 0; /*keep distance*/
ryuna 0:fb4c529248d7 585 int rateDM = 0; /*keep_distrance to boal //ir_max use*/
ryuna 0:fb4c529248d7 586 int rateP[4] = {0}; /*keep in front of goal*/
ryuna 0:fb4c529248d7 587 void (*offense[10])(double *,double *,int *,int);
ryuna 0:fb4c529248d7 588 void (*defense[10])(double *,double *,int *,int);
ryuna 0:fb4c529248d7 589
ryuna 0:fb4c529248d7 590 //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ryuna 0:fb4c529248d7 591 motor.baud(115200); //ボーレート設定
ryuna 0:fb4c529248d7 592 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
ryuna 0:fb4c529248d7 593 motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
ryuna 0:fb4c529248d7 594 sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
ryuna 0:fb4c529248d7 595 //compassdef = (compass / 10); //コンパス初期値保存
ryuna 0:fb4c529248d7 596 //compassdef = (dcompass.sample() / 10);
ryuna 0:fb4c529248d7 597 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
ryuna 0:fb4c529248d7 598 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
ryuna 0:fb4c529248d7 599 pid.setBias(PID_BIAS); //pid sed def
ryuna 0:fb4c529248d7 600 pid.setMode(AUTO_MODE); //pid sed def
ryuna 0:fb4c529248d7 601 pid.setSetPoint(REFERENCE); //pid sed def
ryuna 0:fb4c529248d7 602 start.mode(PullDown); //digitalin pulldown
ryuna 0:fb4c529248d7 603 check.mode(PullUp); //digitalin pullup
ryuna 0:fb4c529248d7 604 kick_check.mode(PullUp);
ryuna 0:fb4c529248d7 605
ryuna 0:fb4c529248d7 606 offense[0] = ir_offense0;
ryuna 0:fb4c529248d7 607 offense[1] = ir_offense1;
ryuna 0:fb4c529248d7 608 offense[2] = ir_offense2;
ryuna 0:fb4c529248d7 609 offense[3] = ir_offense3;
ryuna 0:fb4c529248d7 610 offense[4] = ir_offense4;
ryuna 0:fb4c529248d7 611 offense[5] = ir_offense5;
ryuna 0:fb4c529248d7 612 offense[6] = ir_offense6;
ryuna 0:fb4c529248d7 613 offense[7] = ir_offense7;
ryuna 0:fb4c529248d7 614 offense[8] = ir_offense8;
ryuna 0:fb4c529248d7 615 offense[9] = ir_offense9;
ryuna 0:fb4c529248d7 616
ryuna 0:fb4c529248d7 617
ryuna 0:fb4c529248d7 618 defense[0] = ir_defense0;
ryuna 0:fb4c529248d7 619 defense[1] = ir_defense1;
ryuna 0:fb4c529248d7 620 defense[2] = ir_defense2;
ryuna 0:fb4c529248d7 621 defense[3] = ir_defense3;
ryuna 0:fb4c529248d7 622 defense[4] = ir_defense4;
ryuna 0:fb4c529248d7 623 defense[5] = ir_defense5;
ryuna 0:fb4c529248d7 624 defense[6] = ir_defense6;
ryuna 0:fb4c529248d7 625 defense[7] = ir_defense7;
ryuna 0:fb4c529248d7 626 defense[8] = ir_defense8;
ryuna 0:fb4c529248d7 627 defense[9] = ir_defense9;
ryuna 0:fb4c529248d7 628
ryuna 0:fb4c529248d7 629
ryuna 0:fb4c529248d7 630
ryuna 0:fb4c529248d7 631 pidUpdata.attach(&PidUpdate, PID_CYCLE);
ryuna 0:fb4c529248d7 632 //move(30,0,0,0);
ryuna 0:fb4c529248d7 633 count = 0;
ryuna 0:fb4c529248d7 634 if(check){// NOT use Xbee
ryuna 0:fb4c529248d7 635 myled[3] = 0;
ryuna 0:fb4c529248d7 636 count = 1;
ryuna 0:fb4c529248d7 637 //lcd.printf("NOT USE XBee\nDEFENSE\n");
ryuna 0:fb4c529248d7 638 /*
ryuna 0:fb4c529248d7 639 count = 1:defense
ryuna 0:fb4c529248d7 640 count = 0:offense
ryuna 0:fb4c529248d7 641 */
ryuna 0:fb4c529248d7 642 }else {
ryuna 0:fb4c529248d7 643 myled[3] = 1;
ryuna 0:fb4c529248d7 644 xbeerx.attach(&xbee_rx,5);
ryuna 0:fb4c529248d7 645 xbeetx.attach(&xbee_tx,5);
ryuna 0:fb4c529248d7 646 }
ryuna 0:fb4c529248d7 647 myled[0] = 1;
ryuna 0:fb4c529248d7 648 while(!start){
ryuna 0:fb4c529248d7 649 lcd.printf("comp %3d min %3dnum %3d main %3d",compass,ir_min,ir_num,ir_main);
ryuna 0:fb4c529248d7 650 if(!kick_check){
ryuna 0:fb4c529248d7 651 move(0,0,0,25);
ryuna 0:fb4c529248d7 652 }else{
ryuna 0:fb4c529248d7 653 move(0,0,0,0);
ryuna 0:fb4c529248d7 654 }
ryuna 0:fb4c529248d7 655 }
ryuna 0:fb4c529248d7 656 myled[0] = 0;
ryuna 0:fb4c529248d7 657 lcd.cls();
ryuna 0:fb4c529248d7 658 while(1){
ryuna 0:fb4c529248d7 659 vx = 0, vy = 0, vk = 0;
ryuna 0:fb4c529248d7 660 standTu = 0;
ryuna 0:fb4c529248d7 661
ryuna 0:fb4c529248d7 662 if(ir_num<10){
ryuna 0:fb4c529248d7 663 dist_fun_DM(&rateDM);//used defense first junction
ryuna 0:fb4c529248d7 664
ryuna 0:fb4c529248d7 665 if(count){/**** DEFENSE ACTION!! *******/
ryuna 0:fb4c529248d7 666 if((rateDM > 0)||(ping[2]>33)){//key point
ryuna 0:fb4c529248d7 667 dist_fun_ping(rateP);//check ping state.
ryuna 0:fb4c529248d7 668 vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
ryuna 0:fb4c529248d7 669 vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
ryuna 0:fb4c529248d7 670 if(ping[2]<28){
ryuna 0:fb4c529248d7 671
ryuna 0:fb4c529248d7 672 }else if(ping[2]<45){
ryuna 0:fb4c529248d7 673 vy = -30;
ryuna 0:fb4c529248d7 674 }else {
ryuna 0:fb4c529248d7 675 vx =0;
ryuna 0:fb4c529248d7 676 vy = -35;
ryuna 0:fb4c529248d7 677 }
ryuna 0:fb4c529248d7 678
ryuna 0:fb4c529248d7 679
ryuna 0:fb4c529248d7 680 }else{
ryuna 0:fb4c529248d7 681 dist_fun_pingSTRONG(rateP);
ryuna 0:fb4c529248d7 682 vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]);
ryuna 0:fb4c529248d7 683 vy = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][1]+strongPing1[rateP[1]+10][1]+strongPing2[rateP[2]+10][1]+strongPing3[rateP[3]+10][1]);
ryuna 0:fb4c529248d7 684 (*defense[ir_num])(&vx,&vy,&vk,rateD);
ryuna 0:fb4c529248d7 685
ryuna 0:fb4c529248d7 686 if(ir_num == 2||ir_num == 3||ir_num == 4){
ryuna 0:fb4c529248d7 687 vk=25;
ryuna 0:fb4c529248d7 688 }
ryuna 0:fb4c529248d7 689 }
ryuna 0:fb4c529248d7 690
ryuna 0:fb4c529248d7 691
ryuna 0:fb4c529248d7 692 }else{/**** OFFENSE ACTION!! *****/
ryuna 0:fb4c529248d7 693 // if count =0
ryuna 0:fb4c529248d7 694 if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){
ryuna 0:fb4c529248d7 695 //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する
ryuna 0:fb4c529248d7 696 dist_fun_ping(rateP);//check ping_state
ryuna 0:fb4c529248d7 697
ryuna 0:fb4c529248d7 698 vx = OFFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
ryuna 0:fb4c529248d7 699 vy = OFFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
ryuna 0:fb4c529248d7 700
ryuna 0:fb4c529248d7 701 if(ping[2]<30){
ryuna 0:fb4c529248d7 702 //stop place
ryuna 0:fb4c529248d7 703 }else if(ping[2]<50){
ryuna 0:fb4c529248d7 704 vy = -30;
ryuna 0:fb4c529248d7 705 }else {
ryuna 0:fb4c529248d7 706 vx = 0;
ryuna 0:fb4c529248d7 707 vy = -35;
ryuna 0:fb4c529248d7 708 }
ryuna 0:fb4c529248d7 709
ryuna 0:fb4c529248d7 710 }else{/**** Main offense action */
ryuna 0:fb4c529248d7 711
ryuna 0:fb4c529248d7 712 if(!dash_stop){
ryuna 0:fb4c529248d7 713 dist_fun_D(&rateD);//check ball state.
ryuna 0:fb4c529248d7 714 if(rateD > 0){
ryuna 0:fb4c529248d7 715 //dist_fun_pingSTRONG(rateP);
ryuna 0:fb4c529248d7 716 vx = OFFENSE_POWER*(near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
ryuna 0:fb4c529248d7 717 vy = OFFENSE_POWER*(near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
ryuna 0:fb4c529248d7 718
ryuna 0:fb4c529248d7 719 }else{
ryuna 0:fb4c529248d7 720 vx = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][0]+go[ir_num][0]);
ryuna 0:fb4c529248d7 721 vy = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][1]+go[ir_num][1]);
ryuna 0:fb4c529248d7 722 }
ryuna 0:fb4c529248d7 723
ryuna 0:fb4c529248d7 724 if(dash_charge.read()>=CHARGE_DACH_TIME){
ryuna 0:fb4c529248d7 725 dash_start.attach(&dash_reset,DASH_TIME);
ryuna 0:fb4c529248d7 726 dash_stop = 1;
ryuna 0:fb4c529248d7 727 //lcd.printf(" FALCON KICK \n\n");
ryuna 0:fb4c529248d7 728 dash_charge.stop();
ryuna 0:fb4c529248d7 729 dash_charge.reset();
ryuna 0:fb4c529248d7 730 }
ryuna 0:fb4c529248d7 731 }
ryuna 0:fb4c529248d7 732 (*offense[ir_num])(&vx,&vy,&vk,rateD);
ryuna 0:fb4c529248d7 733 if(dash_stop){
ryuna 0:fb4c529248d7 734 vk = 25;
ryuna 0:fb4c529248d7 735 vx = DASH_STRENGTH*way16[0][0];
ryuna 0:fb4c529248d7 736 vy = DASH_STRENGTH*way16[0][1];
ryuna 0:fb4c529248d7 737
ryuna 0:fb4c529248d7 738
ryuna 0:fb4c529248d7 739 }
ryuna 0:fb4c529248d7 740
ryuna 0:fb4c529248d7 741 }
ryuna 0:fb4c529248d7 742
ryuna 0:fb4c529248d7 743 }
ryuna 0:fb4c529248d7 744 }
ryuna 0:fb4c529248d7 745
ryuna 0:fb4c529248d7 746 if(ir_num >= 10){
ryuna 0:fb4c529248d7 747 back_home(&vx,&vy);
ryuna 0:fb4c529248d7 748 }
ryuna 0:fb4c529248d7 749 line_check(line);
ryuna 0:fb4c529248d7 750 line_state(&vx,&vy,line);
ryuna 0:fb4c529248d7 751 vs = compassPID;
ryuna 0:fb4c529248d7 752 move(vx,vy,vs,vk);
ryuna 0:fb4c529248d7 753 //lcd.printf("%0.2lf clock\n\n",clockL);
ryuna 0:fb4c529248d7 754 //pc.printf("compassPID:%d\t compass:%d\n",s,compass);
ryuna 0:fb4c529248d7 755 //pc.printf("%lf %lf\n",x,y);
ryuna 0:fb4c529248d7 756 //pc.printf("compass: %d\n",compass);
ryuna 0:fb4c529248d7 757 //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
ryuna 0:fb4c529248d7 758 //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
ryuna 0:fb4c529248d7 759 //pc.printf("line0:%d\tline1:%d\tline2:%d\tline3:%d\n",adcline[0].read_u16(),adcline[1].read_u16(),adcline[2].read_u16(),adcline[3].read_u16());
ryuna 0:fb4c529248d7 760 }
ryuna 0:fb4c529248d7 761
ryuna 0:fb4c529248d7 762
ryuna 0:fb4c529248d7 763
ryuna 0:fb4c529248d7 764
ryuna 0:fb4c529248d7 765
ryuna 0:fb4c529248d7 766
ryuna 0:fb4c529248d7 767
ryuna 0:fb4c529248d7 768 /**************************************************
ryuna 0:fb4c529248d7 769
ryuna 0:fb4c529248d7 770
ryuna 0:fb4c529248d7 771
ryuna 0:fb4c529248d7 772
ryuna 0:fb4c529248d7 773
ryuna 0:fb4c529248d7 774
ryuna 0:fb4c529248d7 775
ryuna 0:fb4c529248d7 776
ryuna 0:fb4c529248d7 777
ryuna 0:fb4c529248d7 778
ryuna 0:fb4c529248d7 779
ryuna 0:fb4c529248d7 780
ryuna 0:fb4c529248d7 781
ryuna 0:fb4c529248d7 782
ryuna 0:fb4c529248d7 783
ryuna 0:fb4c529248d7 784
ryuna 0:fb4c529248d7 785
ryuna 0:fb4c529248d7 786
ryuna 0:fb4c529248d7 787 ****************************************************/
ryuna 0:fb4c529248d7 788 /*
ryuna 0:fb4c529248d7 789 while(1){
ryuna 0:fb4c529248d7 790 vx = 0, vy = 0, vk = 0;
ryuna 0:fb4c529248d7 791 standTu = 0;
ryuna 0:fb4c529248d7 792 //dist_fun(&rateD,&rateDM,rateP);//3to-ri
ryuna 0:fb4c529248d7 793 if(ir_num<10){*/
ryuna 0:fb4c529248d7 794 /*if(rateD >= 0){//if(ir_num =2ka4) =3
ryuna 0:fb4c529248d7 795 vx = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
ryuna 0:fb4c529248d7 796 vy = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
ryuna 0:fb4c529248d7 797 }else{
ryuna 0:fb4c529248d7 798 vx = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
ryuna 0:fb4c529248d7 799 vy = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
ryuna 0:fb4c529248d7 800 }*/
ryuna 0:fb4c529248d7 801 /*
ryuna 0:fb4c529248d7 802 if(rateD >= 0){
ryuna 0:fb4c529248d7 803 vx = STRENGTH*(modulate[rateD+5]*near0[ir_num][0]+modulate[rateD+5]*go[ir_num][0]);
ryuna 0:fb4c529248d7 804 vy = STRENGTH*(modulate[rateD+5]*near0[ir_num][1]+modulate[rateD+5]*go[ir_num][1]);
ryuna 0:fb4c529248d7 805 }else{
ryuna 0:fb4c529248d7 806 vx = STRENGTH*(modulate[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
ryuna 0:fb4c529248d7 807 vy = STRENGTH*(modulate[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
ryuna 0:fb4c529248d7 808 }*/
ryuna 0:fb4c529248d7 809 /*
ryuna 0:fb4c529248d7 810 dist_fun_DM(&rateDM);//used defense first junction
ryuna 0:fb4c529248d7 811 if(count){//defense
ryuna 0:fb4c529248d7 812 if(rateDM > 0){//key point
ryuna 0:fb4c529248d7 813 dist_fun_ping(rateP);//check ping state.
ryuna 0:fb4c529248d7 814 vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
ryuna 0:fb4c529248d7 815 vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
ryuna 0:fb4c529248d7 816 if(ping[2]<35){
ryuna 0:fb4c529248d7 817
ryuna 0:fb4c529248d7 818 }else if(ping[2]<48){
ryuna 0:fb4c529248d7 819 vy = -30;
ryuna 0:fb4c529248d7 820 }else {
ryuna 0:fb4c529248d7 821 vy = 0;
ryuna 0:fb4c529248d7 822 vy = -35;
ryuna 0:fb4c529248d7 823 }
ryuna 0:fb4c529248d7 824 }else{*/
ryuna 0:fb4c529248d7 825 /*if(ir_num<7){
ryuna 0:fb4c529248d7 826 vx = DEFENSE_POWER*ball_state1[ir_num][0];
ryuna 0:fb4c529248d7 827 vy = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]);
ryuna 0:fb4c529248d7 828 }else if(ir_num<10){
ryuna 0:fb4c529248d7 829 vx = DEFENSE_POWER*(strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]+ball_state1[ir_num][0]);
ryuna 0:fb4c529248d7 830 vx = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]);
ryuna 0:fb4c529248d7 831 }*/
ryuna 0:fb4c529248d7 832 /*
ryuna 0:fb4c529248d7 833 dist_fun_pingSTRONG(rateP);
ryuna 0:fb4c529248d7 834 vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]);
ryuna 0:fb4c529248d7 835 vy = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][1]+strongPing1[rateP[1]+10][1]+strongPing2[rateP[2]+10][1]+strongPing3[rateP[3]+10][1]);
ryuna 0:fb4c529248d7 836 if(ir_num == 2||ir_num == 3||ir_num == 4){
ryuna 0:fb4c529248d7 837 vk=25;
ryuna 0:fb4c529248d7 838 }
ryuna 0:fb4c529248d7 839 }
ryuna 0:fb4c529248d7 840 }else{//ofense
ryuna 0:fb4c529248d7 841 if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){
ryuna 0:fb4c529248d7 842 //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する
ryuna 0:fb4c529248d7 843 dist_fun_ping(rateP);//check ping state.
ryuna 0:fb4c529248d7 844 vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
ryuna 0:fb4c529248d7 845 vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
ryuna 0:fb4c529248d7 846 if(ping[2]<35){
ryuna 0:fb4c529248d7 847
ryuna 0:fb4c529248d7 848 }else if(ping[2]<50){
ryuna 0:fb4c529248d7 849 vy = -30;
ryuna 0:fb4c529248d7 850 }else {
ryuna 0:fb4c529248d7 851 vx = 0;
ryuna 0:fb4c529248d7 852 vy = -35;
ryuna 0:fb4c529248d7 853 }
ryuna 0:fb4c529248d7 854 }else{
ryuna 0:fb4c529248d7 855
ryuna 0:fb4c529248d7 856 dist_fun_D(&rateD);//check ball state.
ryuna 0:fb4c529248d7 857 if(rateD > 0){
ryuna 0:fb4c529248d7 858 dist_fun_pingSTRONG(rateP);
ryuna 0:fb4c529248d7 859 vx = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][0]+go[ir_num][0]);
ryuna 0:fb4c529248d7 860 vy = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][1]+go[ir_num][1]);
ryuna 0:fb4c529248d7 861 }else{
ryuna 0:fb4c529248d7 862 vx = OFFENSE_POWER*(near1[ir_num][0]+go[ir_num][0]);
ryuna 0:fb4c529248d7 863 vy = OFFENSE_POWER*(near1[ir_num][1]+go[ir_num][1]);
ryuna 0:fb4c529248d7 864 }
ryuna 0:fb4c529248d7 865 (*offense[ir_num])(&vx,&vy,&vk,rateD);
ryuna 0:fb4c529248d7 866 }
ryuna 0:fb4c529248d7 867 }
ryuna 0:fb4c529248d7 868 }
ryuna 0:fb4c529248d7 869 if(ir_num >= 10){
ryuna 0:fb4c529248d7 870 back_home(&vx,&vy);
ryuna 0:fb4c529248d7 871 }
ryuna 0:fb4c529248d7 872 */
ryuna 0:fb4c529248d7 873 /*
ryuna 0:fb4c529248d7 874 if(ir_num<10){
ryuna 0:fb4c529248d7 875 (*offense[ir_num])(&vx,&vy,&vk);
ryuna 0:fb4c529248d7 876 //vx =25*way10[ir_num][0],vy = 25*way10[ir_num][1];
ryuna 0:fb4c529248d7 877 }else{
ryuna 0:fb4c529248d7 878 clockL = 00.0;
ryuna 0:fb4c529248d7 879 back_home(&x,&y);
ryuna 0:fb4c529248d7 880 }
ryuna 0:fb4c529248d7 881
ryuna 0:fb4c529248d7 882 */
ryuna 0:fb4c529248d7 883 /*
ryuna 0:fb4c529248d7 884 line_check(line);
ryuna 0:fb4c529248d7 885 line_state(&vx,&vy,line);
ryuna 0:fb4c529248d7 886 vs = compassPID;
ryuna 0:fb4c529248d7 887 move(vx,vy,vs,vk);
ryuna 0:fb4c529248d7 888 //lcd.printf("%0.2lf clock\n\n",clockL);
ryuna 0:fb4c529248d7 889 //pc.printf("compassPID:%d\t compass:%d\n",s,compass);
ryuna 0:fb4c529248d7 890 //pc.printf("%lf %lf\n",x,y);
ryuna 0:fb4c529248d7 891 //pc.printf("compass: %d\n",compass);
ryuna 0:fb4c529248d7 892 //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
ryuna 0:fb4c529248d7 893 //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
ryuna 0:fb4c529248d7 894 //pc.printf("line0:%d\tline1:%d\tline2:%d\tline3:%d\n",adcline[0].read_u16(),adcline[1].read_u16(),adcline[2].read_u16(),adcline[3].read_u16());
ryuna 0:fb4c529248d7 895
ryuna 0:fb4c529248d7 896
ryuna 0:fb4c529248d7 897
ryuna 0:fb4c529248d7 898
ryuna 0:fb4c529248d7 899
ryuna 0:fb4c529248d7 900
ryuna 0:fb4c529248d7 901
ryuna 0:fb4c529248d7 902
ryuna 0:fb4c529248d7 903
ryuna 0:fb4c529248d7 904
ryuna 0:fb4c529248d7 905
ryuna 0:fb4c529248d7 906
ryuna 0:fb4c529248d7 907
ryuna 0:fb4c529248d7 908
ryuna 0:fb4c529248d7 909
ryuna 0:fb4c529248d7 910
ryuna 0:fb4c529248d7 911 }*/
ryuna 0:fb4c529248d7 912 }