main.h

00001 
00002 /*PID処理*/
00003 #define RATE    0.052//52
00004 #define PID_BIAS    0.2
00005 #define REFERENCE   180.0
00006 #define MINIMUM     0.0
00007 #define MAXIMUM     359.0
00008 #define P_GAIN  1.4//1.4    //0.78   
00009 #define I_GAIN  0.0     //0.0
00010 #define D_GAIN  0.05  //0.009
00011 #define OUT_LIMIT   40.0
00012 #define MAX_POW     100
00013 #define MIN_POW     -100
00014 #define PID_CYCLE   0.05   //s
00015 
00016 /*Servoなんか*/
00017 #define WAIT_TIME 0.20
00018 
00019 /*Serial Mbed間*/
00020 #define ADDRESS 0xAA
00021 
00022 
00023 /*BusIn sw　入力値*/
00024 #define StartS 0x01
00025 #define Debug1 0x02
00026 #define Debug2 0x04
00027 #define Debug3 0x08
00028 #define Debug12 0x06
00029 #define Debug23 0x0C
00030 #define Kicker 0x10
00031 
00032 #define CW 1
00033 #define CCW -1
00034 #define ENTER   0
00035 #define EXIT    1
00036 
00037 Serial Mbed(p9,p10);//tx,rx
00038 
00039 BusIn   Sw(p22,p23,p24,p25,p26);
00040 BusIn   Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
00041 DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};
00042 DigitalOut  Kick(p29);
00043 Servo       S555(p21);
00044 AQM0802A    Lcd(p28,p27);//sda,scl
00045 Serial      Motor(p13,p14);//tx,rx
00046 Serial      pc(USBTX,USBRX);
00047 
00048 
00049 /*Compass PID SetUp*/
00050 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
00051 Ticker pidupdate;
00052 
00053 
00054 /*Servo timer */
00055 Timeout STimer;
00056 
00057 extern string StringFIN;
00058 extern void array(int,int,int,int);
00059 extern void micon_rx(void);
00060 
00061 int speed[4] = {0};            
00062 
00063 
00064 
00065 double SetDegree = 0;
00066 
00067 /*Mbed sensor data */
00068 uint8_t IrData[3] = {0},IrNum = 0;
00069 uint8_t PingData[4]={0};
00070 int Compass = 0;
00071 unsigned int CompassDef = 0;
00072 double SetC = 0, InputPID = 180.0, CompassPID = 0.0;
00073 
00074 bool WaitFlag = 0;
00075 
00076 /* motor driver*/
00077 
00078 enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
00079 
00080 /*mbed sensor data */
00081 
00082 volatile  int Angle[180] = {
00083     0   ,   1   ,   2   ,   3   ,
00084     4   ,   5   ,   6   ,   7   ,
00085     8   ,   9   ,   10  ,   11  ,
00086     12  ,   13  ,   14  ,   15  ,
00087     16  ,   17  ,   18  ,   19  ,
00088     20  ,   21  ,   22  ,   23  ,
00089     24  ,   25  ,   26  ,   27  ,
00090     28  ,   29  ,   30  ,   31  ,
00091     32  ,   33  ,   34  ,   35  ,
00092     36  ,   37  ,   38  ,   39  ,
00093     40  ,   41  ,   42  ,   43  ,
00094     44  ,   45  ,   46  ,   47  ,
00095     48  ,   49  ,   50  ,   51  ,
00096     52  ,   53  ,   54  ,   55  ,
00097     56  ,   57  ,   58  ,   59  ,
00098     60  ,   61  ,   62  ,   63  ,
00099     64  ,   65  ,   66  ,   67  ,
00100     68  ,   69  ,   70  ,   71  ,
00101     72  ,   73  ,   74  ,   75  ,
00102     76  ,   77  ,   78  ,   79  ,
00103     80  ,   81  ,   82  ,   83  ,
00104     84  ,   85  ,   86  ,   87  ,
00105     88  ,   89  ,   90  ,   -89 ,
00106     -88 ,   -87 ,   -86 ,   -85 ,
00107     -84 ,   -83 ,   -82 ,   -81 ,
00108     -80 ,   -79 ,   -78 ,   -77 ,
00109     -76 ,   -75 ,   -74 ,   -73 ,
00110     -72 ,   -71 ,   -70 ,   -69 ,
00111     -68 ,   -67 ,   -66 ,   -65 ,
00112     -64 ,   -63 ,   -62 ,   -61 ,
00113     -60 ,   -59 ,   -58 ,   -57 ,
00114     -56 ,   -55 ,   -54 ,   -53 ,
00115     -52 ,   -51 ,   -50 ,   -49 ,
00116     -48 ,   -47 ,   -46 ,   -45 ,
00117     -44 ,   -43 ,   -42 ,   -41 ,
00118     -40 ,   -39 ,   -38 ,   -37 ,
00119     -36 ,   -35 ,   -34 ,   -33 ,
00120     -32 ,   -31 ,   -30 ,   -29 ,
00121     -28 ,   -27 ,   -26 ,   -25 ,
00122     -24 ,   -23 ,   -22 ,   -21 ,
00123     -20 ,   -19 ,   -18 ,   -17 ,
00124     -16 ,   -15 ,   -14 ,   -13 ,
00125     -12 ,   -11 ,   -10 ,   -9  ,
00126     -8  ,   -7  ,   -6  ,   -5  ,
00127     -4  ,   -3  ,   -2  ,   -1  };
00128 
00129 volatile int  DegreeToVector[5] = {1,-1,-1,1,1};
00130 
00131 int wrapDegree[24] = { 0,//
00132                     30,//15
00133                     60,//30
00134                     90,//45
00135                     130,//60
00136                     140,//75
00137                     170,//90
00138                     170,//105
00139                     170,//120
00140                     180,//135
00141                     200,//150
00142                     220,//165
00143                     90, //180 or270
00144                     130,//195
00145                     150,//210
00146                     180,//225
00147                     175,//240
00148                     195,//255
00149                     195,//270
00150                     240,//285
00151                     230,//300
00152                     270,//315
00153                     290,//330
00154                     320//345                
00155                     };
00156         
00157     
00158     
00159     
00160     
00161 /*
00162     |               |
00163     |               |
00164     |       o       |
00165     |               |
00166     |               |
00167     |---------------p,q
00168     
00169     
00170 */