Diff: main.cpp

Revision:

1:52d3136e1a22

Parent:

0:a1accf06b614

Child:

2:22b200c374cd

--- a/main.cpp	Fri Nov 30 12:34:22 2012 +0000
+++ b/main.cpp	Wed Dec 05 13:56:13 2012 +0000
@@ -12,7 +12,6 @@
 DigitalOut led4(LED4);
 Serial pc(USBTX, USBRX);                    // tx, rx
 SDFileSystem sd(p5, p6, p7, p8, "sd");      // the pinout on the mbed Cool Components workshop board
-InterruptIn urg_flag(p12);
 Serial navi(p13, p14);                       // tx, rx
 DigitalIn stop(p17);                        // button (green)
 Serial urg(p28, p27);                       // tx, rx
@@ -42,9 +41,6 @@
     int pow2[18]                =   {0};
     int urg_depth[682]          =   {0};
     
-    int depth_sum_right         =   0;
-    int depth_sum_left          =   0;
-    
     // for IAF +-45deg 1600mm
     double range                =   1600;
     double sin_iaf_angle        =   sin(0.352*pi/180);              // mabiki nashi
@@ -118,9 +114,7 @@
         if( c_urg_data[0]==aLF && c_urg_data[1]==aLF ){   flag = 1;   }
         for( int i=0;i<2;i++ ){     c_urg_data[i+1] = c_urg_data[i];    }
     }
-    
-    urg_flag.rise(&send_to_navi);  // attach the address of the flip function to the rising edge
-    
+    /*
     //while(1){
     while( stop==0 ){
         
@@ -137,13 +131,7 @@
             for( int i=0;i<31;i++ ){    for( int j=66*i;j<66*i+64;j++ ){    i_urg_data_buf[j-2*i] = (int)c_urg_data_buf[j]-48;    }   }
             
             for( int j=2046;j<2108;j++ ){    i_urg_data_buf[j-62] = (int)c_urg_data_buf[j]-48;    }
-            /*
-            for( int i=0;i<2110;i++ ){   pc.printf("%c", c_urg_data_buf[i]);        }
-            pc.printf("\n\r\n\r");
-            
-            for( int i=0;i<2046;i++ ){   pc.printf("%2d ", i_urg_data_buf[i]);        }
-            pc.printf("\n\r\n\r");
-            */
+
             for( int i=0;i<682;i++ ){
                 
                 for( int j=0;j<3;j++ ){
@@ -159,38 +147,16 @@
                     }
                     
                 }
-                /* binary check
-                for( int j=0;j<3;j++ ){
-                    for( int k=0;k<6;k++ ){
-                        pc.printf("%d", b_urg_data[2-j][5-k]);
-                    }
-                    pc.printf(" ");
-                }
-                */
+
                 for( int j=0;j<3;j++ ){     for( int k=0;k<6;k++ ){     b_urg_data_sum[j] += pow2[6*j+k]*b_urg_data[j][k];  }   }
                 for( int j=0;j<3;j++ ){     urg_depth[i] += b_urg_data_sum[j];  }
                 for( int j=0;j<3;j++ ){     b_urg_data_sum[j] = 0;              }
                 if( urg_depth[i]>range ){   urg_depth[i] = range;                }
-                //pc.printf("%d ", urg_depth[i]);
-            }
-            //pc.printf("\r\n\r\n");
-            
-            /*
-            // Full range
-            for( int i=0;i<341;i++ ){
-                depth_sum_right += urg_depth[i];
-                depth_sum_left += urg_depth[i+341];
+                
             }
-            //pc.printf("\r\n%d %d\r\n\r\n", depth_sum_right, depth_sum_left);
-            // Initialization
-            for( int i=0;i<682;i++ ){   urg_depth[i] = 0;   }
-            depth_sum_right =   0;
-            depth_sum_left  =   0;
-            */
-            
+                        
             // IAF (+-45deg) (/1000000 is length conversion mm to m)
-            //for( int i=212;i<469;i++ ){     pc.printf("%d ", urg_depth[i]);     }
-            //pc.printf("\r\n");
+
             for( int i=212;i<340;i++ ){     iaf_SR += (double)urg_depth[i]*(double)urg_depth[i+1]/1000000*sin_iaf_angle/2;      }
             for( int i=340;i<468;i++ ){     iaf_SL += (double)urg_depth[i]*(double)urg_depth[i+1]/1000000*sin_iaf_angle/2;       }
             // iaf_delta: (10*delta + 500)[deg/s]
@@ -199,17 +165,8 @@
             else{   iaf_delta = 500;    }
             if( iaf_delta>800 ){    iaf_delta = 800;    }
             if( iaf_delta<200 ){    iaf_delta = 200;    }
-                                    
-            //pc.printf("%6.3f, %6.3f\r\n", iaf_SR, iaf_SL);
-            pc.printf("%4d\r\n", iaf_delta);
-            /*
-            if( urg_flag==1 ){
-                navi.printf("%4d",iaf_delta);
-                //dotHR.printf("a");
-                pc.printf("%4d, %4d\r\n", t, iaf_delta);
-                t = t+dt;
-            }
-            */
+            
+
             // Initialization
             for( int i=0;i<682;i++ ){   urg_depth[i] = 0;   }
             iaf_SR_print    =   iaf_SR;
@@ -220,13 +177,5 @@
         }
         
     }
-    
+    */
 }
-
-void send_to_navi() {
-
-    navi.printf("%4d",iaf_delta);
-    pc.printf("%4d, %f, %f, %4d\r\n", t, iaf_SL_print, iaf_SR_print, iaf_delta);
-    t = t+dt;
-    
-}