Psi Swarm Robot / PsiSwarmV7

Library for the PsiSwarm Robot - Version 0.7

Dependents:   PsiSwarm_V7_Blank

Fork of PsiSwarmLibrary by James Hilder

sensors.cpp@5:3cdd1a37cdd7, 2016-10-15 (annotated)

Committer:
jah128
Date:
Sat Oct 15 13:29:37 2016 +0000
Revision:
5:3cdd1a37cdd7
Parent:
2:c6986ee3c7c5
Child:
6:b340a527add9
Initial commit of API Version 7 (fork from version 5)

Who changed what in which revision?

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