Diff: main.cpp

Revision:

1:8e9ee100d854

Parent:

0:182437c14483

Child:

2:099a6c81a02e

--- a/main.cpp	Sat Dec 06 07:42:33 2014 +0000
+++ b/main.cpp	Fri Jan 09 06:56:25 2015 +0000
@@ -1,26 +1,33 @@
 /*
-メインと通信するLPC1114のプログラム()
-
-現在は両方がmainと通信します．
-
-
-*/
+ *メインと通信するLPC1114のプログラム()
+ *
+ *現在は両方がmainと通信します．
+ * 
+ *mainの通信回数が多いためそれに耐えうる速度で通信しなければならないのではという懸念。 
+ *試してみないとわからないが、RTOSにより常に通信のみを監視するのもありかもしれない。
+ *それでダメなら詰む。
+ */
 
 #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"
 
 #define PING_COUNT 5 //5回に1回にPing処理
-#define LINE_LINE 0.45 //すごく雑 
+#define LINE_LEVEL (1-0.55) //しきい値を設定　0.55がしきい値
+#define IR_LEVEL 400 //irのしきい値
 
-IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,dp28};
+#define I2C_ADDRESS 0xAA // another (0xAC)
+
+
+
+IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,NC};
 Ping Ping1(dp1,dp2);
 Ping Ping2(dp4,dp6); 
 AnalogIn Line[4] ={dp9,dp10,dp11,dp13};
@@ -29,25 +36,76 @@
 I2CSlave Mbed(dp5,dp27);//sda,scl
 Serial pc(USBTX,USBRX);
 
-
-uint8_t IrMovig_ave(uint8_t num, int data){
-    static unsigned int sum[6];
-    static unsigned temp[6][5];
+/*irm2121平均化*/
+uint8_t IrMoving_ave(uint8_t num, int data){
+    static unsigned int sum[10];
+    static unsigned temp[6][10];
     
-    sum[num] -= temp[num][4]; 
+    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];
+    temp[num][6] = temp[num][5];
+    temp[num][5] = temp[num][4];
     temp[num][4] = temp[num][3];
     temp[num][3] = temp[num][2];
     temp[num][2] = temp[num][1];
     temp[num][1] = temp[num][0];
     temp[num][0] = data;
     
-    return  sum[data]/5;   
+    return  sum[num]/10;   
 }
 
-
-
+void IrDataSelect(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]){
+            continue;
+        }
+        if(nigh1 < input[i]){
+            nigh2 = input[i];
+            ni2 = i;
+            continue;
+        }
+        nigh2 = nigh1;
+        ni2   = ni1;
+        nigh1 = input[i];
+        ni1   = 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;
+        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;//周回の初め    
@@ -60,20 +118,30 @@
 int main() {
     
     //default Mbed.frequency();
-    Mbed.address(0x0A);
+    Mbed.address(I2C_ADDRESS);
     /*Data*/
-    uint8_t IrMemory[6] = {0};//IRのデータ保管
+    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;
+    
     /*Count*/
     uint8_t PingCk = 0;
     
     /* I2Cdata*/
     char SendData[9]={0};
+    char AddressData = 0;
+    
+    /*Debug valiable*/
+    bool valid = 0,busy = 0;
 
     while(1) {
         
@@ -83,18 +151,33 @@
         Ir[2].Set();
         Ir[3].Set();
         Ir[4].Set();
-        Ir[5].Set();
+        //Ir[5].Set();
+        
+        wait_ms(50);//wait値は最適値を探す必要がある．
         
-        wait_ms(30);//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();
         
-        IrMemory[0] = IrMovig_ave(0,Ir[0].Read());
-        IrMemory[1] = IrMovig_ave(1,Ir[1].Read());
-        IrMemory[2] = IrMovig_ave(2,Ir[2].Read());
-        IrMemory[3] = IrMovig_ave(3,Ir[3].Read());
-        IrMemory[4] = IrMovig_ave(4,Ir[4].Read());
-        IrMemory[5] = IrMovig_ave(5,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]);
+        */
         
-        /*大きい二つだけでいい気がする．*/
+        /*小さい(大きい)二つだけでいい気がする．値をみて小さかったらそうしょう*/
+        /**/
+        //IrDataSelect(IrMemory,IrSendData);
+        /**/
+        /*
         if(PingCk >= PING_COUNT){
             Ping1.Send();
             Ping2.Send();    
@@ -103,38 +186,53 @@
             PingMemory[1] = Ping2.Read_cm();
             PingCk = 0;
         }
-        
+        */
         LineMemory[0] = Line[0];
         LineMemory[1] = Line[1];
         LineMemory[2] = Line[2];
         LineMemory[3] = Line[3];
         
-        LineFlag = ((int)(LineMemory[3]+LINE_LINE))*8 +
-                   ((int)(LineMemory[2]+LINE_LINE))*4 +
-                   ((int)(LineMemory[1]+LINE_LINE))*2 +
-                   ((int)(LineMemory[0]+LINE_LINE))*1;
-           
-        SendData[0] = (char)IrMemory[0];
-        SendData[1] = (char)IrMemory[1];
-        SendData[2] = (char)IrMemory[2];
-        SendData[3] = (char)IrMemory[3];
-        SendData[4] = (char)IrMemory[4];
-        SendData[5] = (char)IrMemory[5];
-        SendData[6] = (char)PingMemory[0];
-        SendData[7] = (char)PingMemory[1];
-        SendData[8] = (char)LineFlag;
+        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(Mbed.receive()){    
+        switch(I2CState){    
             case I2CSlave::ReadAddressed:
-                 Mbed.write(SendData, 9); // Includes null char
+                 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:
+            case I2CSlave::WriteGeneral:
+                 AddressData = Mbed.read();
                  break;
-             case I2CSlave::WriteAddressed:
+            case I2CSlave::WriteAddressed:
+                 AddressData = Mbed.read();
                  break;
         
         }
-    
+        */
+        pc.printf("%d  %d %d %d %d %d\n\r",IrTemp[0],IrTemp[1],IrTemp[2],IrTemp[3],IrTemp[4],IrTemp[5]);
+        
         Led = 0;
     
     }