CatPot for defence on RoboCup in 2015 winter

Dependencies:   HMC6352 Ping IRM2121_2 mbed MultiSerial Watchdog

Files at this revision

API Documentation at this revision

Comitter:
lilac0112_1
Date:
Wed Mar 11 01:11:40 2015 +0000
Parent:
0:94ced082e13a
Commit message:
aaaaaaaa

Changed in this revision

HMC6352.lib Show annotated file Show diff for this revision Revisions of this file
IRM2121.lib Show diff for this revision Revisions of this file
IRM2121_2.lib Show annotated file Show diff for this revision Revisions of this file
MultiSerial.lib Show annotated file Show diff for this revision Revisions of this file
Watchdog.lib Show annotated file Show diff for this revision Revisions of this file
interruptin_mod.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Wed Mar 11 01:11:40 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099
--- a/IRM2121.lib	Sat Jan 24 05:47:18 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/ryuna/code/IRM2121/#40101fcb6d44
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IRM2121_2.lib	Wed Mar 11 01:11:40 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/2014/code/IRM2121_2/#bb38fe5771fb
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MultiSerial.lib	Wed Mar 11 01:11:40 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/B/code/MultiSerial/#f2f3c3d6a6be
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Watchdog.lib	Wed Mar 11 01:11:40 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/WiredHome/code/Watchdog/#22c5c4aa4661
--- a/interruptin_mod.lib	Sat Jan 24 05:47:18 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/bousiya03/code/interruptin_mod/#1cc9d9e3cf8c
--- a/main.cpp	Sat Jan 24 05:47:18 2015 +0000
+++ b/main.cpp	Wed Mar 11 01:11:40 2015 +0000
@@ -1,82 +1,50 @@
 /*
- *メインと通信するLPC1114のプログラム()
- *
- *現在は両方がmainと通信します.
- *
- *mainの通信回数が多いためそれに耐えうる速度で通信しなければならないのではという懸念。
- *試してみないとわからないが、RTOSにより常に通信のみを監視するのもありかもしれない。
- *それでダメなら詰む。
- */
-/***********************************
- *RoboCupJunior Soccer B Open 2014 
- *Koshinestu progrum
- *
- *  このプログラムでは以下の処理をする.
- *
- * 主にセンサーデータの作成を行う.
- *
- * IRM2121を6(12/2)個用いてボールの位置を読み取る.
- * 
- * フォトダイオードを4(8/2)個用いてラインを読み取る.
- * 
- * 超音波センサを2個用いて周囲の壁との距離を読み取る.
- *
- *  LPC1768へI2Cを用いて各種センサーデータを送信する.
+ *main用LPC1768と通信するSensor用LPC1768のプログラム()
  *
- ************************* 
+ *現在は2つのLPC1768でSerial通信します.
  * 
- *Pin Map
- *
- *  dp17,dp18,dp24,dp25,dp26,NC >> IRM2121
- *  
- *  dp1,dp2 >> Ping1
- *
- *  dp4,dp6 >> Ping2
- *
- *  dp9,dp10,dp11,dp13 >> PhotoDiode
- *
- *  dp5,dp27 >> I2C >> LPC1768
- *
- ******************************/
-
+ */
+ 
 #include "mbed.h"
-/*
-#define SYSAHBCLKDIV_Val      0x00000001        // Reset: 0x001
-#define CLOCK_SETUP           1                  // デフォルト=1
-#define SYSPLLCTRL_Val        0x00000023        // デフォルト=23 Reset: 0x000
-#define SYSPLLCLKSEL_Val      0x00000001        // デフォルト=0  Reset: 0x000
-#define MAINCLKSEL_Val        0x00000003        // デフォルト=3  Reset: 0x000
-*/
+ 
 #include "IRM2121.h"
 #include "Ping.h"
-
+#include "MultiSerial.h"
+#include "HMC6352.h"
+#include "Watchdog.h"
+ 
 #define PING_COUNT 5 //5回に1回にPing処理
-#define LINE_LEVEL (1-0.55) //しきい値を設定 0.55がしきい値
 #define IR_LEVEL 400 //irのしきい値
+#define ADDRESS 0xAA//
+#define DATA_NUM 9
+ 
+uint8_t tx_data[DATA_NUM]={0};
 
