Revision 2:09fabba6c00d, committed 2013-04-05

Comitter:

yusuke_robocup

Date:

Fri Apr 05 07:26:42 2013 +0000

Parent:

1:89408fff7cc9

Child:

3:b4fb2b5365a7

Commit message:

aa

Changed in this revision

--- a/IR.cpp	Sun Mar 10 07:31:31 2013 +0000
+++ b/IR.cpp	Fri Apr 05 07:26:42 2013 +0000
@@ -20,6 +20,11 @@
     int memory_ir = 0;         /*赤外線時間カウンタ*/
     int flag_ir = 0;
     int value = 0;
+    
+    static int direc = 99; 
+    static int past_direc = 99;
+    
+    int liftball;
 
     for(int i=0; i<ALL_IR; i++) {
         flag_ir = 1;
@@ -91,32 +96,42 @@
                     youso_min = i;
                 }
             }
-    
-            double hiritu = 0;
-    
-            int direc = 0;
-    
-            if(youso_min == 0) {
-                hiritu = (double)ir_value[7]/(double)ir_value[1];
-            } else if(youso_min ==7) {
-                hiritu = (double)ir_value[6]/(double)ir_value[0];
-            } else {
-                hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1];
+            
+            if(youso_min == 8){
+                direc = 1;        
+            }else if(youso_min == 9){
+                direc = 15;            
+            }else{
+                direc = youso_min * 2;
+            }
+            /*
+            if(ir_value[youso_min] > 50){
+                liftball = 1;
+            }else{
+                liftball = 0;
             }
             
-            if((hiritu <= 0.85)&&(youso_min != 0)) {
-                direc = youso_min*2-1;
-            }else if((hiritu <= 0.85)&&(youso_min == 0)){
-                direc = 15; 
-            }else if(hiritu >= 1.15) {
-                direc = youso_min*2+1;
-            } else {
-                direc = youso_min*2;
-            }
+            if((past_direc != 99)&&(liftball == 0)){
+                if(past_direc == 0){
+                    if((direc >= 4)&&(direc <= 12)){
+                        direc = past_direc;
+                    }
+                }else if(past_direc == 1){
+                    if((direc >= 4)&&(direc <= 14)){
+                        direc = past_direc;
+                    }
+                }else if(past_direc == 15){
+                    if((direc >= 2)&&(direc <= 12)){
+                        direc = past_direc;
+                    }
+                }else{
+                    if(abs(past_direc - direc) >= 4){
+                        direc = past_direc;
+                    }
+                }
+            }*/
             
-            /*if(youso_min == 0){
-                direc = 0;   
-            }*/
+            //direc = youso_min * 2;
     
             /*******  direction end  *******/
     
@@ -147,42 +162,45 @@
             
             double hihhihi = 0;
             
-            hihhihi = (double)ir_value[youso_min]/(double)ir_value[8];
+            if(!ir_value[10]) ir_value[10] = 1000000;
             
+            hihhihi = (double)ir_value[youso_min]/(double)ir_value[10];
             
-            if((direc == 0)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            }else if((direc == 1)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            }else if((direc == 2)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 3)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 4)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            }else if((direc == 5)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 6)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 7)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            }else if((direc == 8)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            }else if((direc == 9)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            }else if((direc == 10)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 11)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 12)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            }else if((direc == 13)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 14)&&(hihhihi  >= 0.65)){
-                dista = 10;
-            }else if((direc == 15)&&(hihhihi  >= 0.80)){
-                dista = 10;
-            } 
+            if(ir_value[10] <= 45){ 
+                if((direc == 0)&&(hihhihi  >= 1.0)){
+                    dista = 10;
+                }else if((direc == 1)&&(hihhihi  >= 1.0)){
+                    dista = 10;
+                }else if((direc == 2)&&(hihhihi  >= 0.75)){
+                    dista = 10;
+                }else if((direc == 3)&&(hihhihi  >= 0.65)){
+                    dista = 10;
+                }else if((direc == 4)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 5)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 6)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 7)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 8)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 9)&&(hihhihi  >= 0.70)){
+                    dista = 10;
+                }else if((direc == 10)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 11)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 12)&&(hihhihi  >= 0.60)){
+                    dista = 10;
+                }else if((direc == 13)&&(hihhihi  >= 0.65)){
+                    dista = 10;
+                }else if((direc == 14)&&(hihhihi  >= 0.75)){
+                    dista = 10;
+                }else if((direc == 15)&&(hihhihi  >= 1.0)){
+                    dista = 10;
+                }
+            }  
             
             int count = 0;
             
