sensors.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: Sensor Functions Source File
00002  * 
00003  * Copyright 2016 University of York
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. 
00006  * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
00007  * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
00008  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
00009  * See the License for the specific language governing permissions and limitations under the License.
00010  *
00011  * File: sensors.cpp
00012  *
00013  * (C) Dept. Electronics & Computer Science, University of York
00014  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
00015  *
00016  * PsiSwarm Library Version: 0.8
00017  *
00018  * October 2016
00019  *
00020  *
00021  */
00022 
00023 #include "psiswarm.h"
00024 
00025 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00026 // Ultrasonic Sensor (SRF02) Functions
00027 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00028 
00029 // The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure().
00030 // It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker
00031 
00032 void Sensors::enable_ultrasonic_ticker()
00033 {
00034     ultrasonic_ticker.attach_us(this,&Sensors::update_ultrasonic_measure,100000);
00035 }
00036 
00037 void Sensors::disable_ultrasonic_ticker()
00038 {
00039     ultrasonic_ticker.detach();
00040 }
00041 
00042 void Sensors::update_ultrasonic_measure()
00043 {
00044     if(waiting_for_ultrasonic == 0) {
00045         waiting_for_ultrasonic = 1;
00046         if(has_ultrasonic_sensor) {
00047             char command[2];
00048             command[0] = 0x00;                              // Set the command register
00049             command[1] = 0x51;                          // Get result is centimeters
00050             primary_i2c.write(ULTRASONIC_ADDRESS, command, 2);              // Send the command to start a ranging burst
00051         }
00052         ultrasonic_timeout.attach_us(this,&Sensors::IF_read_ultrasonic_measure,70000);
00053     } else {
00054         psi.debug("WARNING:  Ultrasonic sensor called too frequently.\n");
00055     }
00056 }
00057 
00058 void Sensors::IF_read_ultrasonic_measure()
00059 {
00060     if(has_ultrasonic_sensor) {
00061         char command[1];
00062         char result[2];
00063         command[0] = 0x02;                           // The start address of measure result
00064         primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1);           // Send address to read a measure
00065         primary_i2c.read(ULTRASONIC_ADDRESS, result, 2);                // Read two byte of measure
00066         ultrasonic_distance = (result[0]<<8)+result[1];
00067     } else ultrasonic_distance = 0;
00068     ultrasonic_distance_updated = 1;
00069     waiting_for_ultrasonic = 0;
00070     //psi.debug("US:%d cm\n",ultrasonic_distance);
00071 }
00072 
00073 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00074 // Additional Sensing Functions
00075 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00076 
00077 float Sensors::get_temperature()
00078 {   
00079     if(has_temperature_sensor)return i2c_setup.IF_read_from_temperature_sensor();
00080     return 0;
00081 }
00082 
00083 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00084 // Voltage Sensing Functions
00085 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00086 
00087 float Sensors::get_battery_voltage ()
00088 {
00089     float raw_value = vin_battery.read();
00090     return raw_value * 4.95;
00091 }
00092 
00093 float Sensors::get_current ()
00094 {
00095     // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor
00096     // Device gain = 500
00097     float raw_value = vin_current.read();
00098     return raw_value * 3.3;
00099 }
00100 
00101 float Sensors::get_dc_voltage ()
00102 {
00103     float raw_value = vin_dc.read();
00104     return raw_value * 6.6;
00105 }
00106 
00107 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00108 // IR Sensor Functions
00109 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00110 
00111 // Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).
00112 // The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
00113 // NB This function is preserved for code-compatability and cases where only a single reading is needed
00114 // In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance()
00115 float Sensors::read_reflected_ir_distance ( char index )
00116 {
00117     // Sanity check
00118     if(index>7) return 0.0;
00119 
00120     //1.  Read the ADC value without IR emitter on; store in the background_ir_values[] array
00121     background_ir_values [index] = i2c_setup.IF_read_IR_adc_value(1, index );
00122 
00123     //2.  Enable the relevant IR emitter by turning on its pulse output
00124     switch(index) {
00125         case 0:
00126         case 1:
00127         case 6:
00128         case 7:
00129             i2c_setup.IF_set_IR_emitter_output(0, 1);
00130             break;
00131         case 2:
00132         case 3:
00133         case 4:
00134         case 5:
00135             i2c_setup.IF_set_IR_emitter_output(1, 1);
00136             break;
00137     }
00138     wait_us(ir_pulse_delay);
00139 
00140     //3.  Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array
00141     illuminated_ir_values [index] = i2c_setup.IF_read_IR_adc_value (1, index );
00142 
00143     //4.  Turn off IR emitter
00144     switch(index) {
00145         case 0:
00146         case 1:
00147         case 6:
00148         case 7:
00149             i2c_setup.IF_set_IR_emitter_output(0, 0);
00150             break;
00151         case 2:
00152         case 3:
00153         case 4:
00154         case 5:
00155             i2c_setup.IF_set_IR_emitter_output(1, 0);
00156             break;
00157     }
00158 
00159     //5.  Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances()
00160     reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]);
00161     ir_values_stored = 1;
00162     return reflected_ir_distances [index];
00163 }
00164 
00165 
00166 // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances
00167 float Sensors::get_reflected_ir_distance ( char index )
00168 {
00169     // Sanity check: check range of index and that values have been stored
00170     if (index>7 || ir_values_stored == 0) return 0.0;
00171     return reflected_ir_distances[index];
00172 }
00173 
00174 // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
00175 char Sensors::get_background_raw_ir_value ( char index )
00176 {
00177     // Sanity check: check range of index and that values have been stored
00178     if (index>7 || ir_values_stored == 0) return 0.0;
00179     return background_ir_values[index];
00180 }
00181 
00182 // Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values
00183 unsigned short Sensors::get_illuminated_raw_ir_value ( char index )
00184 {
00185     // Sanity check: check range of index and that values have been stored
00186     if (index>7 || ir_values_stored == 0) return 0.0;
00187     return illuminated_ir_values[index];
00188 }
00189 
00190 // Stores the reflected distances for all 8 IR sensors
00191 void Sensors::store_reflected_ir_distances ( void )
00192 {
00193     store_ir_values();
00194     for(int i=0; i<8; i++) {
00195         reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
00196     }
00197 }
00198 
00199 // Stores the background and illuminated values for all 8 IR sensors
00200 void Sensors::store_ir_values ( void )
00201 {
00202     store_background_raw_ir_values();
00203     store_illuminated_raw_ir_values();
00204 }
00205 
00206 // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters
00207 void Sensors::store_background_raw_ir_values ( void )
00208 {
00209     ir_values_stored = 1;
00210     for(int i=0; i<8; i++) {
00211         background_ir_values [i] = i2c_setup.IF_read_IR_adc_value(1,i);
00212     }
00213 }
00214 
00215 // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse
00216 void Sensors::store_illuminated_raw_ir_values ( void )
00217 {
00218     //1.  Enable the front IR emitters and store the values
00219     i2c_setup.IF_set_IR_emitter_output(0, 1);
00220     wait_us(ir_pulse_delay);
00221     illuminated_ir_values [0] = i2c_setup.IF_read_IR_adc_value(1,0);
00222     illuminated_ir_values [1] = i2c_setup.IF_read_IR_adc_value(1,1);
00223     illuminated_ir_values [6] = i2c_setup.IF_read_IR_adc_value(1,6);
00224     illuminated_ir_values [7] = i2c_setup.IF_read_IR_adc_value(1,7);
00225     i2c_setup.IF_set_IR_emitter_output(0, 0);
00226 
00227     //2.  Enable the rear+side IR emitters and store the values
00228     i2c_setup.IF_set_IR_emitter_output(1, 1);
00229     wait_us(ir_pulse_delay);
00230     illuminated_ir_values [2] = i2c_setup.IF_read_IR_adc_value(1,2);
00231     illuminated_ir_values [3] = i2c_setup.IF_read_IR_adc_value(1,3);
00232     illuminated_ir_values [4] = i2c_setup.IF_read_IR_adc_value(1,4);
00233     illuminated_ir_values [5] = i2c_setup.IF_read_IR_adc_value(1,5);
00234     i2c_setup.IF_set_IR_emitter_output(1, 0);
00235 }
00236 
00237 // Converts a background and illuminated value into a distance
00238 float Sensors::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value )
00239 {
00240     float approximate_distance = 4000 + background_value - illuminated_value;
00241     if(approximate_distance < 0) approximate_distance = 0;
00242 
00243     // Very approximate: root value, divide by 2, approx distance in mm
00244     approximate_distance = sqrt (approximate_distance) / 2.0;
00245 
00246     // Then add adjustment value if value >27
00247     if(approximate_distance > 27) {
00248         float shift = pow(approximate_distance - 27,3);
00249         approximate_distance += shift;
00250     }
00251     if(approximate_distance > 90) approximate_distance = 100.0;
00252     return approximate_distance;
00253 }
00254 
00255 // Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse
00256 unsigned short Sensors::read_illuminated_raw_ir_value ( char index )
00257 {
00258     //This function reads the IR strength when illuminated - used for PC system debugging purposes
00259     //1.  Enable the relevant IR emitter by turning on its pulse output
00260     switch(index) {
00261         case 0:
00262         case 1:
00263         case 6:
00264         case 7:
00265             i2c_setup.IF_set_IR_emitter_output(0, 1);
00266             break;
00267         case 2:
00268         case 3:
00269         case 4:
00270         case 5:
00271             i2c_setup.IF_set_IR_emitter_output(1, 1);
00272             break;
00273     }
00274     wait_us(ir_pulse_delay);
00275     //2.  Read the ADC value now IR emitter is on
00276     unsigned short strong_value = i2c_setup.IF_read_IR_adc_value( 1,index );
00277     //3.  Turn off IR emitter
00278     switch(index) {
00279         case 0:
00280         case 1:
00281         case 6:
00282         case 7:
00283             i2c_setup.IF_set_IR_emitter_output(0, 0);
00284             break;
00285         case 2:
00286         case 3:
00287         case 4:
00288         case 5:
00289             i2c_setup.IF_set_IR_emitter_output(1, 0);
00290             break;
00291     }
00292     return strong_value;
00293 }
00294 
00295 // Base IR sensor functions
00296 
00297 
00298 // Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values
00299 unsigned short Sensors::get_background_base_ir_value ( char index )
00300 {
00301     // Sanity check: check range of index and that values have been stored
00302     if (index>4 || base_ir_values_stored == 0) return 0.0;
00303     return background_base_ir_values[index];
00304 }
00305 
00306 // Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values
00307 unsigned short Sensors::get_illuminated_base_ir_value ( char index )
00308 {
00309     // Sanity check: check range of index and that values have been stored
00310     if (index>4 || base_ir_values_stored == 0) return 0.0;
00311     return illuminated_base_ir_values[index];
00312 }
00313 
00314 // Stores the reflected distances for all 5 base IR sensors
00315 void Sensors::store_base_ir_values ( void )
00316 {
00317     store_background_base_ir_values();
00318     store_illuminated_base_ir_values();
00319     //for(int i=0;i<5;i++){
00320     //  reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]);
00321     //}
00322 }
00323 
00324 // Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters
00325 void Sensors::store_background_base_ir_values ( void )
00326 {
00327     base_ir_values_stored = 1;
00328     for(int i=0; i<5; i++) {
00329         background_base_ir_values [i] = i2c_setup.IF_read_IR_adc_value(2,i);
00330     }
00331 }
00332 
00333 // Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse
00334 void Sensors::store_illuminated_base_ir_values ( void )
00335 {
00336     //1.  Enable the base IR emitters and store the values
00337     i2c_setup.IF_set_IR_emitter_output(2, 1);
00338     wait_us(base_ir_pulse_delay);
00339     for(int i=0; i<5; i++) {
00340         illuminated_base_ir_values [i] = i2c_setup.IF_read_IR_adc_value(2,i);
00341     }
00342 
00343     i2c_setup.IF_set_IR_emitter_output(2, 0);
00344 }
00345 
00346 // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm
00347 void Sensors::store_line_position ( )
00348 {
00349     // Store background and reflected base IR values
00350     store_base_ir_values();
00351     int h_value[5];
00352     int line_threshold = 1000;
00353     int line_threshold_hi = 2000;
00354     char count = 0;
00355     line_found = 0;
00356     line_position = 0;
00357     for(int i=0; i<5; i++) {
00358         if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0;
00359         else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i);
00360         if(h_value[i] < line_threshold) count++;
00361     }
00362     if(count == 1) {
00363         line_found = 1;
00364         if(h_value[0] < line_threshold) {
00365             line_position = -1;
00366             if(h_value[1] < line_threshold_hi) line_position = -0.8;
00367         }
00368 
00369         if (h_value[1] < line_threshold) {
00370             line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);;
00371         }
00372         if(h_value[2] < line_threshold) {
00373             line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]);
00374         }
00375         if(h_value[3] < line_threshold) {
00376             line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);;
00377         }
00378         if(h_value[4] < line_threshold) {
00379             line_position = 1;
00380             if(h_value[3] < line_threshold_hi) line_position = 0.8;
00381         }
00382     }
00383     if(count == 2) {
00384         if(h_value[0] && h_value[1] < line_threshold) {
00385             line_found = 1;
00386             line_position = -0.6;
00387         }
00388 
00389         if(h_value[1] && h_value[2] < line_threshold) {
00390             line_found = 1;
00391             line_position = -0.4;
00392         }
00393 
00394         if(h_value[2] && h_value[3] < line_threshold) {
00395             line_found = 1;
00396             line_position = 0.4;
00397         }
00398 
00399         if(h_value[3] && h_value[4] < line_threshold) {
00400             line_found = 1;
00401             line_position = 0.6;
00402         }
00403     }
00404 }
00405 
00406 // Returns the subtraction of the background base IR value from the reflection based on last call of store_illuminated_base_ir_values
00407 unsigned short Sensors::calculate_base_ir_value ( char index ){
00408     // If the index is not in the correct range or the base IR values have not been stored, return zero
00409     if (index>4 || base_ir_values_stored == 0) return 0.0;
00410     // If the reflection value is greater than the background value, return the subtraction 
00411     if(illuminated_base_ir_values[index] > background_base_ir_values[index]){
00412         return illuminated_base_ir_values[index] - background_base_ir_values[index];
00413     //Otherwise return zero
00414     } else {
00415         return 0.0;   
00416     }
00417 }
00418 
00419 // Returns the subtraction of the background side IR value from the reflection based on last call of store_illuminated_base_ir_values
00420 unsigned short Sensors::calculate_side_ir_value ( char index ){
00421     // If the index is not in the correct range or the base IR values have not been stored, return zero
00422     if (index>7 || ir_values_stored == 0) return 0.0;
00423     // If the reflection value is greater than the background value, return the subtraction
00424     if(illuminated_ir_values[index] > background_ir_values[index]){
00425         return illuminated_ir_values[index] - background_ir_values[index];
00426     //Otherwise return zero
00427     } else {
00428         return 0.0;   
00429     }
00430 }
00431 
00432 void Sensors::calibrate_base_ir_sensors (void)
00433 {
00434     short white_background[5];
00435     short white_active[5];
00436     short black_background[5];
00437     short black_active[5];
00438     for(int k=0; k<5; k++) {
00439         white_background[k]=0;
00440         black_background[k]=0;
00441         white_active[k]=0;
00442         black_active[k]=0;
00443     }
00444     pc.printf("Base IR Calibration\n");
00445     display.clear_display();
00446     display.write_string("Calibrating base");
00447     display.set_position(1,0);
00448     display.write_string("IR sensor");
00449     wait(0.5);
00450     display.clear_display();
00451     display.write_string("Place robot on");
00452     display.set_position(1,0);
00453     display.write_string("white surface");
00454     wait(3);
00455     display.clear_display();
00456     display.write_string("Calibrating base");
00457     display.set_position(1,0);
00458     display.write_string("IR sensor");
00459     wait(0.5);
00460     pc.printf("\nWhite Background Results:\n");
00461 
00462     for(int i=0; i<5; i++) {
00463         wait(0.2);
00464         store_background_base_ir_values();
00465 
00466         display.set_position(1,9);
00467         display.write_string(".");
00468         wait(0.2);
00469         store_illuminated_base_ir_values();
00470         for(int k=0; k<5; k++) {
00471             white_background[k]+=  get_background_base_ir_value(k);
00472             white_active[k] += get_illuminated_base_ir_value(k);
00473         }
00474         pc.printf("Sample %d     1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n", (i+1),
00475                   get_background_base_ir_value(0),            get_illuminated_base_ir_value(0),
00476                   get_background_base_ir_value(1),            get_illuminated_base_ir_value(1),
00477                   get_background_base_ir_value(2),            get_illuminated_base_ir_value(2),
00478                   get_background_base_ir_value(3),            get_illuminated_base_ir_value(3),
00479                   get_background_base_ir_value(4),            get_illuminated_base_ir_value(4));
00480     }
00481     for(int k=0; k<5; k++) {
00482         white_background[k]/=5;
00483         white_active[k]/=5;
00484     }
00485     pc.printf("Mean results 1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n",
00486               white_background[0],          white_active[0],
00487               white_background[1],          white_active[1],
00488               white_background[2],          white_active[2],
00489               white_background[3],          white_active[3],
00490               white_background[4],          white_active[4]);
00491 
00492     display.clear_display();
00493     display.write_string("Place robot on");
00494     display.set_position(1,0);
00495     display.write_string("black surface");
00496     wait(3);
00497 
00498     display.clear_display();
00499     display.write_string("Calibrating base");
00500     display.set_position(1,0);
00501     display.write_string("IR sensor");
00502     wait(0.5);
00503     pc.printf("\nBlack Background Results:\n");
00504 
00505     for(int i=0; i<5; i++) {
00506         wait(0.2);
00507 
00508         store_background_base_ir_values();
00509         display.set_position(1,9);
00510         display.write_string(".");
00511         wait(0.2);
00512         store_illuminated_base_ir_values();
00513         for(int k=0; k<5; k++) {
00514             black_background[k]+=  get_background_base_ir_value(k);
00515             black_active[k] += get_illuminated_base_ir_value(k);
00516         }
00517         pc.printf("Sample %d     1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n", (i+1),
00518                   get_background_base_ir_value(0),            get_illuminated_base_ir_value(0),
00519                   get_background_base_ir_value(1),            get_illuminated_base_ir_value(1),
00520                   get_background_base_ir_value(2),            get_illuminated_base_ir_value(2),
00521                   get_background_base_ir_value(3),            get_illuminated_base_ir_value(3),
00522                   get_background_base_ir_value(4),            get_illuminated_base_ir_value(4));
00523     }
00524     for(int k=0; k<5; k++) {
00525         black_background[k]/=5;
00526         black_active[k]/=5;
00527     }
00528     pc.printf("Mean results 1: %04d-%04d  2: %04d-%04d  3: %04d-%04d  4: %04d-%04d  5: %04d-%04d\n",
00529               black_background[0],          black_active[0],
00530               black_background[1],          black_active[1],
00531               black_background[2],          black_active[2],
00532               black_background[3],          black_active[3],
00533               black_background[4],          black_active[4]);
00534 
00535 }
00536 
00537 
00538 int Sensors::get_bearing_from_ir_array (unsigned short * ir_sensor_readings)
00539 {
00540     //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]);
00541 
00542     float degrees_per_radian = 57.295779513;
00543 
00544     // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors
00545     float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432};
00546     float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533};
00547 
00548     float sin_sum = 0;
00549     float cos_sum = 0;
00550 
00551     for(int i = 0; i < 8; i++) {
00552         // Use IR sensor reading to weight bearing vector
00553         sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i];
00554         cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i];
00555     }
00556 
00557     float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source
00558     bearing *= degrees_per_radian; // Convert to degrees
00559 
00560     //out("Sin sum:%f Cos sum:%f Bearing:%f\n",sin_sum,cos_sum,bearing);
00561 
00562     return (int) bearing;
00563 }
00564