C++ Library for the PsiSwarm Robot - Version 0.7
Fork of PsiSwarmV7 by
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?
User | Revision | Line number | New 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 |