Diff: piswarm.cpp

Revision:

9:7a4fc1d7e484

Parent:

4:52b3e4c5a425

Child:

11:5ebcb52726cf

--- a/piswarm.cpp	Mon Feb 03 12:59:51 2014 +0000
+++ b/piswarm.cpp	Tue Feb 18 17:14:03 2014 +0000
@@ -4,7 +4,11 @@
  *
  * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
  * 
- * Version 0.5  February 2014
+ * Version 0.6  February 2014
+ *
+ * Change notes:
+ * 0.6  :  Added new IR sensor functions to improve efficiency (see manual for details)
+ * 0.5  :  Initial release
  *
  * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
  *
@@ -45,6 +49,10 @@
 float left_speed = 0;
 float right_speed = 0;
 signed short mag_values [3];
+unsigned short background_ir_values [8];
+unsigned short illuminated_ir_values [8];
+float reflected_ir_distances [8];
+char ir_values_stored = 0;
 
 
 PiSwarm::PiSwarm() :  Stream("PiSwarm"), _ser(p9, p10), _oled_r(p26), _oled_g(p25), _oled_b(p24), _cled_r(p23), _cled_g(p22), _cled_b(p21), _gyro(p16), _accel_x(p19), _accel_y(p18), _accel_z(p17), _temperature(p20), _light(p15), _i2c(p28,p27), _irpulse_1(p12), _irpulse_2(p13), _rf(p5,p6,p7,p8,p11)  {
@@ -207,16 +215,22 @@
 // IR Sensor Functions
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).  The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
+// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).  
+// The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
+// NB This function is preserved for code-compatability and cases where only a single reading is needed
+// In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance()
 float PiSwarm::read_reflected_ir_distance ( char index ){
-    //This function attempts to read a real-world distance approximation using the IR proximity sensors
+    // Sanity check
+    if(index>7) return 0.0;
+    
     //1.  Check that the IR LDO regulator is on, enable if it isn't
     if(enable_ir_ldo == 0){
         enable_ir_ldo = 1;
         enable_ldo_outputs();
     }
-    //2.  Read the ADC value without IR emitter on
-    unsigned short background_value = read_adc_value ( index );
+    //2.  Read the ADC value without IR emitter on; store in the background_ir_values[] array
+    background_ir_values [index] = read_adc_value ( index );
+    
     //3.  Enable the relevant IR emitter by turning on its pulse output
     switch(index){
         case 0: case 1: case 6: case 7:
@@ -227,10 +241,12 @@
         break;
     }
     wait_us(500);               
-    //4.  Read the ADC value now IR emitter is on
-    unsigned short strong_value = read_adc_value ( index );
+    
+    //4.  Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array
+    illuminated_ir_values [index] = read_adc_value ( index );
+    
     //5.  Turn off IR emitter
-       switch(index){
+    switch(index){
         case 0: case 1: case 6: case 7:
         _irpulse_1 = 0;
         break;
@@ -238,18 +254,101 @@
         _irpulse_2 = 0;
         break;
     }
-    //6.  Estimate distance based on 2 values
-    float app_distance = 4000 + background_value - strong_value;
-    if(app_distance < 0) app_distance = 0;
+    
+    //6.  Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances()
+    reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]);
+    ir_values_stored = 1;
+    return reflected_ir_distances [index];
+}
+
+
+// Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances
+// Introduced in API 0.6
+float PiSwarm::get_reflected_ir_distance ( char index ){
+    // Sanity check: check range of index and that values have been stored
+    if (index>7 || ir_values_stored == 0) return 0.0;
+    return reflected_ir_distances[index];   
+}
+
+// Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
+// Introduced in API 0.6
+unsigned short PiSwarm::get_background_raw_ir_value ( char index ){
+    // Sanity check: check range of index and that values have been stored
+    if (index>7 || ir_values_stored == 0) return 0.0;
+    return background_ir_values[index];   
+}
+    
+// Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values
+// Introduced in API 0.6
+unsigned short PiSwarm::get_illuminated_raw_ir_value ( char index ){
+    // Sanity check: check range of index and that values have been stored
+    if (index>7 || ir_values_stored == 0) return 0.0;
+    return illuminated_ir_values[index];   
+}
+    
+// Stores the reflected distances for all 8 IR sensors
+// Introduced in API 0.6
+void PiSwarm::store_reflected_ir_distances ( void ){
+    store_background_raw_ir_values();
+    store_illuminated_raw_ir_values();
+    for(int i=0;i<8;i++){
+       reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
+    }  
+}
+    
+// Stores the raw ADC values for all 8 IR sensors without enabling IR emitters
+// Introduced in API 0.6
+void PiSwarm::store_background_raw_ir_values ( void ){
+    ir_values_stored = 1;
+    for(int i=0;i<8;i++){
+        background_ir_values [i] = read_adc_value(i);
+    }
+}
+    
+// Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse
+// Introduced in API 0.6
+void PiSwarm::store_illuminated_raw_ir_values ( void ){
+    //1.  Check that the IR LDO regulator is on, enable if it isn't
+    if(enable_ir_ldo == 0){
+        enable_ir_ldo = 1;
+        enable_ldo_outputs();
+    }
+    
+    //2.  Enable the front IR emitters and store the values 
+    _irpulse_1 = 1;
+    wait_us(500);               
+    illuminated_ir_values [0] = read_adc_value(0);
+    illuminated_ir_values [1] = read_adc_value(1);
+    illuminated_ir_values [6] = read_adc_value(6);
+    illuminated_ir_values [7] = read_adc_value(7);
+    _irpulse_1 = 0;
+    
+    //3.  Enable the rear+side IR emitters and store the values 
+    _irpulse_2 = 1;
+    wait_us(500);               
+    illuminated_ir_values [2] = read_adc_value(2);
+    illuminated_ir_values [3] = read_adc_value(3);
+    illuminated_ir_values [4] = read_adc_value(4);
+    illuminated_ir_values [5] = read_adc_value(5);
+    _irpulse_2 = 0;
+}
+    
+// Converts a background and illuminated value into a distance
+// Introduced in API 0.6 - used by read_reflected_ir_distance and store_reflected_ir_distances
+float PiSwarm::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ){
+    float approximate_distance = 4000 + background_value - illuminated_value;
+    if(approximate_distance < 0) approximate_distance = 0;
+    
     // Very approximate: root value, divide by 2, approx distance in mm
-    app_distance = sqrt (app_distance) / 2.0;
+    approximate_distance = sqrt (approximate_distance) / 2.0;
+    
     // Then add adjustment value if value >27
-    if(app_distance > 27) {
-        float shift = pow(app_distance - 27,3);
-        app_distance += shift;
+    if(approximate_distance > 27) {
+        float shift = pow(approximate_distance - 27,3);
+        approximate_distance += shift;
     }
-    if(app_distance > 90) app_distance = 100.0;
-    return app_distance;
+    if(approximate_distance > 90) approximate_distance = 100.0;
+    return approximate_distance;    
 }
 
 // Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse