C++ Library for the PsiSwarm Robot - Version 0.7
Fork of PsiSwarmV7 by
Diff: sensors.cpp
- Revision:
- 0:d6269d17c8cf
- Child:
- 1:060690a934a9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sensors.cpp Thu Feb 04 21:48:54 2016 +0000 @@ -0,0 +1,480 @@ +/* University of York Robotics Laboratory PsiSwarm Library: Sensor Functions Source File + * + * File: sensors.cpp + * + * (C) Dept. Electronics & Computer Science, University of York + * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis + * + * PsiSwarm Library Version: 0.4 + * + * February 2016 + * + * + */ + +#include "psiswarm.h" + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Ultrasonic Sensor (SRF02) Functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +// The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure(). +// It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker + +void enable_ultrasonic_ticker(){ + ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000); +} + +void disable_ultrasonic_ticker(){ + ultrasonic_ticker.detach(); +} + +void update_ultrasonic_measure(){ + if(waiting_for_ultrasonic == 0){ + waiting_for_ultrasonic = 1; + + char command[2]; + command[0] = 0x00; // Set the command register + command[1] = 0x51; // Get result is centimeters + primary_i2c.write(ULTRASONIC_ADDRESS, command, 2); // Send the command to start a ranging burst + + ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000); + }else{ + debug("WARNING: Ultrasonic sensor called too frequently.\n"); + } +} + +void IF_read_ultrasonic_measure(){ + char command[1]; + char result[2]; + command[0] = 0x02; // The start address of measure result + primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1); // Send address to read a measure + primary_i2c.read(ULTRASONIC_ADDRESS, result, 2); // Read two byte of measure + ultrasonic_distance = (result[0]<<8)+result[1]; + ultrasonic_distance_updated = 1; + waiting_for_ultrasonic = 0; + debug("US:%d cm\n",ultrasonic_distance); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Additional Sensing Functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +float get_temperature(){ + return IF_read_from_temperature_sensor(); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Voltage Sensing Functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +float get_battery_voltage (){ + float raw_value = vin_battery.read(); + return raw_value * 4.95; +} + +float get_current (){ + // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor + // Device gain = 500 + float raw_value = vin_current.read(); + return raw_value * 3.3; +} + +float get_dc_voltage (){ + float raw_value = vin_dc.read(); + return raw_value * 6.6; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// IR Sensor Functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +// 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 +// In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance() +float read_reflected_ir_distance ( char index ){ + // Sanity check + if(index>7) return 0.0; + + //1. Read the ADC value without IR emitter on; store in the background_ir_values[] array + background_ir_values [index] = IF_read_IR_adc_value(1, index ); + + //2. Enable the relevant IR emitter by turning on its pulse output + switch(index){ + case 0: case 1: case 6: case 7: + IF_set_IR_emitter_output(0, 1); + break; + case 2: case 3: case 4: case 5: + IF_set_IR_emitter_output(1, 1); + break; + } + wait_us(ir_pulse_delay); + + //3. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array + illuminated_ir_values [index] = IF_read_IR_adc_value (1, index ); + + //4. Turn off IR emitter + switch(index){ + case 0: case 1: case 6: case 7: + IF_set_IR_emitter_output(0, 0); + break; + case 2: case 3: case 4: case 5: + IF_set_IR_emitter_output(1, 0); + break; + } + + //5. Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances() + reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]); + ir_values_stored = 1; + return reflected_ir_distances [index]; +} + + +// Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances +float get_reflected_ir_distance ( char index ){ + // Sanity check: check range of index and that values have been stored + if (index>7 || ir_values_stored == 0) return 0.0; + return reflected_ir_distances[index]; +} + +// Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values +unsigned short get_background_raw_ir_value ( char index ){ + // Sanity check: check range of index and that values have been stored + if (index>7 || ir_values_stored == 0) return 0.0; + return background_ir_values[index]; +} + +// Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values +unsigned short get_illuminated_raw_ir_value ( char index ){ + // Sanity check: check range of index and that values have been stored + if (index>7 || ir_values_stored == 0) return 0.0; + return illuminated_ir_values[index]; +} + +// Stores the reflected distances for all 8 IR sensors +void store_reflected_ir_distances ( void ){ + store_ir_values(); + for(int i=0;i<8;i++){ + reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]); + } +} + +// Stores the rbackground and illuminated values for all 8 IR sensors +void store_ir_values ( void ){ + store_background_raw_ir_values(); + store_illuminated_raw_ir_values(); +} + +// Stores the raw ADC values for all 8 IR sensors without enabling IR emitters +void store_background_raw_ir_values ( void ){ + ir_values_stored = 1; + for(int i=0;i<8;i++){ + background_ir_values [i] = IF_read_IR_adc_value(1,i); + } +} + +// Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse +void store_illuminated_raw_ir_values ( void ){ + //1. Enable the front IR emitters and store the values + IF_set_IR_emitter_output(0, 1); + wait_us(ir_pulse_delay); + illuminated_ir_values [0] = IF_read_IR_adc_value(1,0); + illuminated_ir_values [1] = IF_read_IR_adc_value(1,1); + illuminated_ir_values [6] = IF_read_IR_adc_value(1,6); + illuminated_ir_values [7] = IF_read_IR_adc_value(1,7); + IF_set_IR_emitter_output(0, 0); + + //2. Enable the rear+side IR emitters and store the values + IF_set_IR_emitter_output(1, 1); + wait_us(ir_pulse_delay); + illuminated_ir_values [2] = IF_read_IR_adc_value(1,2); + illuminated_ir_values [3] = IF_read_IR_adc_value(1,3); + illuminated_ir_values [4] = IF_read_IR_adc_value(1,4); + illuminated_ir_values [5] = IF_read_IR_adc_value(1,5); + IF_set_IR_emitter_output(1, 0); +} + +// Converts a background and illuminated value into a distance +float calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ){ + float approximate_distance = 4000 + background_value - illuminated_value; + if(approximate_distance < 0) approximate_distance = 0; + + // Very approximate: root value, divide by 2, approx distance in mm + approximate_distance = sqrt (approximate_distance) / 2.0; + + // Then add adjustment value if value >27 + if(approximate_distance > 27) { + float shift = pow(approximate_distance - 27,3); + approximate_distance += shift; + } + if(approximate_distance > 90) approximate_distance = 100.0; + return approximate_distance; +} + +// Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse +unsigned short read_illuminated_raw_ir_value ( char index ){ + //This function reads the IR strength when illuminated - used for PC system debugging purposes + //1. Enable the relevant IR emitter by turning on its pulse output + switch(index){ + case 0: case 1: case 6: case 7: + IF_set_IR_emitter_output(0, 1); + break; + case 2: case 3: case 4: case 5: + IF_set_IR_emitter_output(1, 1); + break; + } + wait_us(ir_pulse_delay); + //2. Read the ADC value now IR emitter is on + unsigned short strong_value = IF_read_IR_adc_value( 1,index ); + //3. Turn off IR emitter + switch(index){ + case 0: case 1: case 6: case 7: + IF_set_IR_emitter_output(0, 0); + break; + case 2: case 3: case 4: case 5: + IF_set_IR_emitter_output(1, 0); + break; + } + return strong_value; +} + +// Base IR sensor functions + + +// Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values +unsigned short get_background_base_ir_value ( char index ){ + // Sanity check: check range of index and that values have been stored + if (index>4 || base_ir_values_stored == 0) return 0.0; + return background_base_ir_values[index]; +} + +// Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values +unsigned short get_illuminated_base_ir_value ( char index ){ + // Sanity check: check range of index and that values have been stored + if (index>4 || base_ir_values_stored == 0) return 0.0; + return illuminated_base_ir_values[index]; +} + +// Stores the reflected distances for all 5 base IR sensors +void store_base_ir_values ( void ){ + store_background_base_ir_values(); + store_illuminated_base_ir_values(); + //for(int i=0;i<5;i++){ + // reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]); + //} +} + +// Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters +void store_background_base_ir_values ( void ){ + base_ir_values_stored = 1; + for(int i=0;i<5;i++){ + background_base_ir_values [i] = IF_read_IR_adc_value(2,i); + } +} + +// Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse +void store_illuminated_base_ir_values ( void ){ + //1. Enable the base IR emitters and store the values + IF_set_IR_emitter_output(2, 1); + wait_us(base_ir_pulse_delay); + for(int i=0;i<5;i++){ + illuminated_base_ir_values [i] = IF_read_IR_adc_value(2,i); + } + + IF_set_IR_emitter_output(2, 0); +} + +// Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm +void store_line_position ( ){ + // Store background and reflected base IR values + store_base_ir_values(); + int h_value[5]; + int line_threshold = 1000; + int line_threshold_hi = 2000; + char count = 0; + line_found = 0; + line_position = 0; + for(int i=0;i<5;i++){ + if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0; + else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i); + if(h_value[i] < line_threshold) count++; + } + if(count == 1){ + line_found = 1; + if(h_value[0] < line_threshold) { + line_position = -1; + if(h_value[1] < line_threshold_hi) line_position = -0.8; + } + + if (h_value[1] < line_threshold) { + line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);; + } + if(h_value[2] < line_threshold) { + line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]); + } + if(h_value[3] < line_threshold) { + line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);; + } + if(h_value[4] < line_threshold) { + line_position = 1; + if(h_value[3] < line_threshold_hi) line_position = 0.8; + } + } + if(count == 2){ + if(h_value[0] && h_value[1] < line_threshold){ + line_found = 1; + line_position = -0.6; + } + + if(h_value[1] && h_value[2] < line_threshold){ + line_found = 1; + line_position = -0.4; + } + + if(h_value[2] && h_value[3] < line_threshold){ + line_found = 1; + line_position = 0.4; + } + + if(h_value[3] && h_value[4] < line_threshold){ + line_found = 1; + line_position = 0.6; + } + } +} + + + +void calibrate_base_ir_sensors (void) { + short white_background[5]; + short white_active[5]; + short black_background[5]; + short black_active[5]; + 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"); + 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"); + + for(int i=0;i<5;i++){ + wait(0.2); + store_background_base_ir_values(); + + display.set_position(1,9); + display.write_string("."); + wait(0.2); + store_illuminated_base_ir_values(); + for(int k=0;k<5;k++){ + white_background[k]+= get_background_base_ir_value(k); + white_active[k] += get_illuminated_base_ir_value(k); + } + pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1), + get_background_base_ir_value(0), get_illuminated_base_ir_value(0), + get_background_base_ir_value(1), get_illuminated_base_ir_value(1), + get_background_base_ir_value(2), get_illuminated_base_ir_value(2), + get_background_base_ir_value(3), get_illuminated_base_ir_value(3), + get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); + } + for(int k=0;k<5;k++){ + white_background[k]/=5; + white_active[k]/=5; + } + pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", + white_background[0], white_active[0], + white_background[1], white_active[1], + white_background[2], white_active[2], + white_background[3], white_active[3], + white_background[4], white_active[4]); + + display.clear_display(); + display.write_string("Place robot on"); + display.set_position(1,0); + display.write_string("black 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("\nBlack Background Results:\n"); + + for(int i=0;i<5;i++){ + wait(0.2); + + store_background_base_ir_values(); + display.set_position(1,9); + display.write_string("."); + wait(0.2); + store_illuminated_base_ir_values(); + for(int k=0;k<5;k++){ + black_background[k]+= get_background_base_ir_value(k); + black_active[k] += get_illuminated_base_ir_value(k); + } + pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1), + get_background_base_ir_value(0), get_illuminated_base_ir_value(0), + get_background_base_ir_value(1), get_illuminated_base_ir_value(1), + get_background_base_ir_value(2), get_illuminated_base_ir_value(2), + get_background_base_ir_value(3), get_illuminated_base_ir_value(3), + get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); + } + for(int k=0;k<5;k++){ + black_background[k]/=5; + black_active[k]/=5; + } + pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", + black_background[0], black_active[0], + black_background[1], black_active[1], + black_background[2], black_active[2], + black_background[3], black_active[3], + black_background[4], black_active[4]); + +} + + +int get_bearing_from_ir_array (unsigned short * ir_sensor_readings){ + //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]); + + float degrees_per_radian = 57.295779513; + + // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors + float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432}; + float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533}; + + float sin_sum = 0; + float cos_sum = 0; + + for(int i = 0; i < 8; i++) + { + // Use IR sensor reading to weight bearing vector + sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i]; + cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i]; + } + + float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source + bearing *= degrees_per_radian; // Convert to degrees + + //out("Sin sum:%f Cos sum:%f Bearing:%f\n",sin_sum,cos_sum,bearing); + + return (int) bearing; +} +