Diff: sensors.cpp

Revision:

17:bf614e28668f

Parent:

16:50686c07ad07

Child:

18:9204f74069b4

--- a/sensors.cpp	Thu Jun 01 23:02:32 2017 +0000
+++ b/sensors.cpp	Sun Jun 04 13:11:09 2017 +0000
@@ -1,11 +1,11 @@
 /* University of York Robotics Laboratory PsiSwarm Library: Sensor Functions Source File
- * 
+ *
  * Copyright 2017 University of York
  *
- * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. 
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
  * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and limitations under the License.
  *
  * File: sensors.cpp
@@ -22,6 +22,8 @@
 
 #include "psiswarm.h"
 
+
+
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Ultrasonic Sensor (SRF02) Functions
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -75,7 +77,7 @@
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 float Sensors::get_temperature()
-{   
+{
     if(has_temperature_sensor)return i2c_setup.IF_read_from_temperature_sensor();
     return 0;
 }
@@ -108,6 +110,8 @@
 // IR Sensor Functions
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
+int cv_bir_w[5],cv_bir_b[5];
+
 // 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
@@ -344,7 +348,8 @@
 }
 
 // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm
-void Sensors::store_line_position ( )
+// Old version (doesn't use calibration values)
+void Sensors::store_line_position_old ( )
 {
     // Store background and reflected base IR values
     store_base_ir_values();
@@ -403,67 +408,163 @@
     }
 }
 
+// New version (uses calibration values)
+void Sensors::store_line_position (void)
+{
+    store_line_position(0.5);
+}
+
+// Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm
+void Sensors::store_line_position (float line_threshold)
+{
+    // Store background and reflected base IR values
+    store_base_ir_values();
+    float calibrated_values[5];
+    float adjust_values[5];
+    char count = 0;
+    for(int i=0; i<5; i++) {
+        calibrated_values[i] = get_calibrated_base_ir_value(i);
+        if(calibrated_values[i] < line_threshold) count ++;
+        adjust_values[i] = 1 - calibrated_values[i];
+        adjust_values[i] *= adjust_values[i];
+        adjust_values[i] *= 0.5f;
+        
+    }
+
+    line_found = 0;
+    line_position = 0;
+
+    if(count == 1) {
+        line_found = 1;
+        if(calibrated_values[0] < line_threshold) {
+            line_position = -1 + adjust_values[1];
+        }
+
+        if (calibrated_values[1] < line_threshold) {
+            line_position = -0.5 - adjust_values[0] + adjust_values[2];
+        }
+        if(calibrated_values[2] < line_threshold) {
+            line_position = 0 - adjust_values[1] + adjust_values[3];
+        }
+        if(calibrated_values[3] < line_threshold) {
+            line_position = 0.5 - adjust_values[2] + adjust_values[4];
+        }
+        if(calibrated_values[4] < line_threshold) {
+            line_position = 1 - adjust_values[3];
+        }
+    }
+
+    if(count == 2) {
+        if(calibrated_values[0] < line_threshold && calibrated_values[1] < line_threshold) {
+            line_found = 1;
+            line_position = -0.75 - adjust_values[0] + adjust_values[1];
+        }
+
+        if(calibrated_values[1] < line_threshold && calibrated_values[2] < line_threshold) {
+            line_found = 1;
+            line_position = -0.25 -adjust_values[1] + adjust_values[2];
+        }
+
+        if(calibrated_values[2] < line_threshold && calibrated_values[3] < line_threshold) {
+            line_found = 1;
+            line_position = 0.25 - adjust_values[2] + adjust_values[3];
+        }
+
+        if(calibrated_values[3] < line_threshold && calibrated_values[4] < line_threshold) {
+            line_found = 1;
+            line_position = 0.75 - adjust_values[3] + adjust_values[4];
+        }
+    }
+
+    if(count == 3) {
+        if (calibrated_values[1] < line_threshold && calibrated_values[2] < line_threshold && calibrated_values[3] < line_threshold) {
+            line_found = 1;
+            line_position = 0 - adjust_values[1] + adjust_values[3];
+        }
+    }
+    if(line_position < -1) line_position = -1;
+    if(line_position > 1) line_position = 1;
+}
+
 // Returns the subtraction of the background base IR value from the reflection based on last call of store_illuminated_base_ir_values
-unsigned short Sensors::calculate_base_ir_value ( char index ){
+unsigned short Sensors::calculate_base_ir_value ( char index )
+{
     // If the index is not in the correct range or the base IR values have not been stored, return zero
     if (index>4 || base_ir_values_stored == 0) return 0.0;
-    // If the reflection value is greater than the background value, return the subtraction 
-    if(illuminated_base_ir_values[index] > background_base_ir_values[index]){
+    // If the reflection value is greater than the background value, return the subtraction
+    if(illuminated_base_ir_values[index] > background_base_ir_values[index]) {
         return illuminated_base_ir_values[index] - background_base_ir_values[index];
-    //Otherwise return zero
+        //Otherwise return zero
     } else {
-        return 0.0;   
+        return 0.0;
     }
 }
 
+float Sensors::get_calibrated_base_ir_value ( char index )
+{
+    float uncalibrated_value = (float) calculate_base_ir_value(index);
+    float lower = 0.9f * cv_bir_b[index];
+    float calibrated_value = uncalibrated_value - lower;
+    float upper = 1.1f * (cv_bir_w[index] - lower);
+    calibrated_value /= upper;
+    if(calibrated_value < 0) calibrated_value = 0;
+    if(calibrated_value > 1) calibrated_value = 1;
+    return calibrated_value;
+}
+
 // Returns the subtraction of the background side IR value from the reflection based on last call of store_illuminated_base_ir_values
-unsigned short Sensors::calculate_side_ir_value ( char index ){
+unsigned short Sensors::calculate_side_ir_value ( char index )
+{
     // If the index is not in the correct range or the base IR values have not been stored, return zero
     if (index>7 || ir_values_stored == 0) return 0.0;
     // If the reflection value is greater than the background value, return the subtraction
-    if(illuminated_ir_values[index] > background_ir_values[index]){
+    if(illuminated_ir_values[index] > background_ir_values[index]) {
         return illuminated_ir_values[index] - background_ir_values[index];
-    //Otherwise return zero
+        //Otherwise return zero
     } else {
-        return 0.0;   
+        return 0.0;
     }
 }
 
-void Sensors::calibrate_base_ir_sensors (void)
+void Sensors::calibrate_base_sensors (void)
 {
+    char test_colour_sensor = has_base_colour_sensor;
     short white_background[5];
     short white_active[5];
     short black_background[5];
     short black_active[5];
+    int white_colour[4];
+    int black_colour[4];
     for(int k=0; k<5; k++) {
         white_background[k]=0;
         black_background[k]=0;
         white_active[k]=0;
         black_active[k]=0;
     }
-    pc.printf("Base IR Calibration\n");
+
+    pc.printf("\nBase Sensor Calibration\n");
+    display.clear_display();
+    display.write_string("Starting sensor");
+    display.set_position(1,0);
+    display.write_string("calibration...");
+    wait(1);
+    display.clear_display();
+    display.write_string("Place robot on");
+    display.set_position(1,0);
+    display.write_string("white surface");
+    wait(4);
     display.clear_display();
     display.write_string("Calibrating base");
     display.set_position(1,0);
     display.write_string("IR sensor");
     wait(0.5);
-    display.clear_display();
-    display.write_string("Place robot on");
-    display.set_position(1,0);
-    display.write_string("white surface");
-    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("\nWhite Background Results:\n");
+    pc.printf("\n\nWhite Surface IR Results:\n");
 
     for(int i=0; i<5; i++) {
         wait(0.2);
         store_background_base_ir_values();
 
-        display.set_position(1,9);
+        display.set_position(1,9+i);
         display.write_string(".");
         wait(0.2);
         store_illuminated_base_ir_values();
@@ -488,19 +589,52 @@
               white_background[2],          white_active[2],
               white_background[3],          white_active[3],
               white_background[4],          white_active[4]);
+    wait(0.25);
+    char retake_sample = 1;
+    if(test_colour_sensor) {
+        display.clear_display();
+        display.write_string("Calibrating base");
+        display.set_position(1,0);
+        display.write_string("colour sensor");
+        wait(0.2);
+        char retake_white_count = 0;
+
+        while(retake_sample == 1 && retake_white_count<3) {
+            wait(0.2);
+            led.set_base_led(1);
+            colour.enable_base_colour_sensor();
+            wait(0.03);
+            colour.read_base_colour_sensor_values( white_colour );
+            colour.disable_base_colour_sensor();
+            led.set_base_led(0);
+            if(white_colour[1] < 1 || white_colour[1] > 1022 || white_colour[2] < 1 || white_colour[2] > 1022 || white_colour[3] < 1 || white_colour[3] > 1022) {
+                pc.printf("Warning: Colour tested exceeded expected range\n");
+                retake_white_count ++;
+            } else retake_sample=0;
+        }
+        if(retake_white_count > 2) {
+            test_colour_sensor = 0;
+            pc.printf("The colour sensor test has produced results outside expected range; check gain settings and test setup");
+        } else {
+            wait(0.1);
+            pc.printf("\n\nWhite Surface Colour Sensor Results:\n");
+            pc.printf("BRIGHTNESS:%4d RED:%4d GREEN:%4d BLUE:%d\n",white_colour[0],white_colour[1],white_colour[2],white_colour[3]);
+        }
+    }
+    wait(0.5);
 
     display.clear_display();
     display.write_string("Place robot on");
     display.set_position(1,0);
     display.write_string("black surface");
-    wait(3);
+    wait(3.5);
 
     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");
+    pc.printf("\nBlack Surface Results:\n");
 
     for(int i=0; i<5; i++) {
         wait(0.2);
@@ -531,7 +665,73 @@
               black_background[2],          black_active[2],
               black_background[3],          black_active[3],
               black_background[4],          black_active[4]);
-
+    wait(0.25);
+    if(test_colour_sensor) {
+        display.clear_display();
+        display.write_string("Calibrating base");
+        display.set_position(1,0);
+        display.write_string("colour sensor");
+        wait(0.2);
+        char retake_black_count = 0;
+        retake_sample = 1;
+        while(retake_sample == 1 && retake_black_count<3) {
+            wait(0.2);
+            led.set_base_led(1);
+            colour.enable_base_colour_sensor();
+            wait(0.03);
+            colour.read_base_colour_sensor_values( black_colour );
+            colour.disable_base_colour_sensor();
+            led.set_base_led(0);
+            if(black_colour[1] < 1 || black_colour[1] > 1022 || black_colour[2] < 1 || black_colour[2] > 1022 || black_colour[3] < 1 || black_colour[3] > 1022) {
+                pc.printf("Warning: Colour tested exceeded expected range\n");
+                retake_black_count ++;
+            } else retake_sample=0;
+        }
+        if(retake_black_count > 2) {
+            test_colour_sensor = 0;
+            pc.printf("The colour sensor test has produced results outside expected range; check gain settings and test setup");
+        } else {
+            wait(0.1);
+            pc.printf("\n\nBlack Surface Colour Sensor Results:\n");
+            pc.printf("BRIGHTNESS:%4d RED:%4d GREEN:%4d BLUE:%d\n",black_colour[0],black_colour[1],black_colour[2],black_colour[3]);
+        }
+    }
+    wait(1);
+    while(i2c_setup.IF_get_switch_state() != 0) {
+        display.clear_display();
+        display.set_position(0,0);
+        display.write_string("RELEASE SWITCH!");
+        wait(0.1);
+    }
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string("^ REJECT");
+    display.set_position(1,2);
+    display.write_string("ACCEPT");
+    char switch_pos = i2c_setup.IF_get_switch_state();
+    while(switch_pos != 1 && switch_pos != 2) switch_pos = i2c_setup.IF_get_switch_state();
+    if(switch_pos == 2) {
+        pc.printf("\nChanges accepted:  Updating firmware values in EEPROM\n");
+        display.clear_display();
+        display.set_position(0,0);
+        display.write_string("UPDATING");
+        display.set_position(1,0);
+        display.write_string("FIRMWARE");
+        wait(0.5);
+        pc.printf("- Updating bass IR sensor values\n");
+        eprom.IF_write_base_ir_calibration_values(white_active,black_active);
+        if(test_colour_sensor == 1) {
+            wait(0.5);
+            pc.printf("- Updating bass colour sensor values\n");
+            eprom.IF_write_base_colour_calibration_values(white_colour,black_colour);
+        }
+        wait(1.5);
+    } else {
+        pc.printf("\nChanges rejected.\n");
+    }
+    display.clear_display();
+    wait(0.5);
+    pc.printf("Base sensor calibration routine complete\n\n");
 }
 
 
@@ -562,3 +762,16 @@
     return (int) bearing;
 }
 
+void Sensors::IF_set_base_calibration_values(int bir1w, int bir2w, int bir3w, int bir4w, int bir5w, int bir1b, int bir2b, int bir3b, int bir4b, int bir5b)
+{
+    cv_bir_w[0] = bir1w;
+    cv_bir_w[1] = bir2w;
+    cv_bir_w[2] = bir3w;
+    cv_bir_w[3] = bir4w;
+    cv_bir_w[4] = bir5w;
+    cv_bir_b[0] = bir1b;
+    cv_bir_b[1] = bir2b;
+    cv_bir_b[2] = bir3b;
+    cv_bir_b[3] = bir4b;
+    cv_bir_b[4] = bir5b;
+}