Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
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