@@ -196,6 +214,8 @@
 
             /********  distance end  *******/
             
+            past_direc = direc;
+            
             direction = direc;
             Distance  = dista;
             

--- a/IR.h	Sun Mar 10 07:31:31 2013 +0000
+++ b/IR.h	Fri Apr 05 07:26:42 2013 +0000
@@ -2,8 +2,8 @@
 
 #define IR_TIME_NOTFOUND 833    /* 見つけられなかったと判断するまでの時間(単位：us) */
 #define IR_COUNTMAX 487     /*最大パルス幅 パルスの存在しうる最大時間は487us*/
-#define ALL_IR 9
-#define DIREC_IR 8
+#define ALL_IR 11
+#define DIREC_IR 10
 #define DIRECTION 16
 #define TERM 0
 #define DELTA -2
@@ -20,13 +20,15 @@
     p18,
     p19,
     p20,
-    p30
+    p25,
+    p26,
+    p12
 };
 
 int Convert_Direction[DIRECTION] = {
     90,
     67,
-    45,
+    30,
     22,
     0,
     337,
@@ -38,7 +40,7 @@
     202,
     180,
     157,
-    135,
+    150,
     112
 };
 
@@ -46,7 +48,7 @@
 double ball_sankaku[16][2] = {
     {0     , 1    },
     {0.390 , 0.920},
-    {0.707 , 0.707},
+    {0.866 , 0.500},
     {0.927 , 0.374},
     {1     , 0    },
     {0.920 ,-0.390},
@@ -58,6 +60,48 @@
     {-0.927,-0.374},
     {-1    , 0    },
     {-0.920, 0.390},
-    {-0.707, 0.707},
+    {-0.866, 0.500},
     {-0.374, 0.927}
 };
+
+double turn_sankaku[16][2] = {
+    { 0    ,0      },
+    { 1    ,0      },//{ 0.920,-0.390 },
+    { 0.707,-0.707 }, //{ 0.500,-0.866 },
+    { 0.374,-0.927 },
+    { 0    ,-0.5   },
+    {-0.390,-0.920 },
+    {-0.707,-0.707 },
+    {-0.927,-0.374 },
+    {-0.927,-0.374 },
+    {0.920 ,-0.390 },
+    {0.707 ,-0.707 },
+    {0.374 ,-0.927 },
+    {0     ,-0.5   },
+    {-0.390,-0.920 },
+    {-0.707,-0.707 }, //{-0.500,-0.866 },
+    {-1    ,0      }//{-0.927,-0.374 }
+};
+
+
+/*
+double turn_sankaku[16][2] = {
+    {0 ,0 },
+    {0 ,0 },
+    {1 ,0 },
+    {0 ,-1},
+    {0 ,-1},
+    {0 ,-1},
+    {-1,0 },
+    {-1,0 },
+    {-1,0 },
+    {1 ,0 },
+    {1 ,0 },
+    {0 ,-1},
+    {0 ,-1},
+    {0 ,-1},
+    {-1,0 },
+    {0 ,0 }
+};
+*/
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/eeprom.lib	Fri Apr 05 07:26:42 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/okini3939/code/eeprom/#724b44e46ecf

