CatPot for defence on RoboCup in 2015 winter

Dependencies:   HMC6352 Ping IRM2121_2 mbed MultiSerial Watchdog

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }