version 3 通信方式,マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
main.h
- Committer:
- ryuna
- Date:
- 2015-03-06
- Revision:
- 1:f91d53098d57
- Parent:
- 0:65b9e62cc2b6
- Child:
- 2:e84bb87eea71
File content as of revision 1:f91d53098d57:
/*PID処理*/ #define RATE 0.052//52 #define PID_BIAS 0.3 #define REFERENCE 180.0 #define MINIMUM 0.1 #define MAXIMUM 360.0 #define P_GAIN 1.4//1.4 //0.78 #define I_GAIN 0.0 //0.0 #define D_GAIN 0.05 //0.009 #define OUT_LIMIT 40.0 #define MAX_POW 100 #define MIN_POW -100 /*Serial Mbed間*/ #define ADDRESS 0xAA /*BusIn sw 入力値*/ #define StartS 0x01 #define Debug1 0x02 #define Debug2 0x04 #define Debug3 0x08 #define Kicker 0x10 #define CW 1 #define CCW -1 #define ENTER 0 #define EXIT 1 BusIn Sw(p22,p23,p24,p25,p26); BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up DigitalOut Led[4] = {LED1,LED2,LED3,LED4}; DigitalOut Kick(p29); Servo S555(p21); AQM0802A Lcd(p28,p27);//sda,scl MultiSerial Mbed(p9,p10);//tx,rx Serial Motor(p13,p14);//tx,rx Serial pc(USBTX,USBRX); /*Compass PID SetUp*/ PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); /* motor driver*/ extern string StringFIN; extern void array(int,int,int,int); int speed[4] = {0}; enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction; int SetRad = 0; /*Mbed sensor data */ uint8_t IrData[3] = {0},IrNum = 0; uint8_t PingData[4]={0}; unsigned int Compass = 0; uint8_t rx_data[9] = {0}; /*mbed sensor data */ volatile int Angle[180] = { 0 , 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 , 9 , 10 , 11 , 12 , 13 , 14 , 15 , 16 , 17 , 18 , 19 , 20 , 21 , 22 , 23 , 24 , 25 , 26 , 27 , 28 , 29 , 30 , 31 , 32 , 33 , 34 , 35 , 36 , 37 , 38 , 39 , 40 , 41 , 42 , 43 , 44 , 45 , 46 , 47 , 48 , 49 , 50 , 51 , 52 , 53 , 54 , 55 , 56 , 57 , 58 , 59 , 60 , 61 , 62 , 63 , 64 , 65 , 66 , 67 , 68 , 69 , 70 , 71 , 72 , 73 , 74 , 75 , 76 , 77 , 78 , 79 , 80 , 81 , 82 , 83 , 84 , 85 , 86 , 87 , 88 , 89 , 90 , -89 , -88 , -87 , -86 , -85 , -84 , -83 , -82 , -81 , -80 , -79 , -78 , -77 , -76 , -75 , -74 , -73 , -72 , -71 , -70 , -69 , -68 , -67 , -66 , -65 , -64 , -63 , -62 , -61 , -60 , -59 , -58 , -57 , -56 , -55 , -54 , -53 , -52 , -51 , -50 , -49 , -48 , -47 , -46 , -45 , -44 , -43 , -42 , -41 , -40 , -39 , -38 , -37 , -36 , -35 , -34 , -33 , -32 , -31 , -30 , -29 , -28 , -27 , -26 , -25 , -24 , -23 , -22 , -21 , -20 , -19 , -18 , -17 , -16 , -15 , -14 , -13 , -12 , -11 , -10 , -9 , -8 , -7 , -6 , -5 , -4 , -3 , -2 , -1 }; volatile int RadToVector[4] = {1,-1,-1,1}; int wrapRad[24] = { 0,// 15,//15 60,//30 80,//45 100,//60 110,//75 180,//90 180,//105 185,//120 230,//135 250,//150 260,//165 90, //180 or270 100,//195 110,//210 130,//225 175,//240 180,//255 180,//270 250,//285 260,//300 280,//315 300,//330 345//345 }; /* | | | | | o | | | | | |---------------p,q */