--- a/main.cpp	Sun Mar 10 07:31:31 2013 +0000
+++ b/main.cpp	Fri Apr 05 07:26:42 2013 +0000
@@ -4,19 +4,24 @@
 #include "HMC6352.h"
 #include "PID.h"
 #include "main.h"
+#include "eeprom.h"
 
 
+BusOut mbedleds(LED4,LED3,LED2,LED1);
 
 void PidUpdata()
-{    
-    inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);        
+{   
+    if(turn_flag){
+        now_compass = (((int)(compass.sample() - (past_compass) + 5400.0) % 3600) / 10.0); 
+    }else{ 
+        inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);        
     
     //pc.printf("%f\n",timer1.read());
-    pid.setProcessValue(inputPID);
+        pid.setProcessValue(inputPID);
     //timer1.reset();
     
-    compassPID = -(pid.compute());
-    
+        compassPID = -(pid.compute());
+    }
     //pc.printf("%f\n",compassPID);
 
 }
@@ -67,11 +72,14 @@
 
 void init()
 {
+    uint8_t initFlag = 0;
+    char *hozon;
     
     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     StartButton.mode(PullUp);
     CalibEnterButton.mode(PullUp);
     CalibExitButton.mode(PullUp);
+    EEPROMButton.mode(PullUp);
     driver.baud(BAUD_RATE);
     wait_ms(MOTDRIVER_WAIT);
     driver.printf("1F0002F0003F0004F000\r\n"); 
@@ -88,10 +96,15 @@
             led2 = OFF;
             led3 = ON;
          }
+         if(!EEPROMButton){
+            initFlag = 1;
+            read_eeprom(hozon,(char *)&standard,sizeof(hozon));
+         }
     }
-    
-    standard = compass.sample() / 10.0;
-    
+    if(!initFlag){
+        standard = compass.sample() / 10.0;
+        write_eeprom((char *)&standard,hozon,sizeof((char *)&standard));
+    }
     led1 = OFF;
     led3 = OFF;
     
@@ -102,8 +115,8 @@
     pid.setSetPoint(180.0);
     
     pidUpdata.attach(&PidUpdata, 0.06);
-    wait(1);
-    IR.attach(&IR_Position,0.04);
+    wait(0.3);
+    IR.attach(&IR_Position,0.06);
     ultrasonic.attach(&Ultrasonic, 0.05);
     driver.attach(&Tx_interrupt, Serial::TxIrq);
     //driver.attach(&Rx_interrupt, Serial::RxIrq);
@@ -112,116 +125,222 @@
     timer2.start();
 }
 
