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