sensors.cpp

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