+uint16_t moving_ave_5point(uint16_t data)
+{
+    static uint16_t tmp[5] = {0};
+    static uint32_t sum = 0;
+    uint8_t i;
+    uint8_t count;
+    
+    sum -= tmp[4];
+    sum += data;
+    tmp[4] = tmp[3];
+    tmp[3] = tmp[2];
+    tmp[2] = tmp[1];
+    tmp[1] = tmp[0];
+    tmp[0] = data;
+    
+    return sum/5;
+}
+
+
 int main()
 {
     int vx=0,vy=0,vs=0;
-    int state = HOME_WAIT;
+    int x_dista = 0,y_dista = 0,x_turn = 0,y_turn = 0;
+    int state = NONE;
+    int direction_av = 0;
+    int direction_past = 0;
     
     init();
            
     while(1) {
-    
+        x_dista = 0;
+        y_dista = 0;
+        x_turn  = 0;
+        y_turn  = 0;
+        turn_flag = 0;
+        
         vs = compassPID;
-        //vx = 15;
-        //vy = 10;
+        //vs = 0;
+        //past_compass = compass.sample() / 1.0;
+        //float now_compass = 180.0;
+        /*
+        while(1){
+            vx = -10;
+            vy = 10;
+            vs = compassPID;
+            
+            move(vx,vy,vs);
+        }*/
         
-        /*
+        direction_av = moving_ave_5point(direction);
+           
+        if(direction_av == 0){
+            state = ATTACK;
+        }
+        if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 30)){
+            state = SNAKE;
+        }
+        if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance >= 90)){
+            state = SEARCH;
+        }
+        if((state != SNAKE)&&(Distance >= 90)){
+            state = SEARCH;
+        } 
+        
         if(IR_found){
-            if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
-                vx = (int)(70*ball_sankaku[direction][0]);
-                vy = (int)(70*ball_sankaku[direction][1]);
-            }else{
-                vx = (int)(70*ball_sankaku[direction][0]);
-                vy = (int)(70*ball_sankaku[direction][1]);
-            }
+            if(state == SNAKE){
+                if(Distance == 30){
+                    x_dista = 20*ball_sankaku[direction][0];
+                    y_dista = 20*ball_sankaku[direction][1];            
+                    
+                    x_turn = 10*(turn_sankaku[direction][0]);
+                    y_turn = 10*(turn_sankaku[direction][1]);
+                    
+                    if((direction == 2)||(direction == 14)){
+                        x_turn *= 0.7;
+                        y_turn *= 0.7;
+                    }
+                }
+                
+                if(Distance == 10){
+                    x_dista = 8*(-ball_sankaku[direction][0]);
+                    y_dista = 8*(-ball_sankaku[direction][1]);            
+                
+                    x_turn = 22*(turn_sankaku[direction][0]);
+                    y_turn = 22*(turn_sankaku[direction][1]);
+                    
+                    
+                    if(direction == 2){
+                        vs = -3;  
+                    }
+                    if(direction == 14){
+                        vs = 3;  
+                    }
+                    
+                    if((direction == 2)||(direction == 14)){
+                        x_turn *= 0.7;
+                        y_turn *= 0.7;
+                    }
+                }
+                
+                if((direction == 2)||(direction == 14)||(direction == 1)||(direction == 15)||(direction == 0)){
+                    x_dista = 0;
+                    y_dista = 0;    
+                }
+                    
+                vx = x_turn + x_dista;
+                vy = y_turn + y_dista;
+                
+            }else if(state == ATTACK){
+                if(direction == 0){
+                    vx = 10*ball_sankaku[direction][0];
+                    vy = 15*ball_sankaku[direction][1];  
+                }
+                if(direction == 1){
+                    vx = 30*ball_sankaku[direction][0];
+                    vy = 8*ball_sankaku[direction][1];  
+                }
+                if(direction == 15){
+                    vx = 30*ball_sankaku[direction][0];
+                    vy = 8*ball_sankaku[direction][1];  
+                }
+                if(direction == 2){
+                    vx = 10*ball_sankaku[direction][0];
+                    vy = 10*ball_sankaku[direction][1];
+                    vs = -3;  
+                }
+                if(direction == 14){
+                    vx = 10*ball_sankaku[direction][0];
+                    vy = 10*ball_sankaku[direction][1];
+                    vs = 3;  
+                }
+                
+            }else if(state == SEARCH){
+                //vx = 15*ball_sankaku[direction][0];
+                //vy = 15*ball_sankaku[direction][1];
+                
+                if(direction == 2){
+                    vx = 25*ball_sankaku[direction][0];
+                    vy = 10*ball_sankaku[direction][1];
+                    vs = -2;  
+                }
+                if(direction == 14){
+                    vx = -25*ball_sankaku[direction][0];
+                    vy = 10*ball_sankaku[direction][1];
+                    vs = 2;  
+                }          
+                if(direction == 0){
+                    vx = 10*ball_sankaku[direction][0];
+                    vy = 15*ball_sankaku[direction][1];  
+                }
+                if(direction == 1){
+                    vx = 30*ball_sankaku[direction][0];
+                    vy = 8*ball_sankaku[direction][1];  
+                }
+                if(direction == 15){
+                    vx = 30*ball_sankaku[direction][0];
+                    vy = 8*ball_sankaku[direction][1];  
+                }               
+            } 
         }else{
             vx = 0;
             vy = 0;
         }
-        */
         
         /*
-        if(IR_found)state = DIFFENCE;
-        else state = HOME_WAIT;
-        */
-        /*
-        if(state == HOME_WAIT){
-            if((ultrasonicVal[0] + ultrasonicVal[2]) < 6000){
-                if(ultrasonicVal[0] > 3200){
-                    vx = 15;
-                    vy = 0;
-                }else if(ultrasonicVal[2] > 3200){
-                    vx = -15;
-                    vy = 0;
-                }else{
-                    if(ultrasonicVal[1] > 800){
-                        vx = 0;
-                        vy = -15;
-                    }else{
-                        vx = 0;
-                        vy = 0;
-                    }   
+        if((ultrasonicVal[0]<50)||(ultrasonicVal[1]<50)||(ultrasonicVal[3]<50)){
+            vx = (int)((30-FULL)*ball_sankaku[direction][0]);
+            vy = (int)((30-FULL)*ball_sankaku[direction][1]);
+        }
+        
+        
+        
+        if(Distance == 10){
+            mbedleds = 0xF;    
+        }else{
+            mbedleds = 0x0;
+        }
+        
+        if((ultrasonicVal[0]<100)&&(ultrasonicVal[1]<100)&&(ultrasonicVal[2]<100)&&((direction == 0)||(direction == 1)||(direction == 15))){
+            vx = 0;
+            vy = 0;
+            vs = 0;
+            past_compass = compass.sample() / 1.0;
+            float now_compass = 180.0;
+            
+            if(inputPID > 180){
+                turn_flag = 1;
+                while((now_compass > 180.0 - (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
+                    vs = 10;
+                    move(vx,vy,vs);
                 }
-            }else{
-                vx = 0;
-                vy = 0;
+                turn_flag = 0;
             }
-        }else if(state == DIFFENCE){
-            if(ultrasonicVal[1] > 800){
-                vx = 0;
-                vy = -15;
-            }else{
-                if(distance <= 30){
-                
-                    if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
-                        vx = 15;
-                
-                    }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
-                        vx = -15;
             
-                    }else{
-                        vx = 0;
-                    }
-                }else{
-                    
+            if(inputPID < 180){
+                turn_flag = 1;
+                while((now_compass < 180.0 + (SHINPUKU / 2.0))&&((direction == 0)||(direction == 1)||(direction == 15))){
+                    vs = -10;
+                    move(vx,vy,vs);
                 }
-            }     
-        }
-        */
-        /*
-        if(state == HOME_WAIT){
-           
+                turn_flag = 0; 
+            }          
         }*/
         
-            if((ultrasonicVal[0] + ultrasonicVal[2]) < 1050.0){
-                if(ultrasonicVal[0] > 300.0){
-                    vx = 15;
-                    led3 = ON;
-                    led4 = OFF;
-                }else if(ultrasonicVal[2] > 300.0){
-                    vx = -15;
-                    led3 = ON;
-                    led4 = OFF;
-                }else{
-                    led3 = OFF;
-                    led4 = ON;
-                    if(ultrasonicVal[1] > 200.0){
-                        vy = -15;
-                    }else if(ultrasonicVal[1] < 100.0){
-                        vy = 15;
-                    }else{
-                        vy = 0;
-                    }
-                }
-                led2 = ON;
-            }else{
-                vx = 0;
-                vy = 0;
-                led2 = OFF;
-            }
-            
-        //pc.printf("%f,%f,%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2]);
-            
+        if(state == SNAKE){
+            mbedleds = 0xF;    
+        }else{
+            mbedleds = 0x0;
+        }
+                
+        
+        vx *= 1;
+        vy *= 1;
+        
+        direction_past = direction;
+        
         move(vx,vy,vs);
     }
 }

