![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
PsiSwarm/sensors.cpp
- Committer:
- alanmillard
- Date:
- 2016-01-31
- Revision:
- 27:7eb032772bc2
- Parent:
- 7:ef9ab01b9e26
File content as of revision 27:7eb032772bc2:
/* 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, Homero Elizondo, Jon Timmis * * Sensor bearing code by Alan Millard * * PsiSwarm Library Version: 0.3 * * October 2015 * */ #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; }