Revision 15:3d27e435e014, committed 2015-03-10

Comitter:

ryuna

Date:

Tue Mar 10 13:22:22 2015 +0000

Parent:

14:f6f5dad3e75f

Child:

16:83721dac1049

Commit message:

???????;

Changed in this revision

--- a/MultiSerial.lib	Thu Mar 05 14:52:37 2015 +0000
+++ b/MultiSerial.lib	Tue Mar 10 13:22:22 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/B/code/MultiSerial/#7620ef9e7187
+http://developer.mbed.org/teams/B/code/MultiSerial/#f2f3c3d6a6be

--- a/main.cpp	Thu Mar 05 14:52:37 2015 +0000
+++ b/main.cpp	Tue Mar 10 13:22:22 2015 +0000
@@ -7,53 +7,32 @@
  
 #include "mbed.h"
  
+#include "MultiSerial.h"
 #include "IRM2121.h"
 #include "Ping.h"
-#include "MultiSerial.h"
 #include "HMC6352.h"
 #include "Watchdog.h"
  
 #define PING_COUNT 5 //5回に1回にPing処理
-#define IR_LEVEL 400 //irのしきい値
+
 #define ADDRESS 0xAA//
-#define DATA_NUM 7
+#define DATA_NUM 9
  
  
 IRM2121 Ir[12] = {p5,p6,p7,p8,  p11,p12,    p15,p16,p17,p18,p19,p20};
 
-Ping Ping1(p21, p22);
-Ping Ping2(p23, p24);
+Ping Ping1(p22, p21);
+Ping Ping2(p24, p23);
 Ping Ping3(p25, p26);
-Ping Ping4(p29, p30);
+Ping Ping4(p30, p29);
 
 DigitalOut Led[4] = {LED1, LED2, LED3, LED4};
  
+HMC6352 hmc6352(p28,p27);
 MultiSerial Mbed(p13, p14);
 Serial pc(USBTX,USBRX);
  
-/*irm2121平均化*/
-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] += 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[num]/10;   
-}
- 
+
 void IrSort(unsigned int input[], uint8_t output[]){
     
     /*
@@ -90,6 +69,7 @@
     }
     if(num[1] == 12){
         num[1] = 0;
+        
     }
     output[0] = num[0]*12 + num[1];//rank1*12 + rank2
     output[1] = near[0] /10;//rank1
@@ -104,9 +84,11 @@
     /*begin SetUp*/
     Watchdog g;
     g.Configure(1.0f);//1秒でWDT発生
+    hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    
     /*end   Setup*/
     
-    
+    int CompassDef = 0,Compass;
     /*Count*/
     uint8_t PingCk = 0;
     
@@ -114,12 +96,14 @@
     unsigned int PingData[4] = {0};
     unsigned int IrBase[12] = {0};
     uint8_t IrData[3] = {0};
+    uint8_t CompassData[2] = {0};
     
     /*Serial*/
     uint8_t tx_data[DATA_NUM]={0};
     Mbed.write_data(tx_data, ADDRESS);
     Mbed.start_write();
  
+    CompassDef = (hmc6352.sample() / 10);
     while(1) {
         
         g.Service();
@@ -154,6 +138,14 @@
         
         IrSort(IrBase,IrData);
         
+        Compass= ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
+        if(Compass >255 ){
+            CompassData[0] = 255;
+            CompassData[1] = Compass - 255 ;
+        }else{
+            CompassData[0] = Compass;
+            CompassData[1] = 0;
+        }
         if(PingCk == 2){
             
             Ping1.Send();    
@@ -184,6 +176,7 @@
             
             PingCk = 0;
         }
+        
         //ex.
         tx_data[0] = IrData[0];
         tx_data[1] = IrData[1];
@@ -192,12 +185,16 @@
         tx_data[4] = PingData[1];
         tx_data[5] = PingData[2];
         tx_data[6] = PingData[3];
-        
+        tx_data[7] = CompassData[0];
+        tx_data[8] = CompassData[1];        
         
         
         
         Led[0] = 0;//周回の終わり
         
         wait_ms(1);
+        //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",tx_data[3],tx_data[4],tx_data[5],tx_data[6],tx_data[7],tx_data[8]);
+        
+        
     }
 }
\ No newline at end of file