main.h

00001 
00002 
00003 #define RATE    0.052
00004 #define Long    1.0
00005 #define ENTER   0
00006 #define EXIT    1
00007 #define X       0
00008 #define Y       1
00009 #define MOT_NUM 4
00010 #define MOTDRIVER_WAIT  300 //ms
00011 #define BAUD_RATE       115200
00012 #define BAUD_RATE2      19200
00013 #define BUT_WAIT        0.3 //s
00014 #define ON      1
00015 #define OFF     0
00016 
00017 #define PING_ERROR  0xFFFF
00018 #define PI          3.14159265
00019 
00020 #define MOT1    1.0
00021 #define MOT2    1.0
00022 #define MOT3    1.0
00023 #define MOT4    1.0
00024 
00025 #define PID_BIAS    0.0
00026 #define REFERENCE   180.0
00027 #define MINIMUM     0.0
00028 #define MAXIMUM     360.0
00029 
00030 #define PID_CYCLE   0.06    //s
00031 
00032 #define P_GAIN  0.65    //0.78   
00033 #define I_GAIN  0.0     //0.0
00034 #define D_GAIN  0.009   //0.009
00035     
00036 #define OUT_LIMIT   30.0
00037 #define MAX_POW     100
00038 #define MIN_POW     -100
00039 
00040 DigitalOut led1(LED1); 
00041 DigitalOut led2(LED2);
00042 DigitalOut led3(LED3);
00043 DigitalOut led4(LED4);
00044 BusOut mbedleds(LED4,LED3,LED2,LED1); 
00045 HMC6352 compass(p9, p10);
00046 Serial driver(p28, p27);    // tx, rx 
00047 Serial device2(p13, p14);   // tx, rx
00048 Serial pc(USBTX, USBRX);    // tx, rx 
00049 DigitalIn StartButton(p21);
00050 DigitalIn CalibEnterButton(p22);
00051 DigitalIn CalibExitButton(p23);
00052 DigitalIn EEPROMButton(p24);
00053 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);      //battery is low power version 30.0 0.58 1.0 0.015    battery is high power version   30.0 0.42, 1.0, 0.013   power is low but perfect 20.0 0.45, 0.0, 0.010
00054 Ticker pidUpdata;
00055 Ticker irDistanceUpdata;
00056 Ticker underIR;
00057 Timer timer1;
00058 Timer Survtimer;
00059 Timer lasttimer;
00060 LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
00061 
00062 enum{
00063     XNORMAL,
00064     LEFT_OUT,
00065     RIGHT_OUT,
00066 };
00067 
00068 enum{
00069     YNORMAL,
00070     FRONT_OUT,
00071     BACK_OUT,
00072 };
00073 
00074 enum{
00075     HOME_WAIT,
00076     DIFFENCE,
00077     WARNING,
00078     HOLD,
00079 };
00080 
00081 enum{
00082     NONE,
00083     ATTACK,
00084     SNAKE,
00085     SEARCH
00086 };
00087 
00088 PinName adc_num[6] = {
00089     p15,
00090     p16,
00091     p17,
00092     p18,
00093     p19,
00094     p20,
00095 };
00096 
00097 int under_val[6] = {0};
00098 
00099 int whiteout_flag = 0;
00100 
00101 double standTu = 0;
00102 int speed[MOT_NUM] = {0};
00103 uint8_t hold_flag = 0;
00104 uint8_t state = HOME_WAIT;
00105 uint8_t lineStateY = YNORMAL;
00106 uint8_t lineStateX = XNORMAL;
00107 
00108 double inputPID = 180.0;
00109 static double standard;
00110 double compassPID = 0.0;
00111 
00112 int whiteout;
00113 int whitevalue;
00114 
00115 extern int diff;
00116 
00117 extern string StringFIN;
00118 
00119 extern uint8_t direction;
00120 extern uint8_t Distance;
00121 extern uint8_t IR_found;
00122 extern uint8_t xbee;
00123 extern int irDistance[6];
00124 
00125 extern void array(int,int,int,int);
00126 
00127 extern void dev_rx(void);
00128 extern void dev_tx(void);
00129 
00130 extern uint16_t ultrasonicVal[4];
00131 
00132 int stand[6];
00133 uint8_t compFlag = 0;
00134 
00135 #define FRONT_SONIC ultrasonicVal[0]
00136 #define BACK_SONIC  ultrasonicVal[2]
00137 #define RIGHT_SONIC ultrasonicVal[1]
00138 #define LEFT_SONIC  ultrasonicVal[3]    
00139 
00140 
00141 double ball_sankaku[16][2] = {
00142     {0     , 1    },
00143     {0.390 , 0.920},
00144     {0.866 , 0.500},
00145     {0.927 , 0.374},
00146     {1     , 0    },
00147     {0.920 ,-0.390},
00148     {0.707 ,-0.707},
00149     {0.374 ,-0.927},
00150     {0     ,-1    },
00151     {-0.390,-0.920},
00152     {-0.707,-0.707},
00153     {-0.927,-0.374},
00154     {-1    , 0    },
00155     {-0.920, 0.390},
00156     {-0.866, 0.500},
00157     {-0.374, 0.927}
00158 };
00159 
00160 double turn_sankaku[16][2] = {
00161     { 0    ,0      },
00162     /*{ 1    ,0      },*/{ 0.920,-0.390 },
00163     { 0.707,-0.707 }, //{ 0.500,-0.866 },
00164     { 0.374,-0.927 },
00165     { 0    ,-0.8   },
00166     {-0.390,-0.920 },
00167     {-0.707,-0.707 },
00168     {-0.927,-0.374 },
00169     {-0.927,-0.374 },
00170     {0.920 ,-0.390 },
00171     {0.707 ,-0.707 },
00172     {0.374 ,-0.927 },
00173     {0     ,-0.8   },
00174     {-0.390,-0.920 },
00175     {-0.707,-0.707 }, //{-0.500,-0.866 },
00176     /*{-1    ,0      }*/{-0.927,-0.374 }
00177 };
00178 
00179 
00180 
00181