-#define I2C_ADDRESS 0xAA // another (0xAC)
-
-
+IRM2121 Ir[12] = {p20,p19,p18,p17,p16,p15,  p12,p11,  p8,p7,p6,p5};
 
-IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,NC};
-Ping Ping1(dp1,dp2);
-Ping Ping2(dp4,dp6);
-AnalogIn Line[4] = {dp9,dp10,dp11,dp13};
-DigitalOut Led(LED1);
+Ping Ping1(p22, p21);
+Ping Ping2(p24, p23);
+Ping Ping3(p26, p25);
+Ping Ping4(p30, p29);
+
+DigitalOut Led[4] = {LED1, LED2, LED3, LED4};
 
-I2CSlave Mbed(dp5,dp27);//sda,scl
-Serial pc(USBTX,USBRX);
+HMC6352 hmc6352(p28, p27);
+
+MultiSerial Mbed(p13, p14);
+//MultiSerial mbed(p9, p10);
 
+Serial pc(USBTX,USBRX);
+ 
 /*irm2121平均化*/
-uint8_t IrMoving_ave(uint8_t num, int data)
-{
+uint8_t IrMoving_ave(uint8_t num, int data){
     static unsigned int sum[10];
     static unsigned temp[6][10];
-
-    sum[num] -= temp[num][9];
+    
+    sum[num] -= temp[num][9]; 
     sum[num] += data;
-
-
+    
+    
     temp[num][9] = temp[num][8];
     temp[num][8] = temp[num][7];
     temp[num][7] = temp[num][6];
@@ -87,188 +55,186 @@
     temp[num][2] = temp[num][1];
     temp[num][1] = temp[num][0];
     temp[num][0] = data;
-
-    return  sum[num]/10;
+    
+    return  sum[num]/10;   
 }
-
-void IrDataSelect(int input[], uint8_t output[])
-{
+ 
+void IrSort(unsigned int input[], uint8_t output[]){
+    
     /*
      *1,2位の値とその場所の番号をアウトプットに格納する.
      *値が255以上だとボールを検知してないことになる。
      */
-    uint8_t nigh1 = 255, nigh2 = 255;
-    uint8_t ni1=12, ni2 = 12, i;
-
-    for(i = 6; i > 0; i-- ) {
-        if(nigh2 < input[i]) {
+    unsigned int  near[2] = {1000,1000};
+    uint8_t num[2] = {12,0}, i;
+    static int sub[12]={0};
+    
+    for(i = 0; i < 12; i++ ){
+        if(!input[i]){
             continue;
         }
-        if(nigh1 < input[i]) {
-            nigh2 = input[i];
-            ni2 = i;
+        
+        if(near[1] < input[i]+sub[i]){
             continue;
         }
-        nigh2 = nigh1;
-        ni2   = ni1;
-        nigh1 = input[i];
-        ni1   = i;
+        if(near[0] < input[i]+sub[i]){
+            near[1] = input[i]+sub[i];
+            num[1] = i;
+            continue;
+        }
+        near[1] = near[0];
+        num [1]  = num[0];
+        near[0] = input[i]+sub[i];
+        num [0]   = i;
     }
-    /*
-    for(i = 0; i < 6; i++){
-
-        if(nigh1 > input[i]){
-            nigh1 = input[i];
-            ni1 = i;
-        }
-        if((nigh1<input[i])&&(nigh2>input[i])){
-            nigh2 = input[i];
-            ni2 = i;
-        }
-    }*/
-    if(ni1 == 12) { //ボールがないとき
-        output[0] = 0;
+ 
+    if(num[0] == 12){//not found
+        output[0] = 255;
         output[1] = 255;
         output[2] = 255;
         return;
     }
-
-    output[0] = ni1*6 + ni2;
-    output[1] = nigh1;
-    output[2] = nigh2;
-
-}
-/*周回の初期動作*/
-void ResetSet(uint8_t *PingCk)
-{
-
-    Led = 1;//周回の初め
-    PingCk ++;
-
+    if(num[1] == 12){
+        num[1] = 0;
+    }
+    /*
+    for(i = 0; i < 12; i++ ){
+        
+        if(i == num[0]){
+            
+            sub[i]++;
+            continue;
+        }
+        if(sub[i]>0){
+            
+            sub[i]--;
+        }
+    }
+    */
+    output[0] = num[0]*12 + num[1];//rank1*12 + rank2
+    output[1] = near[0] /10;//rank1
+    output[2] = near[1] /10;//rank2
+    
 }
-
-
-
-int main()
-{
-
-    //default Mbed.frequency();
-    Mbed.address(I2C_ADDRESS);
-    /*Data*/
-    int IrMemory[6] = {0};//IRのデータ保管
-    uint8_t PingMemory[2] = {0};
-    unsigned int LineMemory[4] = {0};
-
-    int IrTemp[6] = {0};//仮置き場
-    uint8_t IrSendData[3];//[0]:どのirを指すのかアドレスで
-
-    /*Flag*/
-    uint8_t LineFlag;//配置 - - - - Line[3] Line[2] Line[1] Line[0]
-
-    /*State(operate_value etc...)*/
-    int I2CState;
-
+ 
+ 
+ 
+int main() {
+    
+    /*begin SetUp*/
+    Watchdog g;
+    g.Configure(1.0f);//1秒でWDT発生
+    /*end   Setup*/
+    
+    
     /*Count*/
     uint8_t PingCk = 0;
-
-    /* I2Cdata*/
-    char SendData[9]= {0};
-    char AddressData = 0;
-
-    /*Debug valiable*/
-    bool valid = 0,busy = 0;
-
+    
+    /*Data*/
+    unsigned int    PingData[4] = {0};
+    unsigned int    IrBase[12] = {0};
+    uint8_t         IrData[3] = {0};
+    uint16_t        Compass=0;
+    
+    /*Serial*/
+    
+    
+    Mbed.start_write();
+    Mbed.write_data(tx_data, ADDRESS, DATA_NUM);
+    
+    //Continuous mode, periodic set/reset, 20Hz measurement rate.
+    hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ 
     while(1) {
-
-        ResetSet(&PingCk);
-        Ir[0].Set();
-        Ir[1].Set();
-        Ir[2].Set();
-        Ir[3].Set();
-        Ir[4].Set();
-        //Ir[5].Set();
-
-        wait_ms(50);//wait値は最適値を探す必要がある.
-
-        IrTemp[0] = IR_LEVEL - Ir[0].Read();
-        IrTemp[1] = IR_LEVEL - Ir[1].Read();
-        IrTemp[2] = IR_LEVEL - Ir[2].Read();
-        IrTemp[3] = IR_LEVEL - Ir[3].Read();
-        IrTemp[4] = IR_LEVEL - Ir[4].Read();
-        //IrTemp[5] = IR_LEVEL - Ir[5].Read();
-
-        Ir[0].ReturnVB(&valid,&busy);
-        pc.printf("valid %d, busy %d \n\r",valid, busy);
-        /*
-        IrMemory[0] = IrMoving_ave(0,IrTemp[0]);
-        IrMemory[1] = IrMoving_ave(1,IrTemp[1]);
-        IrMemory[2] = IrMoving_ave(2,IrTemp[2]);
-        IrMemory[3] = IrMoving_ave(3,IrTemp[3]);
-        IrMemory[4] = IrMoving_ave(4,IrTemp[4]);
-        //IrMemory[5] = IrMoving_ave(5,IrTemp[5]);
+        
+        g.Service();
+        
+        PingCk ++;
+        
+        Led[0] = 1;//周回の初め    
+        
+        
+        
+        IrBase[0]  = Ir[0].Read();
+        IrBase[1]  = Ir[1].Read();
+        IrBase[2]  = Ir[2].Read();
+        IrBase[3]  = Ir[3].Read();
+        IrBase[4]  = Ir[4].Read();
+        IrBase[5]  = Ir[5].Read();
+        IrBase[6]  = Ir[6].Read();
+        IrBase[7]  = Ir[7].Read();
+        IrBase[8]  = Ir[8].Read();
+        IrBase[9]  = Ir[9].Read();
+        IrBase[10] = Ir[10].Read();
+        IrBase[11] = Ir[11].Read();
+        
+        /*平均化
+        IrTemp[0] = IrMoving_ave(0,IrTemp[0]);
+        IrTemp[1] = IrMoving_ave(1,IrTemp[1]);
+        IrTemp[2] = IrMoving_ave(2,IrTemp[2]);
+        IrTemp[3] = IrMoving_ave(3,IrTemp[3]);
+        IrTemp[4] = IrMoving_ave(4,IrTemp[4]);
+        IrTemp[5] = IrMoving_ave(5,IrTemp[5]);
         */
-
-        /*小さい(大きい)二つだけでいい気がする.値をみて小さかったらそうしょう*/
-        /**/
-        //IrDataSelect(IrMemory,IrSendData);
-        /**/
-        /*
+        
+        IrSort(IrBase,IrData);
+        
+        if(PingCk == 2){
+            
+            Ping1.Send();    
+            wait_ms(30);
+            PingData[0] = Ping1.Read_cm();
+            if(PingData[0]>0xFF) PingData[0]=0xFF;
+        }
+        if(PingCk == 3){
+            
+            Ping2.Send();    
+            wait_ms(30);
+            PingData[1] = Ping2.Read_cm();
+            if(PingData[1]>0xFF) PingData[1]=0xFF;
+        }
+        if(PingCk == 4){
+            
+            Ping3.Send();    
+            wait_ms(30);
+            PingData[2] = Ping3.Read_cm();
+            if(PingData[2]>0xFF) PingData[2]=0xFF;
+        }
         if(PingCk >= PING_COUNT){
-            Ping1.Send();
-            Ping2.Send();
+            
+            Ping4.Send();    
             wait_ms(30);
-            PingMemory[0] = Ping1.Read_cm();
-            PingMemory[1] = Ping2.Read_cm();
+            PingData[3] = Ping4.Read_cm();
+            if(PingData[3]>0xFF) PingData[3]=0xFF;
+            
             PingCk = 0;
         }
-        */
-        LineMemory[0] = Line[0];
-        LineMemory[1] = Line[1];
-        LineMemory[2] = Line[2];
-        LineMemory[3] = Line[3];
-
-        LineFlag = (((int)(LineMemory[3]+LINE_LEVEL)) | ((int)(LineMemory[2]+LINE_LEVEL)))<<1 +
-                   (((int)(LineMemory[1]+LINE_LEVEL)) | ((int)(LineMemory[0]+LINE_LEVEL))); //まとめてる
-        /*
-        I2CState = Mbed.receive();
-
-        switch(I2CState){
-            case I2CSlave::ReadAddressed:
-                 if(AddressData){
-                    switch(AddressData){
-                        case 0x01://IR
-                                SendData[0] = (char)IrSendData[0];
-                                SendData[1] = (char)IrSendData[1];
-                                SendData[2] = (char)IrSendData[2];
-                                Mbed.write(SendData, 3);
-                                break;
-
-                        case 0x02://ping
-                                SendData[0] = (char)PingMemory[0];
-                                SendData[1] = (char)PingMemory[1];
-                                Mbed.write(SendData, 2);
-                                break;
-                        default:
-                                break;
-                    }
-                 }else{//line
-                    SendData[0] =  (char)LineFlag;
-                    Mbed.write(SendData, 1);
-                 }
-                 AddressData = 0;
-                 break;
-            case I2CSlave::WriteGeneral:
-                 AddressData = Mbed.read();
-                 break;
-            case I2CSlave::WriteAddressed:
-                 AddressData = Mbed.read();
-                 break;
-
+        
+        Compass = hmc6352.sample();
+        
+        //ex.
+        tx_data[0] = IrData[0];
+        tx_data[1] = IrData[1];
+        tx_data[2] = IrData[2];
+        tx_data[3] = PingData[0];
+        tx_data[4] = PingData[1];
+        tx_data[5] = PingData[2];
+        tx_data[6] = PingData[3];
+        tx_data[7] = (Compass & 0x00FF)>>0;
+        tx_data[8] = (Compass & 0xFF00)>>8;
+        
+        
+        for(int i=0; i<7; i++){
+            pc.printf("%d\t", tx_data[i]);
         }
-        */
-        pc.printf("%d  %d %d %d %d %d\n\r",IrTemp[0],IrTemp[1],IrTemp[2],IrTemp[3],IrTemp[4],IrTemp[5]);
-
-        Led = 0;
-
+        
+        pc.printf("%d\t", Compass);
+        
+        pc.printf("\n");
+        
+        
+        Led[0] = 0;//周回の終わり
+        
+        wait_ms(1);
     }
-}
+}
\ No newline at end of file