Fumiya Fujisawa
ロボカップ2013のジャパンオープンメインプログラム
Dependencies: PID TextLCD mbed
Revision 0:fb4c529248d7, committed 2014-04-27
- Comitter:
- ryuna
- Date:
- Sun Apr 27 02:37:13 2014 +0000
- Commit message:
- update_4/27
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Sun Apr 27 02:37:13 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Sun Apr 27 02:37:13 2014 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h Sun Apr 27 02:37:13 2014 +0000
@@ -0,0 +1,239 @@
+
+enum{
+ FRONT = 0,
+ LEFT,
+ BACK,
+ RIGHT,
+};
+enum{
+ VX = 0,
+ VY,
+};
+
+const double way16[16][2] = {
+ {0,1},
+ {0.5,0.866},
+ {0.707,0.707},
+ {0.866,0.5},
+ {1.0},
+ {0.866,-0.5},
+ {0.707,-0.707},
+ {0.5,-0.866},
+ {0,-1},
+ {-0.5,-0.866},
+ {-0.707,-0.707},
+ {-0.866,-0.5},
+ {-1,0},
+ {-0.866,0.5},
+ {-0.707,0.707},
+ {-0.5,0.866}
+ };
+
+/*距離に応じた調整値、ボールの指定範囲外なら進む向きが大きい？,
+範囲内なら外に出ようとする力nearに大きな
+調整がかかる、これも配列*/
+
+const double modulate_near[17] = {//0~4 = -5,-4,-3,-2,-1,,5 = 0,,6~16=1,2,3.....10
+ 1.20,1.1,1.05,1.02,1.01,1,1,1,1,1,1,1,1,1,1,1,1
+ };
+
+const double modulate_go[17] = {
+ 1,1,1,1,1,1,1.01,1.02,1.03,1.05,1.07,1.08,1.10,1.12,1.15,1.15,1.15
+ };
+
+const double modulate[17] = {
+ 1.1,1.05,1.02,1.02,1.01,1,1.01,1.02,1.03,1.05,1.07,1.08,1.15,1.15,1.15,1.15,1.20
+ };
+
+//defense move action support.
+const double apply[9] = {
+ 1.4,1.4,1.3,1.2,1.15,1.15,1.10,1.05,1.0
+ };
+
+
+const double near0[10][2] = {//go go goボールと一定感覚を保とうとする輩,これまた角度ではなく値を直接投げておく
+ {-1,0},
+ {-0.505,0.505},
+ {-0.3,0.866},
+ {0,1},
+ {0.3,0.866},
+ {0.505,0.505},
+ {1,0},
+ {0.3,-0.3},//0.707,-0.707
+ {0,-1.0},
+ {-0.3,-0.3}//-0.707,-0.707
+ };
+
+const double near1[10][2] = {//back back back
+ {1,0},
+ {0.5,-0.5},
+ {0.25,0},//0.5,-0.866
+ {0,0.2},
+ {-0.25,0},//-0.5,-0.866
+ {-0.5,-0.5},
+ {-1,0},
+ {-0.707,0.303},//-0.707,0.707
+ {0,0.4},
+ {0.707,0.303}//0.707,0.707
+ };
+
+const double go[10][2] = {//ir_numに沿ったそれぞれの動きを直接配列に投げて置く参照、ir_action0~9
+ {0,-0.433},
+ {-0.303,-0.757},//0.303
+ {-0.3,0},
+ {0,0.8},//other block
+ {0.3,0},
+ {0.303,-0.757},//0.303
+ {0,-0.433},
+ {0,-1},
+ {0,-0.5},//other block@ir_action8
+ {0,-1},
+ };
+
+const int keep_dist[10] = {//ir_num[0~9]
+ 45,48,45,44,45,50,49,41,49,39//45,49,444444,
+ };
+
+const int keep_ping[4] = {
+ 0,58,30,56
+ };
+
+const int keep_pingSTRONG[4] = {
+ 0,40,20,36
+ };
+
+const int keep_ball[10] = {
+ 37,33,27,27,27,30,34,31,32,32//28
+ };
+
+const double goal_state1[12][2] = {
+ {0.866,0.5},
+ {0.866,0.5},
+ {0.866,0.5},
+ {0.866,0.5},
+ {0.866,0.5},
+ {0.433,0.25},
+ {0.433,0.25},
+ {0.433,0.25},
+ {0.433,0},
+ {0,0},
+ {0,0},
+ {0.0}
+ };
+
+const double goal_state2[12][2] = {
+ {0,-1},
+ {0,-0.8},
+ {0,-0.6},
+ {0,-0.4},
+ {0,-0.2},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0.1},
+ {0,0.3},
+ {0,0.4}
+ };
+
+const double goal_state3[12][2] = {
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.433,0.25},
+ {-0.433,0.25},
+ {-0.433,0.25},
+ {-0.433,0},
+ {0,0},
+ {0,0},
+ {0.0}
+ };
+
+const double ball_state0[10][2] = {//ir_num[0~9]
+ {-1,-0.5},
+ {-0.866,0.2},
+ {-0.707,0.5},
+ {0,0},
+ {0.707,0.5},
+ {0.866,0.2},
+ {1,-0.5},
+ {0.7,-1},
+ {0,-1},
+ {-0.7,-1}
+ };
+
+const double ball_state1[10][2] = {//ir_num[0~9]
+ {-1.0,-0.3},
+ {-0.9,-0.4},
+ {-0.9,0.866},
+ {0,1},
+ {0.9,0.866},
+ {0.9,-0.4},
+ {1.0,-0.3},
+ {-0.404,-0.7},
+ {0,0},//other comand
+ {0.404,-0.7}
+ };
+
+
+const double strongPing1[12][2] = {
+ {0.866,0.5},
+ {0.866,0.5},
+ {0.866,0.5},
+ {0.866,0.5},
+ {0.577,0.5},
+ {0.577,0.5},
+ {0.577,0.25},
+ {0.577,0.25},
+ {0.433,0.1},
+ {0.211,0.1},
+ {0,0},
+ {0.0}
+ };
+
+const double strongPing2[12][2] = {
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0},
+ {0,0.3},
+ {0,0.5}
+ };
+
+const double strongPing3[12][2] ={
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.866,0.5},
+ {-0.577,0.5},
+ {-0.577,0.25},
+ {-0.577,0.25},
+ {-0.433,0.1},
+ {-0.211,0.1},
+ {0,0},
+ {0.0}
+ };
+
+/*
+ir_number position
+
+ 2 3 4
+ 1 /----------\5
+ / \
+0| |6
+ \ /
+9 \____________/ 7
+ 8
+
+
+
+*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Apr 27 02:37:13 2014 +0000
@@ -0,0 +1,912 @@
+#include "mbed.h"
+//#include "HMC6352.h"
+#include "TextLCD.h"
+#include "common.h"
+#include "PID.h"
+#include <math.h>
+#include <sstream>
+
+#define LINE_LP 30/*line def@line_state*/
+#define LINE_FP 40/*line def@line_State*/
+#define LINE_ON 0x2710/*line underline*///40000
+#define LINE_TIME 0.5/*used lineAttach*/
+
+/****pid config *************/
+#define RATE 0.08//52
+#define PID_BIAS 0.2
+#define REFERENCE 180.0
+#define MINIMUM 0.1
+#define MAXIMUM 360.0
+#define PID_CYCLE 0.05 //s
+#define P_GAIN 1.4 //0.78
+#define I_GAIN 0.0 //0.0
+#define D_GAIN 0.015 //0.009
+#define OUT_LIMIT 30.0
+#define MAX_POW 100
+#define MIN_POW -100
+/****pid config *************/
+
+ /*keep your distance!!*/
+#define OFFENSE_POWER 31 /*moter power strength*/
+#define DEFENSE_POWER 30
+
+#define BACK_HOME_TIME 0.3
+
+/*dash*/
+#define DASH_TIME 0.7//about
+#define CHARGE_DACH_TIME 0.25//about
+#define DASH_STRENGTH 40
+
+Serial motor(p9,p10);
+Serial sensor(p13,p14);
+Serial xbee(p28,p27);
+Serial pc(USBTX, USBRX);
+
+TextLCD lcd(p26, p25, p24, p23, p22, p21);
+AnalogIn adcline[4] = {p17, p18, p19, p20};
+DigitalIn start(p7);/*start switch*/
+DigitalIn check(p8);/*Xbee chenge switch*/
+DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
+DigitalIn kick_check(p5);
+
+/*Compass PID SetUp*/
+PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
+Ticker pidUpdata;
+
+/*XBee interrupt*/
+Ticker xbeetx;
+Ticker xbeerx;
+
+/*line check,linestate*/
+Timeout liner0;
+Timeout liner1;
+Timeout liner2;
+Timeout liner3;
+
+/*back home*/
+Timeout home;
+Timer timer_home;
+
+/*ofence dash timer*/
+Timer dash_charge;
+Ticker dash_start;
+
+
+extern string StringFIN;
+extern void array(int,int,int,int);
+extern void micon_rx(void);
+extern void xbee_tx(void);
+extern void xbee_rx(void);
+extern int count;
+
+int speed[4] = {0}; /*@move,tx_moter*/
+uint8_t line_stop[4] = {0},home_stop = 0; /*@line_stop(number),@back_home*/
+unsigned int compass = 0; /*relevant compass*/
+double standTu = 0, inputPID = 180.0, compassPID = 0.0; /*relevant compass*/
+uint8_t ping[4]={0}; /*ping_data*/
+uint8_t ir_min = 0,ir_num = 0, ir_main = 0; /*ir_min=25~50~70(value),ir_num=0~9,ir_main=undef*/
+//double clockL = 0;//not used
+uint8_t dash_stop = 0;
+
+
+void move(double vx, double vy, int vs, int vk){
+ double pwm[4] = {0};
+ uint8_t i = 0;
+ pwm[0] = (double)((vx) + vs);
+ pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
+ pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
+ pwm[3] = vk;
+
+ for(i = 0; i < 4; i++){
+ if(pwm[i] > 100){
+ pwm[i] = 100;
+ }else if(pwm[i] < -100){
+ pwm[i] = -100;
+ }
+ speed[i] = pwm[i];
+ }
+}
+//通信（モータ用）
+void tx_motor(){
+ array(speed[0],speed[1],speed[3],speed[2]);
+ motor.printf("%s",StringFIN.c_str());
+}
+void line_state(double *vx,double*vy,uint8_t *line){
+ /*chenge to move ,line*/
+ if(line[FRONT]){
+ *vy = -LINE_FP;
+ }
+ if(line[LEFT]){
+ *vx = LINE_LP;
+ }
+ if(line[BACK]){
+ *vy = LINE_FP;
+ }
+ if(line[RIGHT]){
+ *vx = -LINE_LP;
+ }
+}
+void line_stop0(){
+ line_stop[FRONT] = 0;
+}
+void line_stop1(){
+ line_stop[LEFT] = 0;
+}
+void line_stop2(){
+ line_stop[BACK] = 0;
+}
+void line_stop3(){
+ line_stop[RIGHT] = 0;
+}
+void line_check(uint8_t *line){
+ if(!line_stop[FRONT]){
+ if(adcline[FRONT].read_u16() > LINE_ON){
+ line[FRONT] = 1;
+ line_stop[FRONT] = 1;
+ line_stop[BACK] = 1;
+ myled[0] = 1;
+ liner0.attach(&line_stop0,LINE_TIME);
+ liner2.attach(&line_stop2,LINE_TIME);
+ } else {
+ line[FRONT] = 0;
+ myled[0] = 0;
+ }
+ }
+ if(!line_stop[LEFT]){
+ if(adcline[LEFT].read_u16() > LINE_ON){
+ line[LEFT] = 1;
+ line_stop[LEFT] = 1;
+ line_stop[RIGHT] = 1;
+ myled[1] = 1;
+ liner1.attach(&line_stop1,LINE_TIME);
+ liner3.attach(&line_stop3,LINE_TIME);
+ } else {
+ line[LEFT] = 0;
+ myled[1] = 0;
+ }
+ }
+ if(!line_stop[BACK]){
+ if(adcline[BACK].read_u16() > LINE_ON){
+ line[BACK] = 1;
+ line_stop[BACK] = 1;
+ line_stop[FRONT] = 1;
+ myled[2] = 1;
+ liner2.attach(&line_stop2,LINE_TIME);
+ liner0.attach(&line_stop0,LINE_TIME);
+ } else {
+ line[BACK] = 0;
+ myled[2] = 0;
+ }
+ }
+ if(!line_stop[RIGHT]){
+ if(adcline[RIGHT].read_u16() > LINE_ON){
+ line[RIGHT] = 1;
+ line_stop[RIGHT] = 1;
+ line_stop[LEFT] = 1;
+ myled[3] = 1;
+ liner3.attach(&line_stop3,LINE_TIME);
+ liner1.attach(&line_stop1,LINE_TIME);
+ } else {
+ line[RIGHT] = 0;
+ myled[3] = 0;
+ }
+ }
+}
+void PidUpdate()
+{
+ pid.setSetPoint((int)((REFERENCE + standTu) / 1.0));
+ inputPID = compass;
+
+ pid.setProcessValue(inputPID);
+ compassPID = -(pid.compute());
+}
+
+
+void ir_defense0(double *vx, double *vy, int *k, int rateD){
+
+
+}
+
+void ir_defense1(double *vx, double *vy, int *k, int rateD){
+
+
+}
+
+void ir_defense2(double *vx, double *vy, int *k, int rateD){
+
+
+}
+
+void ir_defense3(double *vx, double *vy, int *k, int rateD){
+
+
+}
+
+void ir_defense4(double *vx, double *vy, int *k, int rateD){
+
+
+}
+
+void ir_defense5(double *vx, double *vy, int *k, int rateD){
+
+
+}
+
+void ir_defense6(double *vx, double *vy, int *k, int rateD){
+
+
+}
+
+void ir_defense7(double *vx, double *vy, int *k, int rateD){
+ if(ping[2]<20){
+ *vx = DEFENSE_POWER*1.2*way16[13][0];
+ *vy = DEFENSE_POWER*1.2*way16[13][1];
+ }
+}
+
+void ir_defense8(double *vx, double *vy, int *k, int rateD){
+ if(ping[LEFT]>ping[RIGHT]){
+ *vx = DEFENSE_POWER*(-0.8+near1[ir_num][VX]+go[ir_num][VX]);
+ *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]);
+ }else if(ping[RIGHT]>ping[LEFT]){
+ *vx = DEFENSE_POWER*(0.8+near1[ir_num][VX]+go[ir_num][VX]);
+ *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]);
+ }
+
+}
+
+void ir_defense9(double *vx, double *vy, int *k, int rateD){
+ if(ping[2]<20){
+ *vx = DEFENSE_POWER*1.2*way16[3][0];
+ *vy = DEFENSE_POWER*1.2*way16[3][1];
+ }
+
+}
+
+void ir_offense0(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+ /*
+ clockL = 6.50;
+ standTu = 0;
+ *vx = (ir_min+5)*way16[9][0];
+ *vy = (ir_min+5)*way16[9][1];
+ */
+
+}
+void ir_offense1(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+ /*
+ clockL = 7.00;
+ standTu = 0;
+ *vx = ir_min*way16[10][0];
+ *vy = ir_min*way16[10][1];
+ */
+}
+void ir_offense2(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+
+ /*y
+ if(rateD <= 0){
+
+ *vx = STRENGTH*1.2*way16[0][0];
+ *vy = STRENGTH*1.2*way16[0][1];
+ }
+ */
+ /*
+ clockL = 10.0;
+ *vx=-ir_min;
+ *vy = ir_min+5;
+ if(ping[LEFT]>ping[RIGHT]){
+ clockL = 10.5;
+ *vx = 35*way16[14][0];
+ *vy = 35*way16[14][1];
+ *//**vx = 0;
+ *vy =35;*/
+ /*
+ }
+ */
+}
+void ir_offense3(double *vx, double *vy, int *vk, int rateD){
+ /**********
+ 一定時間待ち,その間変わらずir[3]が反応していたならば、ブースト直進キック
+ irの最小値の位置が移動した場合,タイマーの時間を初期化、停止
+ 設定時間内に変更がなかった場合,数秒間キック直進し続ける。(ほかからの変更を拒否)
+
+ ********/
+ if(dash_charge.read() == 0){
+
+ dash_charge.start();
+ }
+ /*
+ if(rateD <=0+3){
+ *vk = 25;
+ }else {
+ *vk = 0;
+ }
+ */
+ /*
+ if(rateD <= 0){
+ *vx = STRENGTH*1.2*way16[0][0];
+ *vy = STRENGTH*1.2*way16[0][1];
+ }*/
+ /*
+ clockL = 12.0;
+ *vx = 0,*vy = 30;
+ */
+}
+void ir_offense4(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+
+ /*if(rateD <= 0){
+
+ *vx = STRENGTH*1.2*way16[0][0];
+ *vy = STRENGTH*1.2*way16[0][1];
+ }*/
+ /*
+ clockL = 2.00;
+ *vx=ir_min;
+ *vy = ir_min+5;
+ if(ping[RIGHT]>ping[LEFT]){
+ clockL = 2.50;
+ *vx = 35*way16[2][0];
+ *vy = 35*way16[2][1];
+ *//**vx = 0;
+ *vy = 35;*/
+ /*
+ }
+ */
+}
+void ir_offense5(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+ /*
+ clockL = 5.00;
+ standTu = 0;
+ *vx = ir_min*way16[6][0];
+ *vy = ir_min*way16[6][1];
+ */
+}
+void ir_offense6(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+ /*
+ clockL = 5.50;
+ standTu = 0;
+ *vx = (ir_min+5)*way16[7][0];
+ *vy = (ir_min+5)*way16[7][1];
+ */
+}
+void ir_offense7(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+ /*clockL = 6.00;
+ standTu = 0;
+ *vx = 0;
+ *vy = -(ir_min+8);
+ */
+}
+void ir_offense8(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+ if(rateD<(0)){
+ if(ping[LEFT]>ping[RIGHT]){
+ *vx = OFFENSE_POWER*(-1+near1[ir_num][VX]+go[ir_num][VX]);
+ *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]);
+ }else if(ping[RIGHT]>ping[LEFT]){
+ *vx = OFFENSE_POWER*(1+near1[ir_num][VX]+go[ir_num][VX]);
+ *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]);
+ }
+ }
+ /*standTu = 0;
+ if(ping[LEFT]>ping[RIGHT]){
+ clockL = 8.00;
+ *vx = 40*way16[11][0];
+ *vy = 40*way16[11][1];
+ }else{
+ clockL = 4.00;
+ *vx = 40*way16[5][0];
+ *vy = 40*way16[5][1];
+ }*/
+}
+void ir_offense9(double *vx, double *vy, int *vk, int rateD){
+ dash_charge.stop();
+ dash_charge.reset();
+ /*clockL = 6.00;
+ standTu = 0;
+ *vx = 0;
+ *vy = -(ir_min+8);*/
+}
+
+
+/**kick and dash function**/
+void dash_reset(){
+ dash_stop = 0;
+ lcd.cls();
+}
+
+void dist_fun_DM(int *rate_dm){//used defense first junction
+ *rate_dm = ir_min - keep_ball[ir_num];//zastu
+ if(*rate_dm >0){
+ *rate_dm = 1;
+ }else if(*rate_dm <-7){
+ *rate_dm =-7;
+ }
+
+}
+
+void dist_fun_D(int *rate_d){//ofense action support
+ *rate_d = ir_main - keep_dist[ir_num];
+ if(*rate_d < -5){
+ *rate_d = -5;
+ }else if(*rate_d >10){
+ *rate_d = 11;
+ }
+}
+void dist_fun_ping(int *rate_p){//defense action support
+ rate_p[1] = ping[1] - keep_ping[1];
+ if(rate_p[1] <-10){
+ rate_p[1] = -10;
+ }else if(rate_p[1]>0){
+ rate_p[1] = 1;
+ }
+ rate_p[2] = keep_ping[2] - ping[2];
+ if(rate_p[2]<-5){
+ rate_p[2] = -5;
+ }else if(rate_p[2] >6){
+ rate_p[2] = 6;
+ }
+ rate_p[3] = ping[3] - keep_ping[3];
+ if(rate_p[3] <-10){
+ rate_p[3] = -10;
+ }else if(rate_p[3]>0){
+ rate_p[3] = 1;
+ }
+}
+
+void dist_fun_pingSTRONG(int *rate_p){//defense action support
+ rate_p[2] = keep_pingSTRONG[2] - ping[2];
+ if(rate_p[2]<-5){
+ rate_p[2] = -5;
+ }else if(rate_p[2] >6){
+ rate_p[2] = 6;
+ }
+ if(ping[1]+ping[3] >=155){//tyousei
+ rate_p[1] = ping[1] - keep_pingSTRONG[1];//38
+ if(rate_p[1] <-10){
+ rate_p[1] = -10;
+ }else if(rate_p[1]>0){
+ rate_p[1] = 1;
+ }
+
+ rate_p[3] = ping[3] - keep_pingSTRONG[3];///37
+ if(rate_p[3] <-10){
+ rate_p[3] = -10;
+ }else if(rate_p[3]>0){
+ rate_p[3] = 1;
+ }
+
+ }else {
+ rate_p[1] = 0;
+ rate_p[3] = 0;
+ }
+}
+/*
+void dist_fun(int *rate_d,int *rate_dm,int *rate_p){*//*This is compute some length.?*/
+ /* KEEP_DISTANCE is border line, ir_main is variable
+ if ir_main> keep_dist (access)
+ else ir_main< keep_dist (out)
+ @retaD
+ */
+ /*
+ *rate_d = ir_main - keep_dist[ir_num];
+ if(*rate_d < -5){
+ *rate_d = -5;
+ }else if(*rate_d >10){
+ *rate_d = 11; //+30
+ }
+ *rate_dm = ir_min - keep_ball[ir_num];//zastu
+ rate_p[1] = ping[1] - keep_ping[1];
+ if(rate_p[1] <-10){
+ rate_p[1] = -10;
+ }else if(rate_p[1]>0){
+ rate_p[1] = 1;
+ }
+ rate_p[2] = keep_ping[2] - ping[2];
+ if(rate_p[2]<-5){
+ rate_p[2] = -5;
+ }else if(rate_p[2] >6){
+ rate_p[2] = 6;
+ }
+ rate_p[3] = ping[3] - keep_ping[3];
+ if(rate_p[3] <-10){
+ rate_p[3] = -10;
+ }else if(rate_p[3]>0){
+ rate_p[3] = 1;
+ }
+
+
+}
+*/
+void home_stop0(){
+ home_stop = 0;
+}
+
+void back_home(double *vx,double*vy){
+ *vx = 0;
+ *vy = -40;
+ if(!home_stop){
+ if(timer_home.read() ==0){
+ timer_home.start();
+ }
+ }
+ if(ping[2]<24){
+ *vy = -35;
+ }else if(ping[2]<30){
+ *vy = -25;
+ }
+ if((abs(ping[1]-ping[3]))>25){
+ if(ping[1]>ping[3]){
+ //*vy = 15;
+ *vx = -25;
+ }else if(ping[1]<ping[3]){
+ //*vy = 12;
+ *vx = 25;
+ }
+ }
+
+ //左角を抜ける方法を考える
+
+
+ if((timer_home.read()>0.6)/*||home0.read()==0*/){
+ *vy = 11;
+ *vx = 0;
+ timer_home.stop();
+ timer_home.reset();
+ home_stop = 1;
+ home.attach(&home_stop0,BACK_HOME_TIME);
+ //wait(0.1);
+ }
+ if(ping[2]<20){
+ *vx = 0;
+ *vy = 5;
+ }
+
+}
+int main(void){
+
+ uint8_t line[4] = {0}; /*@line_state,line_check*/
+ double vx = 0,vy = 0; /*used move(moter)*/
+ int vs = 0, vk = 0; /*used move(moter)*/
+ int rateD = 0; /*keep distance*/
+ int rateDM = 0; /*keep_distrance to boal //ir_max use*/
+ int rateP[4] = {0}; /*keep in front of goal*/
+ void (*offense[10])(double *,double *,int *,int);
+ void (*defense[10])(double *,double *,int *,int);
+
+ //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ motor.baud(115200); //ボーレート設定
+ motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
+ motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
+ sensor.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み（センサ用）
+ //compassdef = (compass / 10); //コンパス初期値保存
+ //compassdef = (dcompass.sample() / 10);
+ pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
+ pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
+ pid.setBias(PID_BIAS); //pid sed def
+ pid.setMode(AUTO_MODE); //pid sed def
+ pid.setSetPoint(REFERENCE); //pid sed def
+ start.mode(PullDown); //digitalin pulldown
+ check.mode(PullUp); //digitalin pullup
+ kick_check.mode(PullUp);
+
+ offense[0] = ir_offense0;
+ offense[1] = ir_offense1;
+ offense[2] = ir_offense2;
+ offense[3] = ir_offense3;
+ offense[4] = ir_offense4;
+ offense[5] = ir_offense5;
+ offense[6] = ir_offense6;
+ offense[7] = ir_offense7;
+ offense[8] = ir_offense8;
+ offense[9] = ir_offense9;
+
+
+ defense[0] = ir_defense0;
+ defense[1] = ir_defense1;
+ defense[2] = ir_defense2;
+ defense[3] = ir_defense3;
+ defense[4] = ir_defense4;
+ defense[5] = ir_defense5;
+ defense[6] = ir_defense6;
+ defense[7] = ir_defense7;
+ defense[8] = ir_defense8;
+ defense[9] = ir_defense9;
+
+
+
+ pidUpdata.attach(&PidUpdate, PID_CYCLE);
+ //move(30,0,0,0);
+ count = 0;
+ if(check){// NOT use Xbee
+ myled[3] = 0;
+ count = 1;
+ //lcd.printf("NOT USE XBee\nDEFENSE\n");
+ /*
+ count = 1:defense
+ count = 0:offense
+ */
+ }else {
+ myled[3] = 1;
+ xbeerx.attach(&xbee_rx,5);
+ xbeetx.attach(&xbee_tx,5);
+ }
+ myled[0] = 1;
+ while(!start){
+ lcd.printf("comp %3d min %3dnum %3d main %3d",compass,ir_min,ir_num,ir_main);
+ if(!kick_check){
+ move(0,0,0,25);
+ }else{
+ move(0,0,0,0);
+ }
+ }
+ myled[0] = 0;
+ lcd.cls();
+while(1){
+ vx = 0, vy = 0, vk = 0;
+ standTu = 0;
+
+ if(ir_num<10){
+ dist_fun_DM(&rateDM);//used defense first junction
+
+ if(count){/**** DEFENSE ACTION!! *******/
+ if((rateDM > 0)||(ping[2]>33)){//key point
+ dist_fun_ping(rateP);//check ping state.
+ vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
+ 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]);
+ if(ping[2]<28){
+
+ }else if(ping[2]<45){
+ vy = -30;
+ }else {
+ vx =0;
+ vy = -35;
+ }
+
+
+ }else{
+ dist_fun_pingSTRONG(rateP);
+ vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]);
+ 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]);
+ (*defense[ir_num])(&vx,&vy,&vk,rateD);
+
+ if(ir_num == 2||ir_num == 3||ir_num == 4){
+ vk=25;
+ }
+ }
+
+
+ }else{/**** OFFENSE ACTION!! *****/
+ // if count =0
+ if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){
+ //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する
+ dist_fun_ping(rateP);//check ping_state
+
+ vx = OFFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
+ 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]);
+
+ if(ping[2]<30){
+ //stop place
+ }else if(ping[2]<50){
+ vy = -30;
+ }else {
+ vx = 0;
+ vy = -35;
+ }
+
+ }else{/**** Main offense action */
+
+ if(!dash_stop){
+ dist_fun_D(&rateD);//check ball state.
+ if(rateD > 0){
+ //dist_fun_pingSTRONG(rateP);
+ vx = OFFENSE_POWER*(near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
+ vy = OFFENSE_POWER*(near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
+
+ }else{
+ vx = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][0]+go[ir_num][0]);
+ vy = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][1]+go[ir_num][1]);
+ }
+
+ if(dash_charge.read()>=CHARGE_DACH_TIME){
+ dash_start.attach(&dash_reset,DASH_TIME);
+ dash_stop = 1;
+ //lcd.printf(" FALCON KICK \n\n");
+ dash_charge.stop();
+ dash_charge.reset();
+ }
+ }
+ (*offense[ir_num])(&vx,&vy,&vk,rateD);
+ if(dash_stop){
+ vk = 25;
+ vx = DASH_STRENGTH*way16[0][0];
+ vy = DASH_STRENGTH*way16[0][1];
+
+
+ }
+
+ }
+
+ }
+ }
+
+ if(ir_num >= 10){
+ back_home(&vx,&vy);
+ }
+ line_check(line);
+ line_state(&vx,&vy,line);
+ vs = compassPID;
+ move(vx,vy,vs,vk);
+ //lcd.printf("%0.2lf clock\n\n",clockL);
+ //pc.printf("compassPID:%d\t compass:%d\n",s,compass);
+ //pc.printf("%lf %lf\n",x,y);
+ //pc.printf("compass: %d\n",compass);
+ //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
+ //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
+ //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());
+ }
+
+
+
+
+
+
+
+ /**************************************************
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ****************************************************/
+ /*
+ while(1){
+ vx = 0, vy = 0, vk = 0;
+ standTu = 0;
+ //dist_fun(&rateD,&rateDM,rateP);//3to-ri
+ if(ir_num<10){*/
+ /*if(rateD >= 0){//if(ir_num =2ka4) =3
+ vx = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
+ vy = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
+ }else{
+ vx = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
+ vy = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
+ }*/
+ /*
+ if(rateD >= 0){
+ vx = STRENGTH*(modulate[rateD+5]*near0[ir_num][0]+modulate[rateD+5]*go[ir_num][0]);
+ vy = STRENGTH*(modulate[rateD+5]*near0[ir_num][1]+modulate[rateD+5]*go[ir_num][1]);
+ }else{
+ vx = STRENGTH*(modulate[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
+ vy = STRENGTH*(modulate[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
+ }*/
+ /*
+ dist_fun_DM(&rateDM);//used defense first junction
+ if(count){//defense
+ if(rateDM > 0){//key point
+ dist_fun_ping(rateP);//check ping state.
+ vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
+ 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]);
+ if(ping[2]<35){
+
+ }else if(ping[2]<48){
+ vy = -30;
+ }else {
+ vy = 0;
+ vy = -35;
+ }
+ }else{*/
+ /*if(ir_num<7){
+ vx = DEFENSE_POWER*ball_state1[ir_num][0];
+ vy = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]);
+ }else if(ir_num<10){
+ vx = DEFENSE_POWER*(strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]+ball_state1[ir_num][0]);
+ vx = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]);
+ }*/
+ /*
+ dist_fun_pingSTRONG(rateP);
+ vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]);
+ 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]);
+ if(ir_num == 2||ir_num == 3||ir_num == 4){
+ vk=25;
+ }
+ }
+ }else{//ofense
+ if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){
+ //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する
+ dist_fun_ping(rateP);//check ping state.
+ vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);
+ 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]);
+ if(ping[2]<35){
+
+ }else if(ping[2]<50){
+ vy = -30;
+ }else {
+ vx = 0;
+ vy = -35;
+ }
+ }else{
+
+ dist_fun_D(&rateD);//check ball state.
+ if(rateD > 0){
+ dist_fun_pingSTRONG(rateP);
+ vx = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][0]+go[ir_num][0]);
+ vy = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][1]+go[ir_num][1]);
+ }else{
+ vx = OFFENSE_POWER*(near1[ir_num][0]+go[ir_num][0]);
+ vy = OFFENSE_POWER*(near1[ir_num][1]+go[ir_num][1]);
+ }
+ (*offense[ir_num])(&vx,&vy,&vk,rateD);
+ }
+ }
+ }
+ if(ir_num >= 10){
+ back_home(&vx,&vy);
+ }
+ */
+ /*
+ if(ir_num<10){
+ (*offense[ir_num])(&vx,&vy,&vk);
+ //vx =25*way10[ir_num][0],vy = 25*way10[ir_num][1];
+ }else{
+ clockL = 00.0;
+ back_home(&x,&y);
+ }
+
+ */
+ /*
+ line_check(line);
+ line_state(&vx,&vy,line);
+ vs = compassPID;
+ move(vx,vy,vs,vk);
+ //lcd.printf("%0.2lf clock\n\n",clockL);
+ //pc.printf("compassPID:%d\t compass:%d\n",s,compass);
+ //pc.printf("%lf %lf\n",x,y);
+ //pc.printf("compass: %d\n",compass);
+ //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
+ //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
+ //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());
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ }*/
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Apr 27 02:37:13 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/usart.cpp Sun Apr 27 02:37:13 2014 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "common.h"
+
+#define KEYCODE 0xAA
+#define TX_CHECKCODE (tx_data[1]^tx_data[2]^tx_data[3]^tx_data[4]^tx_data[5]^tx_data[6]^tx_data[7]^tx_data[8]^tx_data[9])
+#define RX_CHECKCODE (rx_data[1]^rx_data[2]^rx_data[3]^rx_data[4]^rx_data[5]^rx_data[6]^rx_data[7]^rx_data[8]^rx_data[9])
+#define DATA_NUM 11
+#define CHECK (DATA_NUM - 1)
+
+
+extern Serial sensor;
+extern Serial xbee;
+extern Serial pc;
+
+extern uint8_t ping[4];
+extern uint8_t ir_min;
+extern uint8_t ir_num;
+extern uint8_t ir_main;
+extern unsigned int compass;
+
+int count;
+
+void xbee_tx(){
+ xbee.putc(1);
+
+}
+
+void xbee_rx(){
+ if(xbee.readable()){
+ count = xbee.getc();
+ } else {
+ count = 0;
+ }
+ //pc.printf("%d\n", count);
+
+}
+
+
+void micon_rx(){
+
+ static uint8_t rx;
+ static int rx_data[DATA_NUM];
+
+ rx_data[rx] = sensor.getc();
+
+ if(rx_data[0] == KEYCODE){
+ rx++;
+ }
+
+ if(rx >= DATA_NUM){
+ if(rx_data[CHECK] == RX_CHECKCODE){
+ ir_min = rx_data[1];
+ ir_num = rx_data[2];
+ ping[FRONT] = rx_data[3];
+ ping[LEFT] = rx_data[4];
+ ping[BACK] = rx_data[5];
+ ping[RIGHT] = rx_data[6];
+ compass = rx_data[7] + rx_data[8];
+ ir_main = rx_data[9];
+ //pc.printf("compass: %d\n",compass);
+ //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
+ //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
+
+ }
+ rx = 0;
+ }
+
+ //pc.printf("%d\n", rx_data[rx]);
+}
+/*
+void micon_tx(){
+
+ static uint8_t tx;
+ static uint8_t tx_data[DATA_NUM];
+
+ if(tx >= DATA_NUM){
+ tx_data[0] = KEYCODE;
+ tx_data[1] = KEYCODE;
+ tx_data[2] = KEYCODE;
+ tx_data[3] = KEYCODE;
+ tx_data[4] = KEYCODE;
+ tx_data[5] = KEYCODE;
+ tx_data[6] = KEYCODE;
+ tx_data[7] = KEYCODE;
+ tx_data[8] = KEYCODE;
+ tx_data[9] = KEYCODE;
+ tx_data[10] = KEYCODE;
+ tx_data[11] = TX_CHECKCODE;
+
+ tx = 0;
+ }
+
+ sensor.putc(tx_data[tx]);
+ tx++;
+}
+*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/wordString.cpp Sun Apr 27 02:37:13 2014 +0000
@@ -0,0 +1,71 @@
+
+#include <sstream>
+#include "mbed.h"
+
+string StringFIN;
+
+using namespace std;
+
+
+//extern Serial pc; // tx, rx
+
+string IntToString(int number)
+{
+ stringstream ss;
+ ss << number;
+ return ss.str();
+}
+
+void array(int power1,int power2,int power3,int power4)
+{
+ int input[4] = {power1,power2,power3,power4};
+ int value = 0;
+ string StringA[4] = {"0","0","0","0"};
+
+
+ string StringX = "0";
+ string StringY = "0";
+ string StringZ = "0";
+ string String0 = "0";
+
+ StringFIN = "0";
+
+ for(uint8_t i = 0 ; i < 4; i++){
+
+ value = input[i];
+
+ StringX = IntToString(i+1);
+
+ if( (value < 0) && (value >= -100) ){
+ StringY = "R";
+ value = abs(value);
+ StringZ = IntToString(value);
+ }else if( (value >= 0) && (value <= 100) ){
+ StringY = "F";
+ StringZ = IntToString(value);
+ }else{
+ value = abs(value);
+ StringY = "F";
+ StringZ = "000";
+ }
+
+ if(value < 10){
+ String0 = "00";
+ StringZ = String0 + StringZ;
+ }else if(value < 100)
+ {
+ String0 = "0";
+ StringZ = String0 + StringZ;
+ }else{
+
+ }
+
+ StringA[i] = (StringX + StringY + StringZ);
+
+ if(i == 0)StringFIN = StringA[i];
+ else StringFIN += StringA[i];
+
+ }
+
+ StringFIN += "\r\n";
+}
\ No newline at end of file