Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
Diff: sensors.cpp
- Revision:
- 17:bf614e28668f
- Parent:
- 16:50686c07ad07
- Child:
- 18:9204f74069b4
diff -r 50686c07ad07 -r bf614e28668f sensors.cpp --- 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; +}