CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
mainh.txt@3:2f74791564c9, 2015-03-11 (annotated)
- Committer:
- lilac0112_1
- Date:
- Wed Mar 11 07:47:19 2015 +0000
- Revision:
- 3:2f74791564c9
- Parent:
- main.h@2:39135c67083d
Send to F.F.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lilac0112_1 | 2:39135c67083d | 1 | |
lilac0112_1 | 2:39135c67083d | 2 | /*PID処理*/ |
lilac0112_1 | 2:39135c67083d | 3 | #define RATE 0.052//52 |
lilac0112_1 | 2:39135c67083d | 4 | #define PID_BIAS 0.3 |
lilac0112_1 | 2:39135c67083d | 5 | #define REFERENCE 180.0 |
lilac0112_1 | 2:39135c67083d | 6 | #define MINIMUM 0.1 |
lilac0112_1 | 2:39135c67083d | 7 | #define MAXIMUM 360.0 |
lilac0112_1 | 2:39135c67083d | 8 | #define P_GAIN 1.4//1.4 //0.78 |
lilac0112_1 | 2:39135c67083d | 9 | #define I_GAIN 0.0 //0.0 |
lilac0112_1 | 2:39135c67083d | 10 | #define D_GAIN 0.05 //0.009 |
lilac0112_1 | 2:39135c67083d | 11 | #define OUT_LIMIT 40.0 |
lilac0112_1 | 2:39135c67083d | 12 | #define MAX_POW 100 |
lilac0112_1 | 2:39135c67083d | 13 | #define MIN_POW -100 |
lilac0112_1 | 2:39135c67083d | 14 | |
lilac0112_1 | 2:39135c67083d | 15 | /*Serial Mbed間*/ |
lilac0112_1 | 2:39135c67083d | 16 | #define ADDRESS 0xAA |
lilac0112_1 | 2:39135c67083d | 17 | #define DATA_NUM 9 |
lilac0112_1 | 2:39135c67083d | 18 | |
lilac0112_1 | 2:39135c67083d | 19 | |
lilac0112_1 | 2:39135c67083d | 20 | /*BusIn sw 入力値*/ |
lilac0112_1 | 2:39135c67083d | 21 | #define StartS 0x01 |
lilac0112_1 | 2:39135c67083d | 22 | #define Debug1 0x02 |
lilac0112_1 | 2:39135c67083d | 23 | #define Debug2 0x04 |
lilac0112_1 | 2:39135c67083d | 24 | #define Debug3 0x08 |
lilac0112_1 | 2:39135c67083d | 25 | #define Kicker 0x10 |
lilac0112_1 | 2:39135c67083d | 26 | |
lilac0112_1 | 2:39135c67083d | 27 | #define CW 1 |
lilac0112_1 | 2:39135c67083d | 28 | #define CCW -1 |
lilac0112_1 | 2:39135c67083d | 29 | #define ENTER 0 |
lilac0112_1 | 2:39135c67083d | 30 | #define EXIT 1 |
lilac0112_1 | 2:39135c67083d | 31 | |
lilac0112_1 | 2:39135c67083d | 32 | |
lilac0112_1 | 2:39135c67083d | 33 | |
lilac0112_1 | 2:39135c67083d | 34 | BusIn Sw(p22,p23,p24,p25,p26); |
lilac0112_1 | 2:39135c67083d | 35 | BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up |
lilac0112_1 | 2:39135c67083d | 36 | DigitalOut Led[4] = {LED1,LED2,LED3,LED4}; |
lilac0112_1 | 2:39135c67083d | 37 | DigitalOut Kick(p29); |
lilac0112_1 | 2:39135c67083d | 38 | Servo S555(p21); |
lilac0112_1 | 2:39135c67083d | 39 | AQM0802A Lcd(p28,p27);//sda,scl |
lilac0112_1 | 2:39135c67083d | 40 | MultiSerial Mbed(p9,p10);//tx,rx |
lilac0112_1 | 2:39135c67083d | 41 | Serial Motor(p13,p14);//tx,rx |
lilac0112_1 | 2:39135c67083d | 42 | Serial pc(USBTX,USBRX); |
lilac0112_1 | 2:39135c67083d | 43 | |
lilac0112_1 | 2:39135c67083d | 44 | Ticker Monitor; |
lilac0112_1 | 2:39135c67083d | 45 | |
lilac0112_1 | 2:39135c67083d | 46 | |
lilac0112_1 | 2:39135c67083d | 47 | /*Compass PID SetUp*/ |
lilac0112_1 | 2:39135c67083d | 48 | PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); |
lilac0112_1 | 2:39135c67083d | 49 | |
lilac0112_1 | 2:39135c67083d | 50 | /* motor driver*/ |
lilac0112_1 | 2:39135c67083d | 51 | extern string StringFIN; |
lilac0112_1 | 2:39135c67083d | 52 | extern void array(int,int,int,int); |
lilac0112_1 | 2:39135c67083d | 53 | int speed[4] = {0}; |
lilac0112_1 | 2:39135c67083d | 54 | |
lilac0112_1 | 2:39135c67083d | 55 | |
lilac0112_1 | 2:39135c67083d | 56 | /*PID補正move加算値 Revise */ |
lilac0112_1 | 2:39135c67083d | 57 | double ReV = 0.0; |
lilac0112_1 | 2:39135c67083d | 58 | |
lilac0112_1 | 2:39135c67083d | 59 | /*Compass*/ |
lilac0112_1 | 2:39135c67083d | 60 | int CompassDef = 0; |
lilac0112_1 | 2:39135c67083d | 61 | double SetAngle = 0; |
lilac0112_1 | 2:39135c67083d | 62 | |
lilac0112_1 | 2:39135c67083d | 63 | enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction; |
lilac0112_1 | 2:39135c67083d | 64 | |
lilac0112_1 | 2:39135c67083d | 65 | int SetRad = 0; |
lilac0112_1 | 2:39135c67083d | 66 | |
lilac0112_1 | 2:39135c67083d | 67 | /*StartLoop*/ |
lilac0112_1 | 2:39135c67083d | 68 | bool loop; |
lilac0112_1 | 2:39135c67083d | 69 | |
lilac0112_1 | 2:39135c67083d | 70 | /*Mbed sensor data */ |
lilac0112_1 | 2:39135c67083d | 71 | uint8_t IrData[3] = {0},IrNum = 0;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態 |
lilac0112_1 | 2:39135c67083d | 72 | uint8_t PingData[4]={0}; |
lilac0112_1 | 2:39135c67083d | 73 | unsigned int Compass = 0; |
lilac0112_1 | 2:39135c67083d | 74 | |
lilac0112_1 | 2:39135c67083d | 75 | uint8_t rx_data[DATA_NUM]; |
lilac0112_1 | 2:39135c67083d | 76 | /*mbed sensor data */ |
lilac0112_1 | 2:39135c67083d | 77 | |
lilac0112_1 | 2:39135c67083d | 78 | volatile int Angle[180] = { |
lilac0112_1 | 2:39135c67083d | 79 | 0 , 1 , 2 , 3 , |
lilac0112_1 | 2:39135c67083d | 80 | 4 , 5 , 6 , 7 , |
lilac0112_1 | 2:39135c67083d | 81 | 8 , 9 , 10 , 11 , |
lilac0112_1 | 2:39135c67083d | 82 | 12 , 13 , 14 , 15 , |
lilac0112_1 | 2:39135c67083d | 83 | 16 , 17 , 18 , 19 , |
lilac0112_1 | 2:39135c67083d | 84 | 20 , 21 , 22 , 23 , |
lilac0112_1 | 2:39135c67083d | 85 | 24 , 25 , 26 , 27 , |
lilac0112_1 | 2:39135c67083d | 86 | 28 , 29 , 30 , 31 , |
lilac0112_1 | 2:39135c67083d | 87 | 32 , 33 , 34 , 35 , |
lilac0112_1 | 2:39135c67083d | 88 | 36 , 37 , 38 , 39 , |
lilac0112_1 | 2:39135c67083d | 89 | 40 , 41 , 42 , 43 , |
lilac0112_1 | 2:39135c67083d | 90 | 44 , 45 , 46 , 47 , |
lilac0112_1 | 2:39135c67083d | 91 | 48 , 49 , 50 , 51 , |
lilac0112_1 | 2:39135c67083d | 92 | 52 , 53 , 54 , 55 , |
lilac0112_1 | 2:39135c67083d | 93 | 56 , 57 , 58 , 59 , |
lilac0112_1 | 2:39135c67083d | 94 | 60 , 61 , 62 , 63 , |
lilac0112_1 | 2:39135c67083d | 95 | 64 , 65 , 66 , 67 , |
lilac0112_1 | 2:39135c67083d | 96 | 68 , 69 , 70 , 71 , |
lilac0112_1 | 2:39135c67083d | 97 | 72 , 73 , 74 , 75 , |
lilac0112_1 | 2:39135c67083d | 98 | 76 , 77 , 78 , 79 , |
lilac0112_1 | 2:39135c67083d | 99 | 80 , 81 , 82 , 83 , |
lilac0112_1 | 2:39135c67083d | 100 | 84 , 85 , 86 , 87 , |
lilac0112_1 | 2:39135c67083d | 101 | 88 , 89 , 90 , -89 , |
lilac0112_1 | 2:39135c67083d | 102 | -88 , -87 , -86 , -85 , |
lilac0112_1 | 2:39135c67083d | 103 | -84 , -83 , -82 , -81 , |
lilac0112_1 | 2:39135c67083d | 104 | -80 , -79 , -78 , -77 , |
lilac0112_1 | 2:39135c67083d | 105 | -76 , -75 , -74 , -73 , |
lilac0112_1 | 2:39135c67083d | 106 | -72 , -71 , -70 , -69 , |
lilac0112_1 | 2:39135c67083d | 107 | -68 , -67 , -66 , -65 , |
lilac0112_1 | 2:39135c67083d | 108 | -64 , -63 , -62 , -61 , |
lilac0112_1 | 2:39135c67083d | 109 | -60 , -59 , -58 , -57 , |
lilac0112_1 | 2:39135c67083d | 110 | -56 , -55 , -54 , -53 , |
lilac0112_1 | 2:39135c67083d | 111 | -52 , -51 , -50 , -49 , |
lilac0112_1 | 2:39135c67083d | 112 | -48 , -47 , -46 , -45 , |
lilac0112_1 | 2:39135c67083d | 113 | -44 , -43 , -42 , -41 , |
lilac0112_1 | 2:39135c67083d | 114 | -40 , -39 , -38 , -37 , |
lilac0112_1 | 2:39135c67083d | 115 | -36 , -35 , -34 , -33 , |
lilac0112_1 | 2:39135c67083d | 116 | -32 , -31 , -30 , -29 , |
lilac0112_1 | 2:39135c67083d | 117 | -28 , -27 , -26 , -25 , |
lilac0112_1 | 2:39135c67083d | 118 | -24 , -23 , -22 , -21 , |
lilac0112_1 | 2:39135c67083d | 119 | -20 , -19 , -18 , -17 , |
lilac0112_1 | 2:39135c67083d | 120 | -16 , -15 , -14 , -13 , |
lilac0112_1 | 2:39135c67083d | 121 | -12 , -11 , -10 , -9 , |
lilac0112_1 | 2:39135c67083d | 122 | -8 , -7 , -6 , -5 , |
lilac0112_1 | 2:39135c67083d | 123 | -4 , -3 , -2 , -1 }; |
lilac0112_1 | 2:39135c67083d | 124 | |
lilac0112_1 | 2:39135c67083d | 125 | volatile int RadToVector[4] = {1,-1,-1,1}; |
lilac0112_1 | 2:39135c67083d | 126 | |
lilac0112_1 | 2:39135c67083d | 127 | int wrapRad[24] = { 0,// |
lilac0112_1 | 2:39135c67083d | 128 | 15,//15 |
lilac0112_1 | 2:39135c67083d | 129 | 60,//30 |
lilac0112_1 | 2:39135c67083d | 130 | 80,//45 |
lilac0112_1 | 2:39135c67083d | 131 | 100,//60 |
lilac0112_1 | 2:39135c67083d | 132 | 110,//75 |
lilac0112_1 | 2:39135c67083d | 133 | 180,//90 |
lilac0112_1 | 2:39135c67083d | 134 | 180,//105 |
lilac0112_1 | 2:39135c67083d | 135 | 185,//120 |
lilac0112_1 | 2:39135c67083d | 136 | 230,//135 |
lilac0112_1 | 2:39135c67083d | 137 | 250,//150 |
lilac0112_1 | 2:39135c67083d | 138 | 260,//165 |
lilac0112_1 | 2:39135c67083d | 139 | 90, //180 or270 |
lilac0112_1 | 2:39135c67083d | 140 | 100,//195 |
lilac0112_1 | 2:39135c67083d | 141 | 110,//210 |
lilac0112_1 | 2:39135c67083d | 142 | 130,//225 |
lilac0112_1 | 2:39135c67083d | 143 | 175,//240 |
lilac0112_1 | 2:39135c67083d | 144 | 180,//255 |
lilac0112_1 | 2:39135c67083d | 145 | 180,//270 |
lilac0112_1 | 2:39135c67083d | 146 | 250,//285 |
lilac0112_1 | 2:39135c67083d | 147 | 260,//300 |
lilac0112_1 | 2:39135c67083d | 148 | 280,//315 |
lilac0112_1 | 2:39135c67083d | 149 | 300,//330 |
lilac0112_1 | 2:39135c67083d | 150 | 345//345 |
lilac0112_1 | 2:39135c67083d | 151 | }; |
lilac0112_1 | 2:39135c67083d | 152 | |
lilac0112_1 | 2:39135c67083d | 153 | |
lilac0112_1 | 2:39135c67083d | 154 | |
lilac0112_1 | 2:39135c67083d | 155 | |
lilac0112_1 | 2:39135c67083d | 156 | |
lilac0112_1 | 2:39135c67083d | 157 | /* |
lilac0112_1 | 2:39135c67083d | 158 | | | |
lilac0112_1 | 2:39135c67083d | 159 | | | |
lilac0112_1 | 2:39135c67083d | 160 | | o | |
lilac0112_1 | 2:39135c67083d | 161 | | | |
lilac0112_1 | 2:39135c67083d | 162 | | | |
lilac0112_1 | 2:39135c67083d | 163 | |---------------p,q |
lilac0112_1 | 2:39135c67083d | 164 | |
lilac0112_1 | 2:39135c67083d | 165 | |
lilac0112_1 | 2:39135c67083d | 166 | */ |