Psi Swarm Robot / PsiSwarmV7

Library for the PsiSwarm Robot - Version 0.7

Dependents:   PsiSwarm_V7_Blank

Fork of PsiSwarmLibrary by James Hilder

sensors.cpp@1:060690a934a9, 2016-03-03 (annotated)

Committer:
jah128
Date:
Thu Mar 03 23:21:47 2016 +0000
Revision:
1:060690a934a9
Parent:
0:d6269d17c8cf
Child:
2:c6986ee3c7c5
Updated for web page;

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 0:d6269d17c8cf 8 * PsiSwarm Library Version: 0.4
jah128 0:d6269d17c8cf 9 *
jah128 0:d6269d17c8cf 10 * February 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 0:d6269d17c8cf 398
jah128 0:d6269d17c8cf 399
jah128 1:060690a934a9 400 void calibrate_base_ir_sensors (void)
jah128 1:060690a934a9 401 {
jah128 0:d6269d17c8cf 402 short white_background[5];
jah128 0:d6269d17c8cf 403 short white_active[5];
jah128 0:d6269d17c8cf 404 short black_background[5];
jah128 0:d6269d17c8cf 405 short black_active[5];
jah128 1:060690a934a9 406 for(int k=0; k<5; k++) {
jah128 0:d6269d17c8cf 407 white_background[k]=0;
jah128 0:d6269d17c8cf 408 black_background[k]=0;
jah128 0:d6269d17c8cf 409 white_active[k]=0;
jah128 1:060690a934a9 410 black_active[k]=0;
jah128 1:060690a934a9 411 }
jah128 0:d6269d17c8cf 412 pc.printf("Base IR Calibration\n");
jah128 0:d6269d17c8cf 413 display.clear_display();
jah128 0:d6269d17c8cf 414 display.write_string("Calibrating base");
jah128 0:d6269d17c8cf 415 display.set_position(1,0);
jah128 0:d6269d17c8cf 416 display.write_string("IR sensor");
jah128 0:d6269d17c8cf 417 wait(0.5);
jah128 0:d6269d17c8cf 418 display.clear_display();
jah128 0:d6269d17c8cf 419 display.write_string("Place robot on");
jah128 0:d6269d17c8cf 420 display.set_position(1,0);
jah128 0:d6269d17c8cf 421 display.write_string("white surface");
jah128 0:d6269d17c8cf 422 wait(3);
jah128 0:d6269d17c8cf 423 display.clear_display();
jah128 0:d6269d17c8cf 424 display.write_string("Calibrating base");
jah128 0:d6269d17c8cf 425 display.set_position(1,0);
jah128 0:d6269d17c8cf 426 display.write_string("IR sensor");
jah128 0:d6269d17c8cf 427 wait(0.5);
jah128 0:d6269d17c8cf 428 pc.printf("\nWhite Background Results:\n");
jah128 1:060690a934a9 429
jah128 1:060690a934a9 430 for(int i=0; i<5; i++) {
jah128 1:060690a934a9 431 wait(0.2);
jah128 1:060690a934a9 432 store_background_base_ir_values();
jah128 1:060690a934a9 433
jah128 1:060690a934a9 434 display.set_position(1,9);
jah128 1:060690a934a9 435 display.write_string(".");
jah128 1:060690a934a9 436 wait(0.2);
jah128 1:060690a934a9 437 store_illuminated_base_ir_values();
jah128 1:060690a934a9 438 for(int k=0; k<5; k++) {
jah128 1:060690a934a9 439 white_background[k]+= get_background_base_ir_value(k);
jah128 1:060690a934a9 440 white_active[k] += get_illuminated_base_ir_value(k);
jah128 1:060690a934a9 441 }
jah128 1:060690a934a9 442 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 443 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 1:060690a934a9 444 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 1:060690a934a9 445 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 1:060690a934a9 446 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 1:060690a934a9 447 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 0:d6269d17c8cf 448 }
jah128 1:060690a934a9 449 for(int k=0; k<5; k++) {
jah128 0:d6269d17c8cf 450 white_background[k]/=5;
jah128 0:d6269d17c8cf 451 white_active[k]/=5;
jah128 0:d6269d17c8cf 452 }
jah128 0:d6269d17c8cf 453 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 1:060690a934a9 454 white_background[0], white_active[0],
jah128 1:060690a934a9 455 white_background[1], white_active[1],
jah128 1:060690a934a9 456 white_background[2], white_active[2],
jah128 1:060690a934a9 457 white_background[3], white_active[3],
jah128 1:060690a934a9 458 white_background[4], white_active[4]);
jah128 1:060690a934a9 459
jah128 0:d6269d17c8cf 460 display.clear_display();
jah128 0:d6269d17c8cf 461 display.write_string("Place robot on");
jah128 0:d6269d17c8cf 462 display.set_position(1,0);
jah128 0:d6269d17c8cf 463 display.write_string("black surface");
jah128 1:060690a934a9 464 wait(3);
jah128 1:060690a934a9 465
jah128 0:d6269d17c8cf 466 display.clear_display();
jah128 0:d6269d17c8cf 467 display.write_string("Calibrating base");
jah128 0:d6269d17c8cf 468 display.set_position(1,0);
jah128 0:d6269d17c8cf 469 display.write_string("IR sensor");
jah128 0:d6269d17c8cf 470 wait(0.5);
jah128 0:d6269d17c8cf 471 pc.printf("\nBlack Background Results:\n");
jah128 1:060690a934a9 472
jah128 1:060690a934a9 473 for(int i=0; i<5; i++) {
jah128 1:060690a934a9 474 wait(0.2);
jah128 0:d6269d17c8cf 475
jah128 1:060690a934a9 476 store_background_base_ir_values();
jah128 1:060690a934a9 477 display.set_position(1,9);
jah128 1:060690a934a9 478 display.write_string(".");
jah128 1:060690a934a9 479 wait(0.2);
jah128 1:060690a934a9 480 store_illuminated_base_ir_values();
jah128 1:060690a934a9 481 for(int k=0; k<5; k++) {
jah128 1:060690a934a9 482 black_background[k]+= get_background_base_ir_value(k);
jah128 1:060690a934a9 483 black_active[k] += get_illuminated_base_ir_value(k);
jah128 1:060690a934a9 484 }
jah128 1:060690a934a9 485 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 486 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 1:060690a934a9 487 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 1:060690a934a9 488 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 1:060690a934a9 489 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 1:060690a934a9 490 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 1:060690a934a9 491 }
jah128 1:060690a934a9 492 for(int k=0; k<5; k++) {
jah128 0:d6269d17c8cf 493 black_background[k]/=5;
jah128 0:d6269d17c8cf 494 black_active[k]/=5;
jah128 0:d6269d17c8cf 495 }
jah128 1:060690a934a9 496 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 1:060690a934a9 497 black_background[0], black_active[0],
jah128 1:060690a934a9 498 black_background[1], black_active[1],
jah128 1:060690a934a9 499 black_background[2], black_active[2],
jah128 1:060690a934a9 500 black_background[3], black_active[3],
jah128 1:060690a934a9 501 black_background[4], black_active[4]);
jah128 1:060690a934a9 502
jah128 0:d6269d17c8cf 503 }
jah128 0:d6269d17c8cf 504
jah128 0:d6269d17c8cf 505
jah128 1:060690a934a9 506 int get_bearing_from_ir_array (unsigned short * ir_sensor_readings)
jah128 1:060690a934a9 507 {
jah128 0:d6269d17c8cf 508 //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 509
jah128 0:d6269d17c8cf 510 float degrees_per_radian = 57.295779513;
jah128 1:060690a934a9 511
jah128 0:d6269d17c8cf 512 // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors
jah128 0:d6269d17c8cf 513 float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432};
jah128 0:d6269d17c8cf 514 float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533};
jah128 1:060690a934a9 515
jah128 0:d6269d17c8cf 516 float sin_sum = 0;
jah128 0:d6269d17c8cf 517 float cos_sum = 0;
jah128 1:060690a934a9 518
jah128 1:060690a934a9 519 for(int i = 0; i < 8; i++) {
jah128 0:d6269d17c8cf 520 // Use IR sensor reading to weight bearing vector
jah128 0:d6269d17c8cf 521 sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i];
jah128 0:d6269d17c8cf 522 cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i];
jah128 0:d6269d17c8cf 523 }
jah128 1:060690a934a9 524
jah128 1:060690a934a9 525 float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source
jah128 0:d6269d17c8cf 526 bearing *= degrees_per_radian; // Convert to degrees
jah128 0:d6269d17c8cf 527
jah128 0:d6269d17c8cf 528 //out("Sin sum:%f Cos sum:%f Bearing:%f\n",sin_sum,cos_sum,bearing);
jah128 0:d6269d17c8cf 529
jah128 0:d6269d17c8cf 530 return (int) bearing;
jah128 1:060690a934a9 531 }
jah128 0:d6269d17c8cf 532