CatPot for defence on RoboCup in 2015 winter
Dependencies: HMC6352 Ping IRM2121_2 mbed MultiSerial Watchdog
main.cpp
00001 /* 00002 *main用LPC1768と通信するSensor用LPC1768のプログラム() 00003 * 00004 *現在は2つのLPC1768でSerial通信します. 00005 * 00006 */ 00007 00008 #include "mbed.h" 00009 00010 #include "IRM2121.h" 00011 #include "Ping.h" 00012 #include "MultiSerial.h" 00013 #include "HMC6352.h" 00014 #include "Watchdog.h" 00015 00016 #define PING_COUNT 5 //5回に1回にPing処理 00017 #define IR_LEVEL 400 //irのしきい値 00018 #define ADDRESS 0xAA// 00019 #define DATA_NUM 9 00020 00021 uint8_t tx_data[DATA_NUM]={0}; 00022 00023 IRM2121 Ir[12] = {p20,p19,p18,p17,p16,p15, p12,p11, p8,p7,p6,p5}; 00024 00025 Ping Ping1(p22, p21); 00026 Ping Ping2(p24, p23); 00027 Ping Ping3(p26, p25); 00028 Ping Ping4(p30, p29); 00029 00030 DigitalOut Led[4] = {LED1, LED2, LED3, LED4}; 00031 00032 HMC6352 hmc6352(p28, p27); 00033 00034 MultiSerial Mbed(p13, p14); 00035 //MultiSerial mbed(p9, p10); 00036 00037 Serial pc(USBTX,USBRX); 00038 00039 /*irm2121平均化*/ 00040 uint8_t IrMoving_ave(uint8_t num, int data){ 00041 static unsigned int sum[10]; 00042 static unsigned temp[6][10]; 00043 00044 sum[num] -= temp[num][9]; 00045 sum[num] += data; 00046 00047 00048 temp[num][9] = temp[num][8]; 00049 temp[num][8] = temp[num][7]; 00050 temp[num][7] = temp[num][6]; 00051 temp[num][6] = temp[num][5]; 00052 temp[num][5] = temp[num][4]; 00053 temp[num][4] = temp[num][3]; 00054 temp[num][3] = temp[num][2]; 00055 temp[num][2] = temp[num][1]; 00056 temp[num][1] = temp[num][0]; 00057 temp[num][0] = data; 00058 00059 return sum[num]/10; 00060 } 00061 00062 void IrSort(unsigned int input[], uint8_t output[]){ 00063 00064 /* 00065 *1,2位の値とその場所の番号をアウトプットに格納する. 00066 *値が255以上だとボールを検知してないことになる。 00067 */ 00068 unsigned int near[2] = {1000,1000}; 00069 uint8_t num[2] = {12,0}, i; 00070 static int sub[12]={0}; 00071 00072 for(i = 0; i < 12; i++ ){ 00073 if(!input[i]){ 00074 continue; 00075 } 00076 00077 if(near[1] < input[i]+sub[i]){ 00078 continue; 00079 } 00080 if(near[0] < input[i]+sub[i]){ 00081 near[1] = input[i]+sub[i]; 00082 num[1] = i; 00083 continue; 00084 } 00085 near[1] = near[0]; 00086 num [1] = num[0]; 00087 near[0] = input[i]+sub[i]; 00088 num [0] = i; 00089 } 00090 00091 if(num[0] == 12){//not found 00092 output[0] = 255; 00093 output[1] = 255; 00094 output[2] = 255; 00095 return; 00096 } 00097 if(num[1] == 12){ 00098 num[1] = 0; 00099 } 00100 /* 00101 for(i = 0; i < 12; i++ ){ 00102 00103 if(i == num[0]){ 00104 00105 sub[i]++; 00106 continue; 00107 } 00108 if(sub[i]>0){ 00109 00110 sub[i]--; 00111 } 00112 } 00113 */ 00114 output[0] = num[0]*12 + num[1];//rank1*12 + rank2 00115 output[1] = near[0] /10;//rank1 00116 output[2] = near[1] /10;//rank2 00117 00118 } 00119 00120 00121 00122 int main() { 00123 00124 /*begin SetUp*/ 00125 Watchdog g; 00126 g.Configure(1.0f);//1秒でWDT発生 00127 /*end Setup*/ 00128 00129 00130 /*Count*/ 00131 uint8_t PingCk = 0; 00132 00133 /*Data*/ 00134 unsigned int PingData[4] = {0}; 00135 unsigned int IrBase[12] = {0}; 00136 uint8_t IrData[3] = {0}; 00137 uint16_t Compass=0; 00138 00139 /*Serial*/ 00140 00141 00142 Mbed.start_write(); 00143 Mbed.write_data(tx_data, ADDRESS, DATA_NUM); 00144 00145 //Continuous mode, periodic set/reset, 20Hz measurement rate. 00146 hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20); 00147 00148 while(1) { 00149 00150 g.Service(); 00151 00152 PingCk ++; 00153 00154 Led[0] = 1;//周回の初め 00155 00156 00157 00158 IrBase[0] = Ir[0].Read(); 00159 IrBase[1] = Ir[1].Read(); 00160 IrBase[2] = Ir[2].Read(); 00161 IrBase[3] = Ir[3].Read(); 00162 IrBase[4] = Ir[4].Read(); 00163 IrBase[5] = Ir[5].Read(); 00164 IrBase[6] = Ir[6].Read(); 00165 IrBase[7] = Ir[7].Read(); 00166 IrBase[8] = Ir[8].Read(); 00167 IrBase[9] = Ir[9].Read(); 00168 IrBase[10] = Ir[10].Read(); 00169 IrBase[11] = Ir[11].Read(); 00170 00171 /*平均化 00172 IrTemp[0] = IrMoving_ave(0,IrTemp[0]); 00173 IrTemp[1] = IrMoving_ave(1,IrTemp[1]); 00174 IrTemp[2] = IrMoving_ave(2,IrTemp[2]); 00175 IrTemp[3] = IrMoving_ave(3,IrTemp[3]); 00176 IrTemp[4] = IrMoving_ave(4,IrTemp[4]); 00177 IrTemp[5] = IrMoving_ave(5,IrTemp[5]); 00178 */ 00179 00180 IrSort(IrBase,IrData); 00181 00182 if(PingCk == 2){ 00183 00184 Ping1.Send(); 00185 wait_ms(30); 00186 PingData[0] = Ping1.Read_cm(); 00187 if(PingData[0]>0xFF) PingData[0]=0xFF; 00188 } 00189 if(PingCk == 3){ 00190 00191 Ping2.Send(); 00192 wait_ms(30); 00193 PingData[1] = Ping2.Read_cm(); 00194 if(PingData[1]>0xFF) PingData[1]=0xFF; 00195 } 00196 if(PingCk == 4){ 00197 00198 Ping3.Send(); 00199 wait_ms(30); 00200 PingData[2] = Ping3.Read_cm(); 00201 if(PingData[2]>0xFF) PingData[2]=0xFF; 00202 } 00203 if(PingCk >= PING_COUNT){ 00204 00205 Ping4.Send(); 00206 wait_ms(30); 00207 PingData[3] = Ping4.Read_cm(); 00208 if(PingData[3]>0xFF) PingData[3]=0xFF; 00209 00210 PingCk = 0; 00211 } 00212 00213 Compass = hmc6352.sample(); 00214 00215 //ex. 00216 tx_data[0] = IrData[0]; 00217 tx_data[1] = IrData[1]; 00218 tx_data[2] = IrData[2]; 00219 tx_data[3] = PingData[0]; 00220 tx_data[4] = PingData[1]; 00221 tx_data[5] = PingData[2]; 00222 tx_data[6] = PingData[3]; 00223 tx_data[7] = (Compass & 0x00FF)>>0; 00224 tx_data[8] = (Compass & 0xFF00)>>8; 00225 00226 00227 for(int i=0; i<7; i++){ 00228 pc.printf("%d\t", tx_data[i]); 00229 } 00230 00231 pc.printf("%d\t", Compass); 00232 00233 pc.printf("\n"); 00234 00235 00236 Led[0] = 0;//周回の終わり 00237 00238 wait_ms(1); 00239 } 00240 }
Generated on Sun Jul 17 2022 01:32:37 by 1.7.2