Diff: sensors.cpp

Revision:

1:060690a934a9

Parent:

0:d6269d17c8cf

Child:

2:c6986ee3c7c5

--- a/sensors.cpp	Thu Feb 04 21:48:54 2016 +0000
+++ b/sensors.cpp	Thu Mar 03 23:21:47 2016 +0000
@@ -1,5 +1,5 @@
 /* University of York Robotics Laboratory PsiSwarm Library: Sensor Functions Source File
- * 
+ *
  * File: sensors.cpp
  *
  * (C) Dept. Electronics & Computer Science, University of York
@@ -21,109 +21,133 @@
 // The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure().
 // It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker
 
-void enable_ultrasonic_ticker(){
-    ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000);   
+void enable_ultrasonic_ticker()
+{
+    ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000);
 }
 
-void disable_ultrasonic_ticker(){
-    ultrasonic_ticker.detach();   
+void disable_ultrasonic_ticker()
+{
+    ultrasonic_ticker.detach();
 }
 
-void update_ultrasonic_measure(){
-    if(waiting_for_ultrasonic == 0){
+void update_ultrasonic_measure()
+{
+    if(waiting_for_ultrasonic == 0) {
         waiting_for_ultrasonic = 1;
-        
-        char command[2];
-        command[0] = 0x00;                              // Set the command register
+        if(has_ultrasonic_sensor) {
+            char command[2];
+            command[0] = 0x00;                              // Set the command register
             command[1] = 0x51;                          // Get result is centimeters
-        primary_i2c.write(ULTRASONIC_ADDRESS, command, 2);              // Send the command to start a ranging burst
- 
-        ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000);    
-    }else{
-        debug("WARNING:  Ultrasonic sensor called too frequently.\n");   
+            primary_i2c.write(ULTRASONIC_ADDRESS, command, 2);              // Send the command to start a ranging burst
+        }
+        ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000);
+    } else {
+        debug("WARNING:  Ultrasonic sensor called too frequently.\n");
     }
 }
 
-void IF_read_ultrasonic_measure(){
-    char command[1];
-    char result[2];
-    command[0] = 0x02;                           // The start address of measure result
-    primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1);           // Send address to read a measure
-    primary_i2c.read(ULTRASONIC_ADDRESS, result, 2);                // Read two byte of measure   
-    ultrasonic_distance = (result[0]<<8)+result[1];
+void IF_read_ultrasonic_measure()
+{
+    if(has_ultrasonic_sensor) {
+        char command[1];
+        char result[2];
+        command[0] = 0x02;                           // The start address of measure result
+        primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1);           // Send address to read a measure
+        primary_i2c.read(ULTRASONIC_ADDRESS, result, 2);                // Read two byte of measure
+        ultrasonic_distance = (result[0]<<8)+result[1];
+    } else ultrasonic_distance = 0;
     ultrasonic_distance_updated = 1;
     waiting_for_ultrasonic = 0;
-    debug("US:%d cm\n",ultrasonic_distance);
+    //debug("US:%d cm\n",ultrasonic_distance);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Additional Sensing Functions
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-float get_temperature(){
-    return IF_read_from_temperature_sensor(); 
+float get_temperature()
+{   
+    if(has_temperature_sensor)return IF_read_from_temperature_sensor();
+    return 0;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Voltage Sensing Functions
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-float get_battery_voltage (){
+float get_battery_voltage ()
+{
     float raw_value = vin_battery.read();
     return raw_value * 4.95;
 }
 
-float get_current (){
+float get_current ()
+{
     // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor
     // Device gain = 500
     float raw_value = vin_current.read();
     return raw_value * 3.3;
 }
 
-float get_dc_voltage (){
+float get_dc_voltage ()
+{
     float raw_value = vin_dc.read();
-    return raw_value * 6.6;   
+    return raw_value * 6.6;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // IR Sensor Functions
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).  
+// 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 read_reflected_ir_distance ( char index ){
+float read_reflected_ir_distance ( char index )
+{
     // Sanity check
     if(index>7) return 0.0;
-    
+
     //1.  Read the ADC value without IR emitter on; store in the background_ir_values[] array
     background_ir_values [index] = IF_read_IR_adc_value(1, index );
-    
+
     //2.  Enable the relevant IR emitter by turning on its pulse output
-    switch(index){
-        case 0: case 1: case 6: case 7:
-        IF_set_IR_emitter_output(0, 1);
-        break;
-        case 2: case 3: case 4: case 5:
-        IF_set_IR_emitter_output(1, 1);
-        break;
+    switch(index) {
+        case 0:
+        case 1:
+        case 6:
+        case 7:
+            IF_set_IR_emitter_output(0, 1);
+            break;
+        case 2:
+        case 3:
+        case 4:
+        case 5:
+            IF_set_IR_emitter_output(1, 1);
+            break;
     }
-    wait_us(ir_pulse_delay);               
-    
+    wait_us(ir_pulse_delay);
+
     //3.  Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array
     illuminated_ir_values [index] = IF_read_IR_adc_value (1, index );
-    
+
     //4.  Turn off IR emitter
-    switch(index){
-        case 0: case 1: case 6: case 7:
-        IF_set_IR_emitter_output(0, 0);
-        break;
-        case 2: case 3: case 4: case 5:
-        IF_set_IR_emitter_output(1, 0);
-        break;
+    switch(index) {
+        case 0:
+        case 1:
+        case 6:
+        case 7:
+            IF_set_IR_emitter_output(0, 0);
+            break;
+        case 2:
+        case 3:
+        case 4:
+        case 5:
+            IF_set_IR_emitter_output(1, 0);
+            break;
     }
-    
+
     //5.  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;
@@ -132,109 +156,130 @@
 
 
 // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances
-float get_reflected_ir_distance ( char index ){
+float 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];   
+    return reflected_ir_distances[index];
 }
 
 // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
-unsigned short 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
-unsigned short get_illuminated_raw_ir_value ( char index ){
+unsigned short 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 illuminated_ir_values[index];   
+    return background_ir_values[index];
 }
-    
+
+// Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values
+unsigned short 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
-void store_reflected_ir_distances ( void ){
+void store_reflected_ir_distances ( void )
+{
     store_ir_values();
-    for(int i=0;i<8;i++){
-       reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
-    }  
+    for(int i=0; i<8; i++) {
+        reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
+    }
 }
 
 // Stores the rbackground and illuminated values for all 8 IR sensors
-void store_ir_values ( void ){
+void store_ir_values ( void )
+{
     store_background_raw_ir_values();
     store_illuminated_raw_ir_values();
 }
-    
+
 // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters
-void store_background_raw_ir_values ( void ){
+void store_background_raw_ir_values ( void )
+{
     ir_values_stored = 1;
-    for(int i=0;i<8;i++){
+    for(int i=0; i<8; i++) {
         background_ir_values [i] = IF_read_IR_adc_value(1,i);
     }
 }
-    
+
 // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse
-void store_illuminated_raw_ir_values ( void ){
-    //1.  Enable the front IR emitters and store the values 
+void store_illuminated_raw_ir_values ( void )
+{
+    //1.  Enable the front IR emitters and store the values
     IF_set_IR_emitter_output(0, 1);
-    wait_us(ir_pulse_delay);               
+    wait_us(ir_pulse_delay);
     illuminated_ir_values [0] = IF_read_IR_adc_value(1,0);
     illuminated_ir_values [1] = IF_read_IR_adc_value(1,1);
     illuminated_ir_values [6] = IF_read_IR_adc_value(1,6);
     illuminated_ir_values [7] = IF_read_IR_adc_value(1,7);
     IF_set_IR_emitter_output(0, 0);
-    
-    //2.  Enable the rear+side IR emitters and store the values 
+
+    //2.  Enable the rear+side IR emitters and store the values
     IF_set_IR_emitter_output(1, 1);
-    wait_us(ir_pulse_delay);               
+    wait_us(ir_pulse_delay);
     illuminated_ir_values [2] = IF_read_IR_adc_value(1,2);
     illuminated_ir_values [3] = IF_read_IR_adc_value(1,3);
     illuminated_ir_values [4] = IF_read_IR_adc_value(1,4);
     illuminated_ir_values [5] = IF_read_IR_adc_value(1,5);
     IF_set_IR_emitter_output(1, 0);
 }
-    
+
 // Converts a background and illuminated value into a distance
-float calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ){
+float 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
     approximate_distance = sqrt (approximate_distance) / 2.0;
-    
+
     // Then add adjustment value if value >27
     if(approximate_distance > 27) {
         float shift = pow(approximate_distance - 27,3);
         approximate_distance += shift;
     }
     if(approximate_distance > 90) approximate_distance = 100.0;
-    return approximate_distance;    
+    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
-unsigned short read_illuminated_raw_ir_value ( char index ){
+unsigned short read_illuminated_raw_ir_value ( char index )
+{
     //This function reads the IR strength when illuminated - used for PC system debugging purposes
     //1.  Enable the relevant IR emitter by turning on its pulse output
-     switch(index){
-        case 0: case 1: case 6: case 7:
-        IF_set_IR_emitter_output(0, 1);
-        break;
-        case 2: case 3: case 4: case 5:
-        IF_set_IR_emitter_output(1, 1);
-        break;
+    switch(index) {
+        case 0:
+        case 1:
+        case 6:
+        case 7:
+            IF_set_IR_emitter_output(0, 1);
+            break;
+        case 2:
+        case 3:
+        case 4:
+        case 5:
+            IF_set_IR_emitter_output(1, 1);
+            break;
     }
-    wait_us(ir_pulse_delay);               
+    wait_us(ir_pulse_delay);
     //2.  Read the ADC value now IR emitter is on
     unsigned short strong_value = IF_read_IR_adc_value( 1,index );
     //3.  Turn off IR emitter
-      switch(index){
-        case 0: case 1: case 6: case 7:
-        IF_set_IR_emitter_output(0, 0);
-        break;
-        case 2: case 3: case 4: case 5:
-        IF_set_IR_emitter_output(1, 0);
-        break;
+    switch(index) {
+        case 0:
+        case 1:
+        case 6:
+        case 7:
+            IF_set_IR_emitter_output(0, 0);
+            break;
+        case 2:
+        case 3:
+        case 4:
+        case 5:
+            IF_set_IR_emitter_output(1, 0);
+            break;
     }
     return strong_value;
 }
@@ -243,50 +288,56 @@
 
 
 // Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values
-unsigned short get_background_base_ir_value ( char index ){
+unsigned short get_background_base_ir_value ( char index )
+{
     // Sanity check: check range of index and that values have been stored
     if (index>4 || base_ir_values_stored == 0) return 0.0;
-    return background_base_ir_values[index];   
+    return background_base_ir_values[index];
 }
-    
+
 // Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values
-unsigned short get_illuminated_base_ir_value ( char index ){
+unsigned short get_illuminated_base_ir_value ( char index )
+{
     // Sanity check: check range of index and that values have been stored
     if (index>4 || base_ir_values_stored == 0) return 0.0;
-    return illuminated_base_ir_values[index];   
+    return illuminated_base_ir_values[index];
 }
-    
+
 // Stores the reflected distances for all 5 base IR sensors
-void store_base_ir_values ( void ){
+void store_base_ir_values ( void )
+{
     store_background_base_ir_values();
     store_illuminated_base_ir_values();
     //for(int i=0;i<5;i++){
-     //  reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]);
-    //}  
+    //  reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]);
+    //}
 }
-    
+
 // Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters
-void store_background_base_ir_values ( void ){
+void store_background_base_ir_values ( void )
+{
     base_ir_values_stored = 1;
-    for(int i=0;i<5;i++){
+    for(int i=0; i<5; i++) {
         background_base_ir_values [i] = IF_read_IR_adc_value(2,i);
     }
 }
-    
+
 // Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse
-void store_illuminated_base_ir_values ( void ){
-    //1.  Enable the base IR emitters and store the values 
+void store_illuminated_base_ir_values ( void )
+{
+    //1.  Enable the base IR emitters and store the values
     IF_set_IR_emitter_output(2, 1);
-    wait_us(base_ir_pulse_delay);      
-    for(int i=0;i<5;i++){         
-    illuminated_base_ir_values [i] = IF_read_IR_adc_value(2,i);
+    wait_us(base_ir_pulse_delay);
+    for(int i=0; i<5; i++) {
+        illuminated_base_ir_values [i] = IF_read_IR_adc_value(2,i);
     }
-    
+
     IF_set_IR_emitter_output(2, 0);
 }
-    
+
 // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm
-void store_line_position ( ){
+void store_line_position ( )
+{
     // Store background and reflected base IR values
     store_base_ir_values();
     int h_value[5];
@@ -295,49 +346,49 @@
     char count = 0;
     line_found = 0;
     line_position = 0;
-    for(int i=0;i<5;i++){
+    for(int i=0; i<5; i++) {
         if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0;
         else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i);
         if(h_value[i] < line_threshold) count++;
-    }    
-    if(count == 1){
-       line_found = 1;
-       if(h_value[0] < line_threshold) {
-           line_position = -1;
-           if(h_value[1] < line_threshold_hi) line_position = -0.8;   
-       }
-       
-       if (h_value[1] < line_threshold) {
-            line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);;   
-       }
-       if(h_value[2] < line_threshold) {
+    }
+    if(count == 1) {
+        line_found = 1;
+        if(h_value[0] < line_threshold) {
+            line_position = -1;
+            if(h_value[1] < line_threshold_hi) line_position = -0.8;
+        }
+
+        if (h_value[1] < line_threshold) {
+            line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);;
+        }
+        if(h_value[2] < line_threshold) {
             line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]);
-       }
-       if(h_value[3] < line_threshold) {
-            line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);;   
-       }
-       if(h_value[4] < line_threshold) {
-           line_position = 1; 
-           if(h_value[3] < line_threshold_hi) line_position = 0.8;  
-       }
+        }
+        if(h_value[3] < line_threshold) {
+            line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);;
+        }
+        if(h_value[4] < line_threshold) {
+            line_position = 1;
+            if(h_value[3] < line_threshold_hi) line_position = 0.8;
+        }
     }
-    if(count == 2){
-        if(h_value[0] && h_value[1] < line_threshold){
+    if(count == 2) {
+        if(h_value[0] && h_value[1] < line_threshold) {
             line_found = 1;
             line_position = -0.6;
         }
-        
-        if(h_value[1] && h_value[2] < line_threshold){
+
+        if(h_value[1] && h_value[2] < line_threshold) {
             line_found = 1;
             line_position = -0.4;
         }
-        
-        if(h_value[2] && h_value[3] < line_threshold){
+
+        if(h_value[2] && h_value[3] < line_threshold) {
             line_found = 1;
             line_position = 0.4;
         }
-        
-        if(h_value[3] && h_value[4] < line_threshold){
+
+        if(h_value[3] && h_value[4] < line_threshold) {
             line_found = 1;
             line_position = 0.6;
         }
@@ -346,17 +397,18 @@
 
 
 
-void calibrate_base_ir_sensors (void) {
+void calibrate_base_ir_sensors (void)
+{
     short white_background[5];
     short white_active[5];
     short black_background[5];
     short black_active[5];
-     for(int k=0;k<5;k++){
+    for(int k=0; k<5; k++) {
         white_background[k]=0;
         black_background[k]=0;
         white_active[k]=0;
-        black_active[k]=0;  
-     }
+        black_active[k]=0;
+    }
     pc.printf("Base IR Calibration\n");
     display.clear_display();
     display.write_string("Calibrating base");
@@ -374,107 +426,107 @@
     display.write_string("IR sensor");
     wait(0.5);
     pc.printf("\nWhite Background Results:\n");
-    
-    for(int i=0;i<5;i++){
-     wait(0.2);
-     store_background_base_ir_values();
-     
-     display.set_position(1,9);
-     display.write_string(".");
-     wait(0.2);
-     store_illuminated_base_ir_values();
-     for(int k=0;k<5;k++){
-        white_background[k]+=  get_background_base_ir_value(k);
-        white_active[k] += get_illuminated_base_ir_value(k);
-     }
-     pc.printf("Sample %d     1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n", (i+1),
-         get_background_base_ir_value(0),            get_illuminated_base_ir_value(0),
-         get_background_base_ir_value(1),            get_illuminated_base_ir_value(1),
-         get_background_base_ir_value(2),            get_illuminated_base_ir_value(2),
-         get_background_base_ir_value(3),            get_illuminated_base_ir_value(3),
-         get_background_base_ir_value(4),            get_illuminated_base_ir_value(4));
+
+    for(int i=0; i<5; i++) {
+        wait(0.2);
+        store_background_base_ir_values();
+
+        display.set_position(1,9);
+        display.write_string(".");
+        wait(0.2);
+        store_illuminated_base_ir_values();
+        for(int k=0; k<5; k++) {
+            white_background[k]+=  get_background_base_ir_value(k);
+            white_active[k] += get_illuminated_base_ir_value(k);
+        }
+        pc.printf("Sample %d     1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n", (i+1),
+                  get_background_base_ir_value(0),            get_illuminated_base_ir_value(0),
+                  get_background_base_ir_value(1),            get_illuminated_base_ir_value(1),
+                  get_background_base_ir_value(2),            get_illuminated_base_ir_value(2),
+                  get_background_base_ir_value(3),            get_illuminated_base_ir_value(3),
+                  get_background_base_ir_value(4),            get_illuminated_base_ir_value(4));
     }
-    for(int k=0;k<5;k++){
+    for(int k=0; k<5; k++) {
         white_background[k]/=5;
         white_active[k]/=5;
     }
     pc.printf("Mean results 1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n",
-         white_background[0],          white_active[0],  
-         white_background[1],          white_active[1],  
-         white_background[2],          white_active[2],  
-         white_background[3],          white_active[3],  
-         white_background[4],          white_active[4]);
-    
+              white_background[0],          white_active[0],
+              white_background[1],          white_active[1],
+              white_background[2],          white_active[2],
+              white_background[3],          white_active[3],
+              white_background[4],          white_active[4]);
+
     display.clear_display();
     display.write_string("Place robot on");
     display.set_position(1,0);
     display.write_string("black surface");
-    wait(3); 
-    
+    wait(3);
+
     display.clear_display();
     display.write_string("Calibrating base");
     display.set_position(1,0);
     display.write_string("IR sensor");
     wait(0.5);
     pc.printf("\nBlack Background Results:\n");
-    
-    for(int i=0;i<5;i++){
-          wait(0.2);
+
+    for(int i=0; i<5; i++) {
+        wait(0.2);
 
-     store_background_base_ir_values();
-     display.set_position(1,9);
-     display.write_string(".");
-     wait(0.2);
-     store_illuminated_base_ir_values();
-     for(int k=0;k<5;k++){
-        black_background[k]+=  get_background_base_ir_value(k);
-        black_active[k] += get_illuminated_base_ir_value(k);
-     }
-     pc.printf("Sample %d     1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n", (i+1),
-         get_background_base_ir_value(0),            get_illuminated_base_ir_value(0),
-         get_background_base_ir_value(1),            get_illuminated_base_ir_value(1),
-         get_background_base_ir_value(2),            get_illuminated_base_ir_value(2),
-         get_background_base_ir_value(3),            get_illuminated_base_ir_value(3),
-         get_background_base_ir_value(4),            get_illuminated_base_ir_value(4));
-    } 
-    for(int k=0;k<5;k++){
+        store_background_base_ir_values();
+        display.set_position(1,9);
+        display.write_string(".");
+        wait(0.2);
+        store_illuminated_base_ir_values();
+        for(int k=0; k<5; k++) {
+            black_background[k]+=  get_background_base_ir_value(k);
+            black_active[k] += get_illuminated_base_ir_value(k);
+        }
+        pc.printf("Sample %d     1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n", (i+1),
+                  get_background_base_ir_value(0),            get_illuminated_base_ir_value(0),
+                  get_background_base_ir_value(1),            get_illuminated_base_ir_value(1),
+                  get_background_base_ir_value(2),            get_illuminated_base_ir_value(2),
+                  get_background_base_ir_value(3),            get_illuminated_base_ir_value(3),
+                  get_background_base_ir_value(4),            get_illuminated_base_ir_value(4));
+    }
+    for(int k=0; k<5; k++) {
         black_background[k]/=5;
         black_active[k]/=5;
     }
-    pc.printf("Mean results 1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n", 
-         black_background[0],          black_active[0],  
-         black_background[1],          black_active[1],  
-         black_background[2],          black_active[2],  
-         black_background[3],          black_active[3],  
-         black_background[4],          black_active[4]);
-    
+    pc.printf("Mean results 1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n",
+              black_background[0],          black_active[0],
+              black_background[1],          black_active[1],
+              black_background[2],          black_active[2],
+              black_background[3],          black_active[3],
+              black_background[4],          black_active[4]);
+
 }
 
 
-int get_bearing_from_ir_array (unsigned short * ir_sensor_readings){
+int get_bearing_from_ir_array (unsigned short * ir_sensor_readings)
+{
     //out("Getting bearing from array: [%d][%d][%d][%d][%d][%d][%d][%d]\n",ir_sensor_readings[0],ir_sensor_readings[1],ir_sensor_readings[2],ir_sensor_readings[3],ir_sensor_readings[4],ir_sensor_readings[5],ir_sensor_readings[6],ir_sensor_readings[7]);
-    
+
     float degrees_per_radian = 57.295779513;
-    
+
     // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors
     float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432};
     float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533};
-    
+
     float sin_sum = 0;
     float cos_sum = 0;
-    
-    for(int i = 0; i < 8; i++)
-    {        
+
+    for(int i = 0; i < 8; i++) {
         // Use IR sensor reading to weight bearing vector
         sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i];
         cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i];
     }
-    
-     float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source 
+
+    float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source
     bearing *= degrees_per_radian; // Convert to degrees
 
     //out("Sin sum:%f Cos sum:%f Bearing:%f\n",sin_sum,cos_sum,bearing);
 
     return (int) bearing;
-}    
+}