--- a/main.h	Sun Mar 10 07:31:31 2013 +0000
+++ b/main.h	Fri Apr 05 07:26:42 2013 +0000
@@ -10,15 +10,22 @@
 #define ON      1
 #define OFF     0
 
-#define MOT1    1.0
-#define MOT2    1.0
-#define MOT3    1.0
-#define MOT4    1.0
+#define MOT1    1.0             //60  3.0 0.0 0.5 1.0
+#define MOT2    1.0             //120 0.0 3.0 1.0 0.0
+#define MOT3    1.0            //240 0.6 0.0 0.0 3.0
+#define MOT4    1.0             //300 0.0 0.8 3.0 0.0
 
 #define OUT_LIMIT   30.0
 #define MAX_POW     100
 #define MIN_POW     -100
 
+#define SHINPUKU 40
+
+#define FULL 0
+
+
+
+
 DigitalOut led1(LED1); 
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
@@ -29,7 +36,8 @@
 DigitalIn StartButton(p21);
 DigitalIn CalibEnterButton(p22);
 DigitalIn CalibExitButton(p23);
-PID pid(0.59, 1.0, 0.015, RATE);      //30.0 0.35 1.0 0.012 30.0 0.42 1.0 0.013 
+DigitalIn EEPROMButton(p24);
+PID pid(0.35, 0.0, 0.012, RATE);      //30.0 0.35 1.0 0.012 30.0    0.42 1.0 0.013 
 Ticker pidUpdata;
 Ticker IR;
 Ticker ultrasonic;
