James Wilson / Mbed 2 deprecated PsiSwarm-BeaconDemo_Bluetooth

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of PsiSwarm-flockingAddedBluetooth by James Wilson

PsiSwarm/sensors.cpp@6:ff3c66f7372b, 2015-10-22 (annotated)

Committer:
jah128
Date:
Thu Oct 22 00:46:14 2015 +0000
Revision:
6:ff3c66f7372b
Parent:
3:cd048f6e544e
Child:
7:ef9ab01b9e26
Initial version: beacon detection and sync. code, bearing estimation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a5497a2e366 1 /* University of York Robotics Laboratory PsiSwarm Library: Sensor Functions Source File
jah128 0:8a5497a2e366 2 *
jah128 0:8a5497a2e366 3 * File: sensors.cpp
jah128 0:8a5497a2e366 4 *
jah128 6:ff3c66f7372b 5 * (C) Dept. Electronics & Computer Science, University of York
jah128 6:ff3c66f7372b 6 * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 0:8a5497a2e366 7 *
jah128 6:ff3c66f7372b 8 * Sensor bearing code by Alan Millard
jah128 0:8a5497a2e366 9 *
jah128 6:ff3c66f7372b 10 * PsiSwarm Library Version: 0.3
jah128 6:ff3c66f7372b 11 *
jah128 6:ff3c66f7372b 12 * October 2015
jah128 0:8a5497a2e366 13 *
jah128 0:8a5497a2e366 14 */
jah128 0:8a5497a2e366 15
jah128 0:8a5497a2e366 16 #include "psiswarm.h"
jah128 0:8a5497a2e366 17
jah128 0:8a5497a2e366 18 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 19 // Ultrasonic Sensor (SRF02) Functions
jah128 0:8a5497a2e366 20 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 21
jah128 0:8a5497a2e366 22 // The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure().
jah128 0:8a5497a2e366 23 // It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker
jah128 0:8a5497a2e366 24
jah128 0:8a5497a2e366 25 void enable_ultrasonic_ticker(){
jah128 0:8a5497a2e366 26 ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000);
jah128 0:8a5497a2e366 27 }
jah128 0:8a5497a2e366 28
jah128 0:8a5497a2e366 29 void disable_ultrasonic_ticker(){
jah128 0:8a5497a2e366 30 ultrasonic_ticker.detach();
jah128 0:8a5497a2e366 31 }
jah128 0:8a5497a2e366 32
jah128 0:8a5497a2e366 33 void update_ultrasonic_measure(){
jah128 0:8a5497a2e366 34 if(waiting_for_ultrasonic == 0){
jah128 0:8a5497a2e366 35 waiting_for_ultrasonic = 1;
jah128 0:8a5497a2e366 36
jah128 0:8a5497a2e366 37 char command[2];
jah128 0:8a5497a2e366 38 command[0] = 0x00; // Set the command register
jah128 0:8a5497a2e366 39 command[1] = 0x51; // Get result is centimeters
jah128 0:8a5497a2e366 40 primary_i2c.write(ULTRASONIC_ADDRESS, command, 2); // Send the command to start a ranging burst
jah128 0:8a5497a2e366 41
jah128 0:8a5497a2e366 42 ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000);
jah128 0:8a5497a2e366 43 }else{
jah128 0:8a5497a2e366 44 debug("WARNING: Ultrasonic sensor called too frequently.\n");
jah128 0:8a5497a2e366 45 }
jah128 0:8a5497a2e366 46 }
jah128 0:8a5497a2e366 47
jah128 0:8a5497a2e366 48 void IF_read_ultrasonic_measure(){
jah128 0:8a5497a2e366 49 char command[1];
jah128 0:8a5497a2e366 50 char result[2];
jah128 0:8a5497a2e366 51 command[0] = 0x02; // The start address of measure result
jah128 0:8a5497a2e366 52 primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1); // Send address to read a measure
jah128 0:8a5497a2e366 53 primary_i2c.read(ULTRASONIC_ADDRESS, result, 2); // Read two byte of measure
jah128 0:8a5497a2e366 54 ultrasonic_distance = (result[0]<<8)+result[1];
jah128 0:8a5497a2e366 55 ultrasonic_distance_updated = 1;
jah128 0:8a5497a2e366 56 waiting_for_ultrasonic = 0;
jah128 0:8a5497a2e366 57 debug("US:%d cm\n",ultrasonic_distance);
jah128 0:8a5497a2e366 58 }
jah128 0:8a5497a2e366 59
jah128 0:8a5497a2e366 60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 61 // Additional Sensing Functions
jah128 0:8a5497a2e366 62 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 63
jah128 0:8a5497a2e366 64 float get_temperature(){
jah128 0:8a5497a2e366 65 return IF_read_from_temperature_sensor();
jah128 0:8a5497a2e366 66 }
jah128 0:8a5497a2e366 67
jah128 0:8a5497a2e366 68 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 69 // Voltage Sensing Functions
jah128 0:8a5497a2e366 70 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 71
jah128 0:8a5497a2e366 72 float get_battery_voltage (){
jah128 0:8a5497a2e366 73 float raw_value = vin_battery.read();
jah128 0:8a5497a2e366 74 return raw_value * 4.95;
jah128 0:8a5497a2e366 75 }
jah128 0:8a5497a2e366 76
jah128 0:8a5497a2e366 77 float get_current (){
jah128 0:8a5497a2e366 78 // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor
jah128 0:8a5497a2e366 79 // Device gain = 500
jah128 0:8a5497a2e366 80 float raw_value = vin_current.read();
jah128 0:8a5497a2e366 81 return raw_value * 3.3;
jah128 0:8a5497a2e366 82 }
jah128 0:8a5497a2e366 83
jah128 0:8a5497a2e366 84 float get_dc_voltage (){
jah128 0:8a5497a2e366 85 float raw_value = vin_dc.read();
jah128 0:8a5497a2e366 86 return raw_value * 6.6;
jah128 0:8a5497a2e366 87 }
jah128 0:8a5497a2e366 88
jah128 0:8a5497a2e366 89 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 90 // IR Sensor Functions
jah128 0:8a5497a2e366 91 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 92
jah128 0:8a5497a2e366 93 // Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).
jah128 0:8a5497a2e366 94 // The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
jah128 0:8a5497a2e366 95 // NB This function is preserved for code-compatability and cases where only a single reading is needed
jah128 0:8a5497a2e366 96 // In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance()
jah128 0:8a5497a2e366 97 float read_reflected_ir_distance ( char index ){
jah128 0:8a5497a2e366 98 // Sanity check
jah128 0:8a5497a2e366 99 if(index>7) return 0.0;
jah128 0:8a5497a2e366 100
jah128 0:8a5497a2e366 101 //1. Read the ADC value without IR emitter on; store in the background_ir_values[] array
jah128 0:8a5497a2e366 102 background_ir_values [index] = IF_read_IR_adc_value(1, index );
jah128 0:8a5497a2e366 103
jah128 0:8a5497a2e366 104 //2. Enable the relevant IR emitter by turning on its pulse output
jah128 0:8a5497a2e366 105 switch(index){
jah128 0:8a5497a2e366 106 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 107 IF_set_IR_emitter_output(0, 1);
jah128 0:8a5497a2e366 108 break;
jah128 0:8a5497a2e366 109 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 110 IF_set_IR_emitter_output(1, 1);
jah128 0:8a5497a2e366 111 break;
jah128 0:8a5497a2e366 112 }
jah128 0:8a5497a2e366 113 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 114
jah128 0:8a5497a2e366 115 //3. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array
jah128 0:8a5497a2e366 116 illuminated_ir_values [index] = IF_read_IR_adc_value (1, index );
jah128 0:8a5497a2e366 117
jah128 0:8a5497a2e366 118 //4. Turn off IR emitter
jah128 0:8a5497a2e366 119 switch(index){
jah128 0:8a5497a2e366 120 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 121 IF_set_IR_emitter_output(0, 0);
jah128 0:8a5497a2e366 122 break;
jah128 0:8a5497a2e366 123 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 124 IF_set_IR_emitter_output(1, 0);
jah128 0:8a5497a2e366 125 break;
jah128 0:8a5497a2e366 126 }
jah128 0:8a5497a2e366 127
jah128 0:8a5497a2e366 128 //5. Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances()
jah128 0:8a5497a2e366 129 reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]);
jah128 0:8a5497a2e366 130 ir_values_stored = 1;
jah128 0:8a5497a2e366 131 return reflected_ir_distances [index];
jah128 0:8a5497a2e366 132 }
jah128 0:8a5497a2e366 133
jah128 0:8a5497a2e366 134
jah128 0:8a5497a2e366 135 // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances
jah128 0:8a5497a2e366 136 float get_reflected_ir_distance ( char index ){
jah128 0:8a5497a2e366 137 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 138 if (index>7 || ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 139 return reflected_ir_distances[index];
jah128 0:8a5497a2e366 140 }
jah128 0:8a5497a2e366 141
jah128 0:8a5497a2e366 142 // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
jah128 0:8a5497a2e366 143 unsigned short get_background_raw_ir_value ( char index ){
jah128 0:8a5497a2e366 144 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 145 if (index>7 || ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 146 return background_ir_values[index];
jah128 0:8a5497a2e366 147 }
jah128 0:8a5497a2e366 148
jah128 0:8a5497a2e366 149 // Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values
jah128 0:8a5497a2e366 150 unsigned short get_illuminated_raw_ir_value ( char index ){
jah128 0:8a5497a2e366 151 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 152 if (index>7 || ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 153 return illuminated_ir_values[index];
jah128 0:8a5497a2e366 154 }
jah128 0:8a5497a2e366 155
jah128 0:8a5497a2e366 156 // Stores the reflected distances for all 8 IR sensors
jah128 0:8a5497a2e366 157 void store_reflected_ir_distances ( void ){
jah128 0:8a5497a2e366 158 store_ir_values();
jah128 0:8a5497a2e366 159 for(int i=0;i<8;i++){
jah128 0:8a5497a2e366 160 reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
jah128 0:8a5497a2e366 161 }
jah128 0:8a5497a2e366 162 }
jah128 0:8a5497a2e366 163
jah128 0:8a5497a2e366 164 // Stores the rbackground and illuminated values for all 8 IR sensors
jah128 0:8a5497a2e366 165 void store_ir_values ( void ){
jah128 0:8a5497a2e366 166 store_background_raw_ir_values();
jah128 0:8a5497a2e366 167 store_illuminated_raw_ir_values();
jah128 0:8a5497a2e366 168 }
jah128 0:8a5497a2e366 169
jah128 0:8a5497a2e366 170 // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters
jah128 0:8a5497a2e366 171 void store_background_raw_ir_values ( void ){
jah128 0:8a5497a2e366 172 ir_values_stored = 1;
jah128 0:8a5497a2e366 173 for(int i=0;i<8;i++){
jah128 0:8a5497a2e366 174 background_ir_values [i] = IF_read_IR_adc_value(1,i);
jah128 0:8a5497a2e366 175 }
jah128 0:8a5497a2e366 176 }
jah128 0:8a5497a2e366 177
jah128 0:8a5497a2e366 178 // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse
jah128 0:8a5497a2e366 179 void store_illuminated_raw_ir_values ( void ){
jah128 0:8a5497a2e366 180 //1. Enable the front IR emitters and store the values
jah128 0:8a5497a2e366 181 IF_set_IR_emitter_output(0, 1);
jah128 0:8a5497a2e366 182 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 183 illuminated_ir_values [0] = IF_read_IR_adc_value(1,0);
jah128 0:8a5497a2e366 184 illuminated_ir_values [1] = IF_read_IR_adc_value(1,1);
jah128 0:8a5497a2e366 185 illuminated_ir_values [6] = IF_read_IR_adc_value(1,6);
jah128 0:8a5497a2e366 186 illuminated_ir_values [7] = IF_read_IR_adc_value(1,7);
jah128 0:8a5497a2e366 187 IF_set_IR_emitter_output(0, 0);
jah128 0:8a5497a2e366 188
jah128 0:8a5497a2e366 189 //2. Enable the rear+side IR emitters and store the values
jah128 0:8a5497a2e366 190 IF_set_IR_emitter_output(1, 1);
jah128 0:8a5497a2e366 191 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 192 illuminated_ir_values [2] = IF_read_IR_adc_value(1,2);
jah128 0:8a5497a2e366 193 illuminated_ir_values [3] = IF_read_IR_adc_value(1,3);
jah128 0:8a5497a2e366 194 illuminated_ir_values [4] = IF_read_IR_adc_value(1,4);
jah128 0:8a5497a2e366 195 illuminated_ir_values [5] = IF_read_IR_adc_value(1,5);
jah128 0:8a5497a2e366 196 IF_set_IR_emitter_output(1, 0);
jah128 0:8a5497a2e366 197 }
jah128 0:8a5497a2e366 198
jah128 0:8a5497a2e366 199 // Converts a background and illuminated value into a distance
jah128 0:8a5497a2e366 200 float calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ){
jah128 0:8a5497a2e366 201 float approximate_distance = 4000 + background_value - illuminated_value;
jah128 0:8a5497a2e366 202 if(approximate_distance < 0) approximate_distance = 0;
jah128 0:8a5497a2e366 203
jah128 0:8a5497a2e366 204 // Very approximate: root value, divide by 2, approx distance in mm
jah128 0:8a5497a2e366 205 approximate_distance = sqrt (approximate_distance) / 2.0;
jah128 0:8a5497a2e366 206
jah128 0:8a5497a2e366 207 // Then add adjustment value if value >27
jah128 0:8a5497a2e366 208 if(approximate_distance > 27) {
jah128 0:8a5497a2e366 209 float shift = pow(approximate_distance - 27,3);
jah128 0:8a5497a2e366 210 approximate_distance += shift;
jah128 0:8a5497a2e366 211 }
jah128 0:8a5497a2e366 212 if(approximate_distance > 90) approximate_distance = 100.0;
jah128 0:8a5497a2e366 213 return approximate_distance;
jah128 0:8a5497a2e366 214 }
jah128 0:8a5497a2e366 215
jah128 0:8a5497a2e366 216 // Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse
jah128 0:8a5497a2e366 217 unsigned short read_illuminated_raw_ir_value ( char index ){
jah128 0:8a5497a2e366 218 //This function reads the IR strength when illuminated - used for PC system debugging purposes
jah128 0:8a5497a2e366 219 //1. Enable the relevant IR emitter by turning on its pulse output
jah128 0:8a5497a2e366 220 switch(index){
jah128 0:8a5497a2e366 221 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 222 IF_set_IR_emitter_output(0, 1);
jah128 0:8a5497a2e366 223 break;
jah128 0:8a5497a2e366 224 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 225 IF_set_IR_emitter_output(1, 1);
jah128 0:8a5497a2e366 226 break;
jah128 0:8a5497a2e366 227 }
jah128 0:8a5497a2e366 228 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 229 //2. Read the ADC value now IR emitter is on
jah128 0:8a5497a2e366 230 unsigned short strong_value = IF_read_IR_adc_value( 1,index );
jah128 0:8a5497a2e366 231 //3. Turn off IR emitter
jah128 0:8a5497a2e366 232 switch(index){
jah128 0:8a5497a2e366 233 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 234 IF_set_IR_emitter_output(0, 0);
jah128 0:8a5497a2e366 235 break;
jah128 0:8a5497a2e366 236 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 237 IF_set_IR_emitter_output(1, 0);
jah128 0:8a5497a2e366 238 break;
jah128 0:8a5497a2e366 239 }
jah128 0:8a5497a2e366 240 return strong_value;
jah128 0:8a5497a2e366 241 }
jah128 0:8a5497a2e366 242
jah128 0:8a5497a2e366 243 // Base IR sensor functions
jah128 0:8a5497a2e366 244
jah128 0:8a5497a2e366 245
jah128 0:8a5497a2e366 246 // Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values
jah128 0:8a5497a2e366 247 unsigned short get_background_base_ir_value ( char index ){
jah128 0:8a5497a2e366 248 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 249 if (index>4 || base_ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 250 return background_base_ir_values[index];
jah128 0:8a5497a2e366 251 }
jah128 0:8a5497a2e366 252
jah128 0:8a5497a2e366 253 // Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values
jah128 0:8a5497a2e366 254 unsigned short get_illuminated_base_ir_value ( char index ){
jah128 0:8a5497a2e366 255 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 256 if (index>4 || base_ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 257 return illuminated_base_ir_values[index];
jah128 0:8a5497a2e366 258 }
jah128 0:8a5497a2e366 259
jah128 0:8a5497a2e366 260 // Stores the reflected distances for all 5 base IR sensors
jah128 0:8a5497a2e366 261 void store_base_ir_values ( void ){
jah128 0:8a5497a2e366 262 store_background_base_ir_values();
jah128 0:8a5497a2e366 263 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 264 //for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 265 // reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]);
jah128 0:8a5497a2e366 266 //}
jah128 0:8a5497a2e366 267 }
jah128 0:8a5497a2e366 268
jah128 0:8a5497a2e366 269 // Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters
jah128 0:8a5497a2e366 270 void store_background_base_ir_values ( void ){
jah128 0:8a5497a2e366 271 base_ir_values_stored = 1;
jah128 0:8a5497a2e366 272 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 273 background_base_ir_values [i] = IF_read_IR_adc_value(2,i);
jah128 0:8a5497a2e366 274 }
jah128 0:8a5497a2e366 275 }
jah128 0:8a5497a2e366 276
jah128 0:8a5497a2e366 277 // Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse
jah128 0:8a5497a2e366 278 void store_illuminated_base_ir_values ( void ){
jah128 0:8a5497a2e366 279 //1. Enable the base IR emitters and store the values
jah128 0:8a5497a2e366 280 IF_set_IR_emitter_output(2, 1);
jah128 0:8a5497a2e366 281 wait_us(base_ir_pulse_delay);
jah128 0:8a5497a2e366 282 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 283 illuminated_base_ir_values [i] = IF_read_IR_adc_value(2,i);
jah128 0:8a5497a2e366 284 }
jah128 0:8a5497a2e366 285
jah128 0:8a5497a2e366 286 IF_set_IR_emitter_output(2, 0);
jah128 0:8a5497a2e366 287 }
jah128 0:8a5497a2e366 288
jah128 2:a6214fd156ff 289 // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm
jah128 3:cd048f6e544e 290 void store_line_position ( ){
jah128 2:a6214fd156ff 291 // Store background and reflected base IR values
jah128 2:a6214fd156ff 292 store_base_ir_values();
jah128 3:cd048f6e544e 293 int h_value[5];
jah128 3:cd048f6e544e 294 int line_threshold = 1000;
jah128 3:cd048f6e544e 295 int line_threshold_hi = 2000;
jah128 3:cd048f6e544e 296 char count = 0;
jah128 3:cd048f6e544e 297 line_found = 0;
jah128 3:cd048f6e544e 298 line_position = 0;
jah128 3:cd048f6e544e 299 for(int i=0;i<5;i++){
jah128 3:cd048f6e544e 300 if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0;
jah128 3:cd048f6e544e 301 else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i);
jah128 3:cd048f6e544e 302 if(h_value[i] < line_threshold) count++;
jah128 3:cd048f6e544e 303 }
jah128 3:cd048f6e544e 304 if(count == 1){
jah128 3:cd048f6e544e 305 line_found = 1;
jah128 3:cd048f6e544e 306 if(h_value[0] < line_threshold) {
jah128 3:cd048f6e544e 307 line_position = -1;
jah128 3:cd048f6e544e 308 if(h_value[1] < line_threshold_hi) line_position = -0.8;
jah128 3:cd048f6e544e 309 }
jah128 3:cd048f6e544e 310
jah128 3:cd048f6e544e 311 if (h_value[1] < line_threshold) {
jah128 3:cd048f6e544e 312 line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);;
jah128 3:cd048f6e544e 313 }
jah128 3:cd048f6e544e 314 if(h_value[2] < line_threshold) {
jah128 3:cd048f6e544e 315 line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]);
jah128 3:cd048f6e544e 316 }
jah128 3:cd048f6e544e 317 if(h_value[3] < line_threshold) {
jah128 3:cd048f6e544e 318 line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);;
jah128 3:cd048f6e544e 319 }
jah128 3:cd048f6e544e 320 if(h_value[4] < line_threshold) {
jah128 3:cd048f6e544e 321 line_position = 1;
jah128 3:cd048f6e544e 322 if(h_value[3] < line_threshold_hi) line_position = 0.8;
jah128 3:cd048f6e544e 323 }
jah128 3:cd048f6e544e 324 }
jah128 3:cd048f6e544e 325 if(count == 2){
jah128 3:cd048f6e544e 326 if(h_value[0] && h_value[1] < line_threshold){
jah128 3:cd048f6e544e 327 line_found = 1;
jah128 3:cd048f6e544e 328 line_position = -0.6;
jah128 3:cd048f6e544e 329 }
jah128 3:cd048f6e544e 330
jah128 3:cd048f6e544e 331 if(h_value[1] && h_value[2] < line_threshold){
jah128 3:cd048f6e544e 332 line_found = 1;
jah128 3:cd048f6e544e 333 line_position = -0.4;
jah128 3:cd048f6e544e 334 }
jah128 3:cd048f6e544e 335
jah128 3:cd048f6e544e 336 if(h_value[2] && h_value[3] < line_threshold){
jah128 3:cd048f6e544e 337 line_found = 1;
jah128 3:cd048f6e544e 338 line_position = 0.4;
jah128 3:cd048f6e544e 339 }
jah128 3:cd048f6e544e 340
jah128 3:cd048f6e544e 341 if(h_value[3] && h_value[4] < line_threshold){
jah128 3:cd048f6e544e 342 line_found = 1;
jah128 3:cd048f6e544e 343 line_position = 0.6;
jah128 3:cd048f6e544e 344 }
jah128 3:cd048f6e544e 345 }
jah128 2:a6214fd156ff 346 }
jah128 2:a6214fd156ff 347
jah128 2:a6214fd156ff 348
jah128 3:cd048f6e544e 349
jah128 0:8a5497a2e366 350 void calibrate_base_ir_sensors (void) {
jah128 0:8a5497a2e366 351 short white_background[5];
jah128 0:8a5497a2e366 352 short white_active[5];
jah128 0:8a5497a2e366 353 short black_background[5];
jah128 0:8a5497a2e366 354 short black_active[5];
jah128 0:8a5497a2e366 355 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 356 white_background[k]=0;
jah128 0:8a5497a2e366 357 black_background[k]=0;
jah128 0:8a5497a2e366 358 white_active[k]=0;
jah128 0:8a5497a2e366 359 black_active[k]=0;
jah128 0:8a5497a2e366 360 }
jah128 0:8a5497a2e366 361 pc.printf("Base IR Calibration\n");
jah128 0:8a5497a2e366 362 display.clear_display();
jah128 0:8a5497a2e366 363 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 364 display.set_position(1,0);
jah128 0:8a5497a2e366 365 display.write_string("IR sensor");
jah128 0:8a5497a2e366 366 wait(0.5);
jah128 0:8a5497a2e366 367 display.clear_display();
jah128 0:8a5497a2e366 368 display.write_string("Place robot on");
jah128 0:8a5497a2e366 369 display.set_position(1,0);
jah128 0:8a5497a2e366 370 display.write_string("white surface");
jah128 0:8a5497a2e366 371 wait(3);
jah128 0:8a5497a2e366 372 display.clear_display();
jah128 0:8a5497a2e366 373 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 374 display.set_position(1,0);
jah128 0:8a5497a2e366 375 display.write_string("IR sensor");
jah128 0:8a5497a2e366 376 wait(0.5);
jah128 0:8a5497a2e366 377 pc.printf("\nWhite Background Results:\n");
jah128 0:8a5497a2e366 378
jah128 0:8a5497a2e366 379 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 380 wait(0.2);
jah128 0:8a5497a2e366 381 store_background_base_ir_values();
jah128 0:8a5497a2e366 382
jah128 0:8a5497a2e366 383 display.set_position(1,9);
jah128 0:8a5497a2e366 384 display.write_string(".");
jah128 0:8a5497a2e366 385 wait(0.2);
jah128 0:8a5497a2e366 386 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 387 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 388 white_background[k]+= get_background_base_ir_value(k);
jah128 0:8a5497a2e366 389 white_active[k] += get_illuminated_base_ir_value(k);
jah128 0:8a5497a2e366 390 }
jah128 0:8a5497a2e366 391 pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1),
jah128 0:8a5497a2e366 392 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 0:8a5497a2e366 393 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 0:8a5497a2e366 394 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 0:8a5497a2e366 395 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 0:8a5497a2e366 396 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 0:8a5497a2e366 397 }
jah128 0:8a5497a2e366 398 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 399 white_background[k]/=5;
jah128 0:8a5497a2e366 400 white_active[k]/=5;
jah128 0:8a5497a2e366 401 }
jah128 0:8a5497a2e366 402 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 0:8a5497a2e366 403 white_background[0], white_active[0],
jah128 0:8a5497a2e366 404 white_background[1], white_active[1],
jah128 0:8a5497a2e366 405 white_background[2], white_active[2],
jah128 0:8a5497a2e366 406 white_background[3], white_active[3],
jah128 0:8a5497a2e366 407 white_background[4], white_active[4]);
jah128 0:8a5497a2e366 408
jah128 0:8a5497a2e366 409 display.clear_display();
jah128 0:8a5497a2e366 410 display.write_string("Place robot on");
jah128 0:8a5497a2e366 411 display.set_position(1,0);
jah128 0:8a5497a2e366 412 display.write_string("black surface");
jah128 0:8a5497a2e366 413 wait(3);
jah128 0:8a5497a2e366 414
jah128 0:8a5497a2e366 415 display.clear_display();
jah128 0:8a5497a2e366 416 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 417 display.set_position(1,0);
jah128 0:8a5497a2e366 418 display.write_string("IR sensor");
jah128 0:8a5497a2e366 419 wait(0.5);
jah128 0:8a5497a2e366 420 pc.printf("\nBlack Background Results:\n");
jah128 0:8a5497a2e366 421
jah128 0:8a5497a2e366 422 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 423 wait(0.2);
jah128 0:8a5497a2e366 424
jah128 0:8a5497a2e366 425 store_background_base_ir_values();
jah128 0:8a5497a2e366 426 display.set_position(1,9);
jah128 0:8a5497a2e366 427 display.write_string(".");
jah128 0:8a5497a2e366 428 wait(0.2);
jah128 0:8a5497a2e366 429 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 430 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 431 black_background[k]+= get_background_base_ir_value(k);
jah128 0:8a5497a2e366 432 black_active[k] += get_illuminated_base_ir_value(k);
jah128 0:8a5497a2e366 433 }
jah128 0:8a5497a2e366 434 pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1),
jah128 0:8a5497a2e366 435 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 0:8a5497a2e366 436 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 0:8a5497a2e366 437 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 0:8a5497a2e366 438 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 0:8a5497a2e366 439 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 0:8a5497a2e366 440 }
jah128 0:8a5497a2e366 441 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 442 black_background[k]/=5;
jah128 0:8a5497a2e366 443 black_active[k]/=5;
jah128 0:8a5497a2e366 444 }
jah128 0:8a5497a2e366 445 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 0:8a5497a2e366 446 black_background[0], black_active[0],
jah128 0:8a5497a2e366 447 black_background[1], black_active[1],
jah128 0:8a5497a2e366 448 black_background[2], black_active[2],
jah128 0:8a5497a2e366 449 black_background[3], black_active[3],
jah128 0:8a5497a2e366 450 black_background[4], black_active[4]);
jah128 0:8a5497a2e366 451
jah128 6:ff3c66f7372b 452 }
jah128 6:ff3c66f7372b 453
jah128 6:ff3c66f7372b 454
jah128 6:ff3c66f7372b 455 int get_bearing_from_ir_array (unsigned short * ir_sensor_readings){
jah128 6:ff3c66f7372b 456 float degrees_per_radian = 57.295779513;
jah128 6:ff3c66f7372b 457
jah128 6:ff3c66f7372b 458 // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors
jah128 6:ff3c66f7372b 459 float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432};
jah128 6:ff3c66f7372b 460 float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533};
jah128 6:ff3c66f7372b 461
jah128 6:ff3c66f7372b 462 float sin_sum = 0;
jah128 6:ff3c66f7372b 463 float cos_sum = 0;
jah128 6:ff3c66f7372b 464
jah128 6:ff3c66f7372b 465 for(int i = 0; i < 8; i++)
jah128 6:ff3c66f7372b 466 {
jah128 6:ff3c66f7372b 467 // Use IR sensor reading to weight bearing vector
jah128 6:ff3c66f7372b 468 sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i];
jah128 6:ff3c66f7372b 469 cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i];
jah128 6:ff3c66f7372b 470 }
jah128 6:ff3c66f7372b 471
jah128 6:ff3c66f7372b 472 float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source
jah128 6:ff3c66f7372b 473 bearing *= degrees_per_radian; // Convert to degrees
jah128 6:ff3c66f7372b 474
jah128 6:ff3c66f7372b 475 return (int) bearing;
jah128 6:ff3c66f7372b 476 }
jah128 6:ff3c66f7372b 477