@@ -51,7 +59,11 @@
 extern int Distance;
 extern int IR_found;
 extern double ball_sankaku[16][2];
+extern double turn_sankaku[16][2];
 extern double ultrasonicVal[3];
+float now_compass =20.0;
+float past_compass;
+int turn_flag = 0;
 
 extern void Ultrasonic(void);
 extern void IR_Position(void);
@@ -59,7 +71,9 @@
 extern void array(int,int,int,int);
 
 enum{
-    HOME_WAIT,
-    DIFFENCE,
+    NONE,
+    SNAKE,
+    ATTACK,
+    SEARCH
 };
 

--- a/ultrasonic.cpp	Sun Mar 10 07:31:31 2013 +0000
+++ b/ultrasonic.cpp	Fri Apr 05 07:26:42 2013 +0000
@@ -4,13 +4,15 @@
 
 
 extern Timer timer2;
-extern Serial pc; // tx, rx 
+extern Serial pc; // tx, rx
+extern float now_compass; 
 
 double ultrasonicVal[ALL_ULTRASONIC] = {0};
 
 
 void Ultrasonic()
 {
+    
     for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
         uint8_t flag = 0;
     
@@ -29,7 +31,7 @@
         } 
         timer2.reset();
         while(PingPin == 1){
-            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
+            if((timer2.read_us() > 5000) || (flag == 1)){  //18.5ms以上のパルス
                 ultrasonicVal[i] =  PING_ERR;
                 flag = 1;
                 break;
@@ -39,7 +41,8 @@
             ultrasonicVal[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //cm
         }
     }
-    //pc.printf("%f\n",ultrasonicVal[0] + ultrasonicVal[2]);
+    //pc.printf("%f\n",ultrasonicVal[1]);
     //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
-    
+    //pc.printf("%f\n",);
+
 }

--- a/ultrasonic.h	Sun Mar 10 07:31:31 2013 +0000
+++ b/ultrasonic.h	Fri Apr 05 07:26:42 2013 +0000
@@ -1,6 +1,6 @@
 #include "HMC6352.h"
 
-#define ALL_ULTRASONIC  3
+#define ALL_ULTRASONIC  4
 #define PING_ERR        0xFFFF
 
 extern HMC6352 compass;
@@ -10,5 +10,5 @@
     p25,
     p26,
     p29,
-    //p30,
+    p30,
 };