Psi Swarm robot library version 0.9
Fork of PsiSwarmV9 by
sensors.cpp@17:bf614e28668f, 2017-06-04 (annotated)
- Committer:
- jah128
- Date:
- Sun Jun 04 13:11:09 2017 +0000
- Revision:
- 17:bf614e28668f
- Parent:
- 16:50686c07ad07
- Child:
- 18:9204f74069b4
Updated calibration menus, fixed some bugs in demo, create new store_line_position routine using calibrated values
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 | 17:bf614e28668f | 2 | * |
jah128 | 16:50686c07ad07 | 3 | * Copyright 2017 University of York |
jah128 | 6:b340a527add9 | 4 | * |
jah128 | 17:bf614e28668f | 5 | * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. |
jah128 | 6:b340a527add9 | 6 | * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 |
jah128 | 6:b340a527add9 | 7 | * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS |
jah128 | 17:bf614e28668f | 8 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
jah128 | 6:b340a527add9 | 9 | * See the License for the specific language governing permissions and limitations under the License. |
jah128 | 1:060690a934a9 | 10 | * |
jah128 | 0:d6269d17c8cf | 11 | * File: sensors.cpp |
jah128 | 0:d6269d17c8cf | 12 | * |
jah128 | 0:d6269d17c8cf | 13 | * (C) Dept. Electronics & Computer Science, University of York |
jah128 | 0:d6269d17c8cf | 14 | * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis |
jah128 | 0:d6269d17c8cf | 15 | * |
jah128 | 16:50686c07ad07 | 16 | * PsiSwarm Library Version: 0.9 |
jah128 | 0:d6269d17c8cf | 17 | * |
jah128 | 16:50686c07ad07 | 18 | * June 2017 |
jah128 | 0:d6269d17c8cf | 19 | * |
jah128 | 0:d6269d17c8cf | 20 | * |
jah128 | 0:d6269d17c8cf | 21 | */ |
jah128 | 0:d6269d17c8cf | 22 | |
jah128 | 0:d6269d17c8cf | 23 | #include "psiswarm.h" |
jah128 | 0:d6269d17c8cf | 24 | |
jah128 | 17:bf614e28668f | 25 | |
jah128 | 17:bf614e28668f | 26 | |
jah128 | 0:d6269d17c8cf | 27 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 28 | // Ultrasonic Sensor (SRF02) Functions |
jah128 | 0:d6269d17c8cf | 29 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 30 | |
jah128 | 0:d6269d17c8cf | 31 | // The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure(). |
jah128 | 0:d6269d17c8cf | 32 | // It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker |
jah128 | 0:d6269d17c8cf | 33 | |
jah128 | 10:e58323951c08 | 34 | void Sensors::enable_ultrasonic_ticker() |
jah128 | 1:060690a934a9 | 35 | { |
jah128 | 10:e58323951c08 | 36 | ultrasonic_ticker.attach_us(this,&Sensors::update_ultrasonic_measure,100000); |
jah128 | 0:d6269d17c8cf | 37 | } |
jah128 | 0:d6269d17c8cf | 38 | |
jah128 | 10:e58323951c08 | 39 | void Sensors::disable_ultrasonic_ticker() |
jah128 | 1:060690a934a9 | 40 | { |
jah128 | 1:060690a934a9 | 41 | ultrasonic_ticker.detach(); |
jah128 | 0:d6269d17c8cf | 42 | } |
jah128 | 0:d6269d17c8cf | 43 | |
jah128 | 10:e58323951c08 | 44 | void Sensors::update_ultrasonic_measure() |
jah128 | 1:060690a934a9 | 45 | { |
jah128 | 1:060690a934a9 | 46 | if(waiting_for_ultrasonic == 0) { |
jah128 | 0:d6269d17c8cf | 47 | waiting_for_ultrasonic = 1; |
jah128 | 1:060690a934a9 | 48 | if(has_ultrasonic_sensor) { |
jah128 | 1:060690a934a9 | 49 | char command[2]; |
jah128 | 1:060690a934a9 | 50 | command[0] = 0x00; // Set the command register |
jah128 | 0:d6269d17c8cf | 51 | command[1] = 0x51; // Get result is centimeters |
jah128 | 1:060690a934a9 | 52 | primary_i2c.write(ULTRASONIC_ADDRESS, command, 2); // Send the command to start a ranging burst |
jah128 | 1:060690a934a9 | 53 | } |
jah128 | 10:e58323951c08 | 54 | ultrasonic_timeout.attach_us(this,&Sensors::IF_read_ultrasonic_measure,70000); |
jah128 | 1:060690a934a9 | 55 | } else { |
jah128 | 12:878c6e9d9e60 | 56 | psi.debug("WARNING: Ultrasonic sensor called too frequently.\n"); |
jah128 | 0:d6269d17c8cf | 57 | } |
jah128 | 0:d6269d17c8cf | 58 | } |
jah128 | 0:d6269d17c8cf | 59 | |
jah128 | 10:e58323951c08 | 60 | void Sensors::IF_read_ultrasonic_measure() |
jah128 | 1:060690a934a9 | 61 | { |
jah128 | 1:060690a934a9 | 62 | if(has_ultrasonic_sensor) { |
jah128 | 1:060690a934a9 | 63 | char command[1]; |
jah128 | 1:060690a934a9 | 64 | char result[2]; |
jah128 | 1:060690a934a9 | 65 | command[0] = 0x02; // The start address of measure result |
jah128 | 1:060690a934a9 | 66 | primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1); // Send address to read a measure |
jah128 | 1:060690a934a9 | 67 | primary_i2c.read(ULTRASONIC_ADDRESS, result, 2); // Read two byte of measure |
jah128 | 1:060690a934a9 | 68 | ultrasonic_distance = (result[0]<<8)+result[1]; |
jah128 | 1:060690a934a9 | 69 | } else ultrasonic_distance = 0; |
jah128 | 0:d6269d17c8cf | 70 | ultrasonic_distance_updated = 1; |
jah128 | 0:d6269d17c8cf | 71 | waiting_for_ultrasonic = 0; |
jah128 | 12:878c6e9d9e60 | 72 | //psi.debug("US:%d cm\n",ultrasonic_distance); |
jah128 | 0:d6269d17c8cf | 73 | } |
jah128 | 0:d6269d17c8cf | 74 | |
jah128 | 0:d6269d17c8cf | 75 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 76 | // Additional Sensing Functions |
jah128 | 0:d6269d17c8cf | 77 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 78 | |
jah128 | 10:e58323951c08 | 79 | float Sensors::get_temperature() |
jah128 | 17:bf614e28668f | 80 | { |
jah128 | 11:312663037b8c | 81 | if(has_temperature_sensor)return i2c_setup.IF_read_from_temperature_sensor(); |
jah128 | 1:060690a934a9 | 82 | return 0; |
jah128 | 0:d6269d17c8cf | 83 | } |
jah128 | 0:d6269d17c8cf | 84 | |
jah128 | 0:d6269d17c8cf | 85 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 86 | // Voltage Sensing Functions |
jah128 | 0:d6269d17c8cf | 87 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 88 | |
jah128 | 10:e58323951c08 | 89 | float Sensors::get_battery_voltage () |
jah128 | 1:060690a934a9 | 90 | { |
jah128 | 0:d6269d17c8cf | 91 | float raw_value = vin_battery.read(); |
jah128 | 0:d6269d17c8cf | 92 | return raw_value * 4.95; |
jah128 | 0:d6269d17c8cf | 93 | } |
jah128 | 0:d6269d17c8cf | 94 | |
jah128 | 10:e58323951c08 | 95 | float Sensors::get_current () |
jah128 | 1:060690a934a9 | 96 | { |
jah128 | 0:d6269d17c8cf | 97 | // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor |
jah128 | 0:d6269d17c8cf | 98 | // Device gain = 500 |
jah128 | 0:d6269d17c8cf | 99 | float raw_value = vin_current.read(); |
jah128 | 0:d6269d17c8cf | 100 | return raw_value * 3.3; |
jah128 | 0:d6269d17c8cf | 101 | } |
jah128 | 0:d6269d17c8cf | 102 | |
jah128 | 10:e58323951c08 | 103 | float Sensors::get_dc_voltage () |
jah128 | 1:060690a934a9 | 104 | { |
jah128 | 0:d6269d17c8cf | 105 | float raw_value = vin_dc.read(); |
jah128 | 1:060690a934a9 | 106 | return raw_value * 6.6; |
jah128 | 0:d6269d17c8cf | 107 | } |
jah128 | 0:d6269d17c8cf | 108 | |
jah128 | 0:d6269d17c8cf | 109 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 110 | // IR Sensor Functions |
jah128 | 0:d6269d17c8cf | 111 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:d6269d17c8cf | 112 | |
jah128 | 17:bf614e28668f | 113 | int cv_bir_w[5],cv_bir_b[5]; |
jah128 | 17:bf614e28668f | 114 | |
jah128 | 1:060690a934a9 | 115 | // Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7). |
jah128 | 0:d6269d17c8cf | 116 | // The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found. |
jah128 | 0:d6269d17c8cf | 117 | // NB This function is preserved for code-compatability and cases where only a single reading is needed |
jah128 | 0:d6269d17c8cf | 118 | // 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 | 10:e58323951c08 | 119 | float Sensors::read_reflected_ir_distance ( char index ) |
jah128 | 1:060690a934a9 | 120 | { |
jah128 | 0:d6269d17c8cf | 121 | // Sanity check |
jah128 | 0:d6269d17c8cf | 122 | if(index>7) return 0.0; |
jah128 | 1:060690a934a9 | 123 | |
jah128 | 0:d6269d17c8cf | 124 | //1. Read the ADC value without IR emitter on; store in the background_ir_values[] array |
jah128 | 11:312663037b8c | 125 | background_ir_values [index] = i2c_setup.IF_read_IR_adc_value(1, index ); |
jah128 | 1:060690a934a9 | 126 | |
jah128 | 0:d6269d17c8cf | 127 | //2. Enable the relevant IR emitter by turning on its pulse output |
jah128 | 1:060690a934a9 | 128 | switch(index) { |
jah128 | 1:060690a934a9 | 129 | case 0: |
jah128 | 1:060690a934a9 | 130 | case 1: |
jah128 | 1:060690a934a9 | 131 | case 6: |
jah128 | 1:060690a934a9 | 132 | case 7: |
jah128 | 11:312663037b8c | 133 | i2c_setup.IF_set_IR_emitter_output(0, 1); |
jah128 | 1:060690a934a9 | 134 | break; |
jah128 | 1:060690a934a9 | 135 | case 2: |
jah128 | 1:060690a934a9 | 136 | case 3: |
jah128 | 1:060690a934a9 | 137 | case 4: |
jah128 | 1:060690a934a9 | 138 | case 5: |
jah128 | 11:312663037b8c | 139 | i2c_setup.IF_set_IR_emitter_output(1, 1); |
jah128 | 1:060690a934a9 | 140 | break; |
jah128 | 0:d6269d17c8cf | 141 | } |
jah128 | 1:060690a934a9 | 142 | wait_us(ir_pulse_delay); |
jah128 | 1:060690a934a9 | 143 | |
jah128 | 0:d6269d17c8cf | 144 | //3. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array |
jah128 | 11:312663037b8c | 145 | illuminated_ir_values [index] = i2c_setup.IF_read_IR_adc_value (1, index ); |
jah128 | 1:060690a934a9 | 146 | |
jah128 | 0:d6269d17c8cf | 147 | //4. Turn off IR emitter |
jah128 | 1:060690a934a9 | 148 | switch(index) { |
jah128 | 1:060690a934a9 | 149 | case 0: |
jah128 | 1:060690a934a9 | 150 | case 1: |
jah128 | 1:060690a934a9 | 151 | case 6: |
jah128 | 1:060690a934a9 | 152 | case 7: |
jah128 | 11:312663037b8c | 153 | i2c_setup.IF_set_IR_emitter_output(0, 0); |
jah128 | 1:060690a934a9 | 154 | break; |
jah128 | 1:060690a934a9 | 155 | case 2: |
jah128 | 1:060690a934a9 | 156 | case 3: |
jah128 | 1:060690a934a9 | 157 | case 4: |
jah128 | 1:060690a934a9 | 158 | case 5: |
jah128 | 11:312663037b8c | 159 | i2c_setup.IF_set_IR_emitter_output(1, 0); |
jah128 | 1:060690a934a9 | 160 | break; |
jah128 | 0:d6269d17c8cf | 161 | } |
jah128 | 1:060690a934a9 | 162 | |
jah128 | 0:d6269d17c8cf | 163 | //5. Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances() |
jah128 | 0:d6269d17c8cf | 164 | reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]); |
jah128 | 0:d6269d17c8cf | 165 | ir_values_stored = 1; |
jah128 | 0:d6269d17c8cf | 166 | return reflected_ir_distances [index]; |
jah128 | 0:d6269d17c8cf | 167 | } |
jah128 | 0:d6269d17c8cf | 168 | |
jah128 | 0:d6269d17c8cf | 169 | |
jah128 | 0:d6269d17c8cf | 170 | // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances |
jah128 | 10:e58323951c08 | 171 | float Sensors::get_reflected_ir_distance ( char index ) |
jah128 | 1:060690a934a9 | 172 | { |
jah128 | 0:d6269d17c8cf | 173 | // Sanity check: check range of index and that values have been stored |
jah128 | 0:d6269d17c8cf | 174 | if (index>7 || ir_values_stored == 0) return 0.0; |
jah128 | 1:060690a934a9 | 175 | return reflected_ir_distances[index]; |
jah128 | 0:d6269d17c8cf | 176 | } |
jah128 | 0:d6269d17c8cf | 177 | |
jah128 | 0:d6269d17c8cf | 178 | // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values |
jah128 | 10:e58323951c08 | 179 | unsigned short Sensors::get_background_raw_ir_value ( char index ) |
jah128 | 1:060690a934a9 | 180 | { |
jah128 | 0:d6269d17c8cf | 181 | // Sanity check: check range of index and that values have been stored |
jah128 | 0:d6269d17c8cf | 182 | if (index>7 || ir_values_stored == 0) return 0.0; |
jah128 | 1:060690a934a9 | 183 | return background_ir_values[index]; |
jah128 | 0:d6269d17c8cf | 184 | } |
jah128 | 1:060690a934a9 | 185 | |
jah128 | 1:060690a934a9 | 186 | // Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values |
jah128 | 10:e58323951c08 | 187 | unsigned short Sensors::get_illuminated_raw_ir_value ( char index ) |
jah128 | 1:060690a934a9 | 188 | { |
jah128 | 1:060690a934a9 | 189 | // Sanity check: check range of index and that values have been stored |
jah128 | 1:060690a934a9 | 190 | if (index>7 || ir_values_stored == 0) return 0.0; |
jah128 | 1:060690a934a9 | 191 | return illuminated_ir_values[index]; |
jah128 | 1:060690a934a9 | 192 | } |
jah128 | 1:060690a934a9 | 193 | |
jah128 | 0:d6269d17c8cf | 194 | // Stores the reflected distances for all 8 IR sensors |
jah128 | 10:e58323951c08 | 195 | void Sensors::store_reflected_ir_distances ( void ) |
jah128 | 1:060690a934a9 | 196 | { |
jah128 | 0:d6269d17c8cf | 197 | store_ir_values(); |
jah128 | 1:060690a934a9 | 198 | for(int i=0; i<8; i++) { |
jah128 | 1:060690a934a9 | 199 | reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]); |
jah128 | 1:060690a934a9 | 200 | } |
jah128 | 0:d6269d17c8cf | 201 | } |
jah128 | 0:d6269d17c8cf | 202 | |
jah128 | 10:e58323951c08 | 203 | // Stores the background and illuminated values for all 8 IR sensors |
jah128 | 10:e58323951c08 | 204 | void Sensors::store_ir_values ( void ) |
jah128 | 1:060690a934a9 | 205 | { |
jah128 | 0:d6269d17c8cf | 206 | store_background_raw_ir_values(); |
jah128 | 0:d6269d17c8cf | 207 | store_illuminated_raw_ir_values(); |
jah128 | 0:d6269d17c8cf | 208 | } |
jah128 | 1:060690a934a9 | 209 | |
jah128 | 0:d6269d17c8cf | 210 | // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters |
jah128 | 10:e58323951c08 | 211 | void Sensors::store_background_raw_ir_values ( void ) |
jah128 | 1:060690a934a9 | 212 | { |
jah128 | 0:d6269d17c8cf | 213 | ir_values_stored = 1; |
jah128 | 1:060690a934a9 | 214 | for(int i=0; i<8; i++) { |
jah128 | 11:312663037b8c | 215 | background_ir_values [i] = i2c_setup.IF_read_IR_adc_value(1,i); |
jah128 | 0:d6269d17c8cf | 216 | } |
jah128 | 0:d6269d17c8cf | 217 | } |
jah128 | 1:060690a934a9 | 218 | |
jah128 | 0:d6269d17c8cf | 219 | // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse |
jah128 | 10:e58323951c08 | 220 | void Sensors::store_illuminated_raw_ir_values ( void ) |
jah128 | 1:060690a934a9 | 221 | { |
jah128 | 1:060690a934a9 | 222 | //1. Enable the front IR emitters and store the values |
jah128 | 11:312663037b8c | 223 | i2c_setup.IF_set_IR_emitter_output(0, 1); |
jah128 | 1:060690a934a9 | 224 | wait_us(ir_pulse_delay); |
jah128 | 11:312663037b8c | 225 | illuminated_ir_values [0] = i2c_setup.IF_read_IR_adc_value(1,0); |
jah128 | 11:312663037b8c | 226 | illuminated_ir_values [1] = i2c_setup.IF_read_IR_adc_value(1,1); |
jah128 | 11:312663037b8c | 227 | illuminated_ir_values [6] = i2c_setup.IF_read_IR_adc_value(1,6); |
jah128 | 11:312663037b8c | 228 | illuminated_ir_values [7] = i2c_setup.IF_read_IR_adc_value(1,7); |
jah128 | 11:312663037b8c | 229 | i2c_setup.IF_set_IR_emitter_output(0, 0); |
jah128 | 1:060690a934a9 | 230 | |
jah128 | 1:060690a934a9 | 231 | //2. Enable the rear+side IR emitters and store the values |
jah128 | 11:312663037b8c | 232 | i2c_setup.IF_set_IR_emitter_output(1, 1); |
jah128 | 1:060690a934a9 | 233 | wait_us(ir_pulse_delay); |
jah128 | 11:312663037b8c | 234 | illuminated_ir_values [2] = i2c_setup.IF_read_IR_adc_value(1,2); |
jah128 | 11:312663037b8c | 235 | illuminated_ir_values [3] = i2c_setup.IF_read_IR_adc_value(1,3); |
jah128 | 11:312663037b8c | 236 | illuminated_ir_values [4] = i2c_setup.IF_read_IR_adc_value(1,4); |
jah128 | 11:312663037b8c | 237 | illuminated_ir_values [5] = i2c_setup.IF_read_IR_adc_value(1,5); |
jah128 | 11:312663037b8c | 238 | i2c_setup.IF_set_IR_emitter_output(1, 0); |
jah128 | 0:d6269d17c8cf | 239 | } |
jah128 | 1:060690a934a9 | 240 | |
jah128 | 0:d6269d17c8cf | 241 | // Converts a background and illuminated value into a distance |
jah128 | 10:e58323951c08 | 242 | float Sensors::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ) |
jah128 | 1:060690a934a9 | 243 | { |
jah128 | 0:d6269d17c8cf | 244 | float approximate_distance = 4000 + background_value - illuminated_value; |
jah128 | 0:d6269d17c8cf | 245 | if(approximate_distance < 0) approximate_distance = 0; |
jah128 | 1:060690a934a9 | 246 | |
jah128 | 0:d6269d17c8cf | 247 | // Very approximate: root value, divide by 2, approx distance in mm |
jah128 | 0:d6269d17c8cf | 248 | approximate_distance = sqrt (approximate_distance) / 2.0; |
jah128 | 1:060690a934a9 | 249 | |
jah128 | 0:d6269d17c8cf | 250 | // Then add adjustment value if value >27 |
jah128 | 0:d6269d17c8cf | 251 | if(approximate_distance > 27) { |
jah128 | 0:d6269d17c8cf | 252 | float shift = pow(approximate_distance - 27,3); |
jah128 | 0:d6269d17c8cf | 253 | approximate_distance += shift; |
jah128 | 0:d6269d17c8cf | 254 | } |
jah128 | 0:d6269d17c8cf | 255 | if(approximate_distance > 90) approximate_distance = 100.0; |
jah128 | 1:060690a934a9 | 256 | return approximate_distance; |
jah128 | 0:d6269d17c8cf | 257 | } |
jah128 | 0:d6269d17c8cf | 258 | |
jah128 | 0:d6269d17c8cf | 259 | // 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 | 10:e58323951c08 | 260 | unsigned short Sensors::read_illuminated_raw_ir_value ( char index ) |
jah128 | 1:060690a934a9 | 261 | { |
jah128 | 0:d6269d17c8cf | 262 | //This function reads the IR strength when illuminated - used for PC system debugging purposes |
jah128 | 0:d6269d17c8cf | 263 | //1. Enable the relevant IR emitter by turning on its pulse output |
jah128 | 1:060690a934a9 | 264 | switch(index) { |
jah128 | 1:060690a934a9 | 265 | case 0: |
jah128 | 1:060690a934a9 | 266 | case 1: |
jah128 | 1:060690a934a9 | 267 | case 6: |
jah128 | 1:060690a934a9 | 268 | case 7: |
jah128 | 11:312663037b8c | 269 | i2c_setup.IF_set_IR_emitter_output(0, 1); |
jah128 | 1:060690a934a9 | 270 | break; |
jah128 | 1:060690a934a9 | 271 | case 2: |
jah128 | 1:060690a934a9 | 272 | case 3: |
jah128 | 1:060690a934a9 | 273 | case 4: |
jah128 | 1:060690a934a9 | 274 | case 5: |
jah128 | 11:312663037b8c | 275 | i2c_setup.IF_set_IR_emitter_output(1, 1); |
jah128 | 1:060690a934a9 | 276 | break; |
jah128 | 0:d6269d17c8cf | 277 | } |
jah128 | 1:060690a934a9 | 278 | wait_us(ir_pulse_delay); |
jah128 | 0:d6269d17c8cf | 279 | //2. Read the ADC value now IR emitter is on |
jah128 | 11:312663037b8c | 280 | unsigned short strong_value = i2c_setup.IF_read_IR_adc_value( 1,index ); |
jah128 | 0:d6269d17c8cf | 281 | //3. Turn off IR emitter |
jah128 | 1:060690a934a9 | 282 | switch(index) { |
jah128 | 1:060690a934a9 | 283 | case 0: |
jah128 | 1:060690a934a9 | 284 | case 1: |
jah128 | 1:060690a934a9 | 285 | case 6: |
jah128 | 1:060690a934a9 | 286 | case 7: |
jah128 | 11:312663037b8c | 287 | i2c_setup.IF_set_IR_emitter_output(0, 0); |
jah128 | 1:060690a934a9 | 288 | break; |
jah128 | 1:060690a934a9 | 289 | case 2: |
jah128 | 1:060690a934a9 | 290 | case 3: |
jah128 | 1:060690a934a9 | 291 | case 4: |
jah128 | 1:060690a934a9 | 292 | case 5: |
jah128 | 11:312663037b8c | 293 | i2c_setup.IF_set_IR_emitter_output(1, 0); |
jah128 | 1:060690a934a9 | 294 | break; |
jah128 | 0:d6269d17c8cf | 295 | } |
jah128 | 0:d6269d17c8cf | 296 | return strong_value; |
jah128 | 0:d6269d17c8cf | 297 | } |
jah128 | 0:d6269d17c8cf | 298 | |
jah128 | 0:d6269d17c8cf | 299 | // Base IR sensor functions |
jah128 | 0:d6269d17c8cf | 300 | |
jah128 | 0:d6269d17c8cf | 301 | |
jah128 | 0:d6269d17c8cf | 302 | // Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values |
jah128 | 10:e58323951c08 | 303 | unsigned short Sensors::get_background_base_ir_value ( char index ) |
jah128 | 1:060690a934a9 | 304 | { |
jah128 | 0:d6269d17c8cf | 305 | // Sanity check: check range of index and that values have been stored |
jah128 | 0:d6269d17c8cf | 306 | if (index>4 || base_ir_values_stored == 0) return 0.0; |
jah128 | 1:060690a934a9 | 307 | return background_base_ir_values[index]; |
jah128 | 0:d6269d17c8cf | 308 | } |
jah128 | 1:060690a934a9 | 309 | |
jah128 | 0:d6269d17c8cf | 310 | // Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values |
jah128 | 10:e58323951c08 | 311 | unsigned short Sensors::get_illuminated_base_ir_value ( char index ) |
jah128 | 1:060690a934a9 | 312 | { |
jah128 | 0:d6269d17c8cf | 313 | // Sanity check: check range of index and that values have been stored |
jah128 | 0:d6269d17c8cf | 314 | if (index>4 || base_ir_values_stored == 0) return 0.0; |
jah128 | 1:060690a934a9 | 315 | return illuminated_base_ir_values[index]; |
jah128 | 0:d6269d17c8cf | 316 | } |
jah128 | 1:060690a934a9 | 317 | |
jah128 | 0:d6269d17c8cf | 318 | // Stores the reflected distances for all 5 base IR sensors |
jah128 | 10:e58323951c08 | 319 | void Sensors::store_base_ir_values ( void ) |
jah128 | 1:060690a934a9 | 320 | { |
jah128 | 0:d6269d17c8cf | 321 | store_background_base_ir_values(); |
jah128 | 0:d6269d17c8cf | 322 | store_illuminated_base_ir_values(); |
jah128 | 0:d6269d17c8cf | 323 | //for(int i=0;i<5;i++){ |
jah128 | 1:060690a934a9 | 324 | // reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]); |
jah128 | 1:060690a934a9 | 325 | //} |
jah128 | 0:d6269d17c8cf | 326 | } |
jah128 | 1:060690a934a9 | 327 | |
jah128 | 0:d6269d17c8cf | 328 | // Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters |
jah128 | 10:e58323951c08 | 329 | void Sensors::store_background_base_ir_values ( void ) |
jah128 | 1:060690a934a9 | 330 | { |
jah128 | 0:d6269d17c8cf | 331 | base_ir_values_stored = 1; |
jah128 | 1:060690a934a9 | 332 | for(int i=0; i<5; i++) { |
jah128 | 11:312663037b8c | 333 | background_base_ir_values [i] = i2c_setup.IF_read_IR_adc_value(2,i); |
jah128 | 0:d6269d17c8cf | 334 | } |
jah128 | 0:d6269d17c8cf | 335 | } |
jah128 | 1:060690a934a9 | 336 | |
jah128 | 0:d6269d17c8cf | 337 | // Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse |
jah128 | 10:e58323951c08 | 338 | void Sensors::store_illuminated_base_ir_values ( void ) |
jah128 | 1:060690a934a9 | 339 | { |
jah128 | 1:060690a934a9 | 340 | //1. Enable the base IR emitters and store the values |
jah128 | 11:312663037b8c | 341 | i2c_setup.IF_set_IR_emitter_output(2, 1); |
jah128 | 1:060690a934a9 | 342 | wait_us(base_ir_pulse_delay); |
jah128 | 1:060690a934a9 | 343 | for(int i=0; i<5; i++) { |
jah128 | 11:312663037b8c | 344 | illuminated_base_ir_values [i] = i2c_setup.IF_read_IR_adc_value(2,i); |
jah128 | 0:d6269d17c8cf | 345 | } |
jah128 | 1:060690a934a9 | 346 | |
jah128 | 11:312663037b8c | 347 | i2c_setup.IF_set_IR_emitter_output(2, 0); |
jah128 | 0:d6269d17c8cf | 348 | } |
jah128 | 1:060690a934a9 | 349 | |
jah128 | 0:d6269d17c8cf | 350 | // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm |
jah128 | 17:bf614e28668f | 351 | // Old version (doesn't use calibration values) |
jah128 | 17:bf614e28668f | 352 | void Sensors::store_line_position_old ( ) |
jah128 | 1:060690a934a9 | 353 | { |
jah128 | 0:d6269d17c8cf | 354 | // Store background and reflected base IR values |
jah128 | 0:d6269d17c8cf | 355 | store_base_ir_values(); |
jah128 | 0:d6269d17c8cf | 356 | int h_value[5]; |
jah128 | 0:d6269d17c8cf | 357 | int line_threshold = 1000; |
jah128 | 0:d6269d17c8cf | 358 | int line_threshold_hi = 2000; |
jah128 | 0:d6269d17c8cf | 359 | char count = 0; |
jah128 | 0:d6269d17c8cf | 360 | line_found = 0; |
jah128 | 0:d6269d17c8cf | 361 | line_position = 0; |
jah128 | 1:060690a934a9 | 362 | for(int i=0; i<5; i++) { |
jah128 | 0:d6269d17c8cf | 363 | if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0; |
jah128 | 0:d6269d17c8cf | 364 | else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i); |
jah128 | 0:d6269d17c8cf | 365 | if(h_value[i] < line_threshold) count++; |
jah128 | 1:060690a934a9 | 366 | } |
jah128 | 1:060690a934a9 | 367 | if(count == 1) { |
jah128 | 1:060690a934a9 | 368 | line_found = 1; |
jah128 | 1:060690a934a9 | 369 | if(h_value[0] < line_threshold) { |
jah128 | 1:060690a934a9 | 370 | line_position = -1; |
jah128 | 1:060690a934a9 | 371 | if(h_value[1] < line_threshold_hi) line_position = -0.8; |
jah128 | 1:060690a934a9 | 372 | } |
jah128 | 1:060690a934a9 | 373 | |
jah128 | 1:060690a934a9 | 374 | if (h_value[1] < line_threshold) { |
jah128 | 1:060690a934a9 | 375 | line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);; |
jah128 | 1:060690a934a9 | 376 | } |
jah128 | 1:060690a934a9 | 377 | if(h_value[2] < line_threshold) { |
jah128 | 0:d6269d17c8cf | 378 | line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]); |
jah128 | 1:060690a934a9 | 379 | } |
jah128 | 1:060690a934a9 | 380 | if(h_value[3] < line_threshold) { |
jah128 | 1:060690a934a9 | 381 | line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);; |
jah128 | 1:060690a934a9 | 382 | } |
jah128 | 1:060690a934a9 | 383 | if(h_value[4] < line_threshold) { |
jah128 | 1:060690a934a9 | 384 | line_position = 1; |
jah128 | 1:060690a934a9 | 385 | if(h_value[3] < line_threshold_hi) line_position = 0.8; |
jah128 | 1:060690a934a9 | 386 | } |
jah128 | 0:d6269d17c8cf | 387 | } |
jah128 | 1:060690a934a9 | 388 | if(count == 2) { |
jah128 | 1:060690a934a9 | 389 | if(h_value[0] && h_value[1] < line_threshold) { |
jah128 | 0:d6269d17c8cf | 390 | line_found = 1; |
jah128 | 0:d6269d17c8cf | 391 | line_position = -0.6; |
jah128 | 0:d6269d17c8cf | 392 | } |
jah128 | 1:060690a934a9 | 393 | |
jah128 | 1:060690a934a9 | 394 | if(h_value[1] && h_value[2] < line_threshold) { |
jah128 | 0:d6269d17c8cf | 395 | line_found = 1; |
jah128 | 0:d6269d17c8cf | 396 | line_position = -0.4; |
jah128 | 0:d6269d17c8cf | 397 | } |
jah128 | 1:060690a934a9 | 398 | |
jah128 | 1:060690a934a9 | 399 | if(h_value[2] && h_value[3] < line_threshold) { |
jah128 | 0:d6269d17c8cf | 400 | line_found = 1; |
jah128 | 0:d6269d17c8cf | 401 | line_position = 0.4; |
jah128 | 0:d6269d17c8cf | 402 | } |
jah128 | 1:060690a934a9 | 403 | |
jah128 | 1:060690a934a9 | 404 | if(h_value[3] && h_value[4] < line_threshold) { |
jah128 | 0:d6269d17c8cf | 405 | line_found = 1; |
jah128 | 0:d6269d17c8cf | 406 | line_position = 0.6; |
jah128 | 0:d6269d17c8cf | 407 | } |
jah128 | 0:d6269d17c8cf | 408 | } |
jah128 | 0:d6269d17c8cf | 409 | } |
jah128 | 0:d6269d17c8cf | 410 | |
jah128 | 17:bf614e28668f | 411 | // New version (uses calibration values) |
jah128 | 17:bf614e28668f | 412 | void Sensors::store_line_position (void) |
jah128 | 17:bf614e28668f | 413 | { |
jah128 | 17:bf614e28668f | 414 | store_line_position(0.5); |
jah128 | 17:bf614e28668f | 415 | } |
jah128 | 17:bf614e28668f | 416 | |
jah128 | 17:bf614e28668f | 417 | // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm |
jah128 | 17:bf614e28668f | 418 | void Sensors::store_line_position (float line_threshold) |
jah128 | 17:bf614e28668f | 419 | { |
jah128 | 17:bf614e28668f | 420 | // Store background and reflected base IR values |
jah128 | 17:bf614e28668f | 421 | store_base_ir_values(); |
jah128 | 17:bf614e28668f | 422 | float calibrated_values[5]; |
jah128 | 17:bf614e28668f | 423 | float adjust_values[5]; |
jah128 | 17:bf614e28668f | 424 | char count = 0; |
jah128 | 17:bf614e28668f | 425 | for(int i=0; i<5; i++) { |
jah128 | 17:bf614e28668f | 426 | calibrated_values[i] = get_calibrated_base_ir_value(i); |
jah128 | 17:bf614e28668f | 427 | if(calibrated_values[i] < line_threshold) count ++; |
jah128 | 17:bf614e28668f | 428 | adjust_values[i] = 1 - calibrated_values[i]; |
jah128 | 17:bf614e28668f | 429 | adjust_values[i] *= adjust_values[i]; |
jah128 | 17:bf614e28668f | 430 | adjust_values[i] *= 0.5f; |
jah128 | 17:bf614e28668f | 431 | |
jah128 | 17:bf614e28668f | 432 | } |
jah128 | 17:bf614e28668f | 433 | |
jah128 | 17:bf614e28668f | 434 | line_found = 0; |
jah128 | 17:bf614e28668f | 435 | line_position = 0; |
jah128 | 17:bf614e28668f | 436 | |
jah128 | 17:bf614e28668f | 437 | if(count == 1) { |
jah128 | 17:bf614e28668f | 438 | line_found = 1; |
jah128 | 17:bf614e28668f | 439 | if(calibrated_values[0] < line_threshold) { |
jah128 | 17:bf614e28668f | 440 | line_position = -1 + adjust_values[1]; |
jah128 | 17:bf614e28668f | 441 | } |
jah128 | 17:bf614e28668f | 442 | |
jah128 | 17:bf614e28668f | 443 | if (calibrated_values[1] < line_threshold) { |
jah128 | 17:bf614e28668f | 444 | line_position = -0.5 - adjust_values[0] + adjust_values[2]; |
jah128 | 17:bf614e28668f | 445 | } |
jah128 | 17:bf614e28668f | 446 | if(calibrated_values[2] < line_threshold) { |
jah128 | 17:bf614e28668f | 447 | line_position = 0 - adjust_values[1] + adjust_values[3]; |
jah128 | 17:bf614e28668f | 448 | } |
jah128 | 17:bf614e28668f | 449 | if(calibrated_values[3] < line_threshold) { |
jah128 | 17:bf614e28668f | 450 | line_position = 0.5 - adjust_values[2] + adjust_values[4]; |
jah128 | 17:bf614e28668f | 451 | } |
jah128 | 17:bf614e28668f | 452 | if(calibrated_values[4] < line_threshold) { |
jah128 | 17:bf614e28668f | 453 | line_position = 1 - adjust_values[3]; |
jah128 | 17:bf614e28668f | 454 | } |
jah128 | 17:bf614e28668f | 455 | } |
jah128 | 17:bf614e28668f | 456 | |
jah128 | 17:bf614e28668f | 457 | if(count == 2) { |
jah128 | 17:bf614e28668f | 458 | if(calibrated_values[0] < line_threshold && calibrated_values[1] < line_threshold) { |
jah128 | 17:bf614e28668f | 459 | line_found = 1; |
jah128 | 17:bf614e28668f | 460 | line_position = -0.75 - adjust_values[0] + adjust_values[1]; |
jah128 | 17:bf614e28668f | 461 | } |
jah128 | 17:bf614e28668f | 462 | |
jah128 | 17:bf614e28668f | 463 | if(calibrated_values[1] < line_threshold && calibrated_values[2] < line_threshold) { |
jah128 | 17:bf614e28668f | 464 | line_found = 1; |
jah128 | 17:bf614e28668f | 465 | line_position = -0.25 -adjust_values[1] + adjust_values[2]; |
jah128 | 17:bf614e28668f | 466 | } |
jah128 | 17:bf614e28668f | 467 | |
jah128 | 17:bf614e28668f | 468 | if(calibrated_values[2] < line_threshold && calibrated_values[3] < line_threshold) { |
jah128 | 17:bf614e28668f | 469 | line_found = 1; |
jah128 | 17:bf614e28668f | 470 | line_position = 0.25 - adjust_values[2] + adjust_values[3]; |
jah128 | 17:bf614e28668f | 471 | } |
jah128 | 17:bf614e28668f | 472 | |
jah128 | 17:bf614e28668f | 473 | if(calibrated_values[3] < line_threshold && calibrated_values[4] < line_threshold) { |
jah128 | 17:bf614e28668f | 474 | line_found = 1; |
jah128 | 17:bf614e28668f | 475 | line_position = 0.75 - adjust_values[3] + adjust_values[4]; |
jah128 | 17:bf614e28668f | 476 | } |
jah128 | 17:bf614e28668f | 477 | } |
jah128 | 17:bf614e28668f | 478 | |
jah128 | 17:bf614e28668f | 479 | if(count == 3) { |
jah128 | 17:bf614e28668f | 480 | if (calibrated_values[1] < line_threshold && calibrated_values[2] < line_threshold && calibrated_values[3] < line_threshold) { |
jah128 | 17:bf614e28668f | 481 | line_found = 1; |
jah128 | 17:bf614e28668f | 482 | line_position = 0 - adjust_values[1] + adjust_values[3]; |
jah128 | 17:bf614e28668f | 483 | } |
jah128 | 17:bf614e28668f | 484 | } |
jah128 | 17:bf614e28668f | 485 | if(line_position < -1) line_position = -1; |
jah128 | 17:bf614e28668f | 486 | if(line_position > 1) line_position = 1; |
jah128 | 17:bf614e28668f | 487 | } |
jah128 | 17:bf614e28668f | 488 | |
jah128 | 2:c6986ee3c7c5 | 489 | // Returns the subtraction of the background base IR value from the reflection based on last call of store_illuminated_base_ir_values |
jah128 | 17:bf614e28668f | 490 | unsigned short Sensors::calculate_base_ir_value ( char index ) |
jah128 | 17:bf614e28668f | 491 | { |
jah128 | 2:c6986ee3c7c5 | 492 | // If the index is not in the correct range or the base IR values have not been stored, return zero |
jah128 | 2:c6986ee3c7c5 | 493 | if (index>4 || base_ir_values_stored == 0) return 0.0; |
jah128 | 17:bf614e28668f | 494 | // If the reflection value is greater than the background value, return the subtraction |
jah128 | 17:bf614e28668f | 495 | if(illuminated_base_ir_values[index] > background_base_ir_values[index]) { |
jah128 | 2:c6986ee3c7c5 | 496 | return illuminated_base_ir_values[index] - background_base_ir_values[index]; |
jah128 | 17:bf614e28668f | 497 | //Otherwise return zero |
jah128 | 2:c6986ee3c7c5 | 498 | } else { |
jah128 | 17:bf614e28668f | 499 | return 0.0; |
jah128 | 2:c6986ee3c7c5 | 500 | } |
jah128 | 2:c6986ee3c7c5 | 501 | } |
jah128 | 0:d6269d17c8cf | 502 | |
jah128 | 17:bf614e28668f | 503 | float Sensors::get_calibrated_base_ir_value ( char index ) |
jah128 | 17:bf614e28668f | 504 | { |
jah128 | 17:bf614e28668f | 505 | float uncalibrated_value = (float) calculate_base_ir_value(index); |
jah128 | 17:bf614e28668f | 506 | float lower = 0.9f * cv_bir_b[index]; |
jah128 | 17:bf614e28668f | 507 | float calibrated_value = uncalibrated_value - lower; |
jah128 | 17:bf614e28668f | 508 | float upper = 1.1f * (cv_bir_w[index] - lower); |
jah128 | 17:bf614e28668f | 509 | calibrated_value /= upper; |
jah128 | 17:bf614e28668f | 510 | if(calibrated_value < 0) calibrated_value = 0; |
jah128 | 17:bf614e28668f | 511 | if(calibrated_value > 1) calibrated_value = 1; |
jah128 | 17:bf614e28668f | 512 | return calibrated_value; |
jah128 | 17:bf614e28668f | 513 | } |
jah128 | 17:bf614e28668f | 514 | |
jah128 | 2:c6986ee3c7c5 | 515 | // Returns the subtraction of the background side IR value from the reflection based on last call of store_illuminated_base_ir_values |
jah128 | 17:bf614e28668f | 516 | unsigned short Sensors::calculate_side_ir_value ( char index ) |
jah128 | 17:bf614e28668f | 517 | { |
jah128 | 2:c6986ee3c7c5 | 518 | // If the index is not in the correct range or the base IR values have not been stored, return zero |
jah128 | 2:c6986ee3c7c5 | 519 | if (index>7 || ir_values_stored == 0) return 0.0; |
jah128 | 2:c6986ee3c7c5 | 520 | // If the reflection value is greater than the background value, return the subtraction |
jah128 | 17:bf614e28668f | 521 | if(illuminated_ir_values[index] > background_ir_values[index]) { |
jah128 | 2:c6986ee3c7c5 | 522 | return illuminated_ir_values[index] - background_ir_values[index]; |
jah128 | 17:bf614e28668f | 523 | //Otherwise return zero |
jah128 | 2:c6986ee3c7c5 | 524 | } else { |
jah128 | 17:bf614e28668f | 525 | return 0.0; |
jah128 | 2:c6986ee3c7c5 | 526 | } |
jah128 | 2:c6986ee3c7c5 | 527 | } |
jah128 | 0:d6269d17c8cf | 528 | |
jah128 | 17:bf614e28668f | 529 | void Sensors::calibrate_base_sensors (void) |
jah128 | 1:060690a934a9 | 530 | { |
jah128 | 17:bf614e28668f | 531 | char test_colour_sensor = has_base_colour_sensor; |
jah128 | 0:d6269d17c8cf | 532 | short white_background[5]; |
jah128 | 0:d6269d17c8cf | 533 | short white_active[5]; |
jah128 | 0:d6269d17c8cf | 534 | short black_background[5]; |
jah128 | 0:d6269d17c8cf | 535 | short black_active[5]; |
jah128 | 17:bf614e28668f | 536 | int white_colour[4]; |
jah128 | 17:bf614e28668f | 537 | int black_colour[4]; |
jah128 | 1:060690a934a9 | 538 | for(int k=0; k<5; k++) { |
jah128 | 0:d6269d17c8cf | 539 | white_background[k]=0; |
jah128 | 0:d6269d17c8cf | 540 | black_background[k]=0; |
jah128 | 0:d6269d17c8cf | 541 | white_active[k]=0; |
jah128 | 1:060690a934a9 | 542 | black_active[k]=0; |
jah128 | 1:060690a934a9 | 543 | } |
jah128 | 17:bf614e28668f | 544 | |
jah128 | 17:bf614e28668f | 545 | pc.printf("\nBase Sensor Calibration\n"); |
jah128 | 17:bf614e28668f | 546 | display.clear_display(); |
jah128 | 17:bf614e28668f | 547 | display.write_string("Starting sensor"); |
jah128 | 17:bf614e28668f | 548 | display.set_position(1,0); |
jah128 | 17:bf614e28668f | 549 | display.write_string("calibration..."); |
jah128 | 17:bf614e28668f | 550 | wait(1); |
jah128 | 17:bf614e28668f | 551 | display.clear_display(); |
jah128 | 17:bf614e28668f | 552 | display.write_string("Place robot on"); |
jah128 | 17:bf614e28668f | 553 | display.set_position(1,0); |
jah128 | 17:bf614e28668f | 554 | display.write_string("white surface"); |
jah128 | 17:bf614e28668f | 555 | wait(4); |
jah128 | 0:d6269d17c8cf | 556 | display.clear_display(); |
jah128 | 0:d6269d17c8cf | 557 | display.write_string("Calibrating base"); |
jah128 | 0:d6269d17c8cf | 558 | display.set_position(1,0); |
jah128 | 0:d6269d17c8cf | 559 | display.write_string("IR sensor"); |
jah128 | 0:d6269d17c8cf | 560 | wait(0.5); |
jah128 | 17:bf614e28668f | 561 | pc.printf("\n\nWhite Surface IR Results:\n"); |
jah128 | 1:060690a934a9 | 562 | |
jah128 | 1:060690a934a9 | 563 | for(int i=0; i<5; i++) { |
jah128 | 1:060690a934a9 | 564 | wait(0.2); |
jah128 | 1:060690a934a9 | 565 | store_background_base_ir_values(); |
jah128 | 1:060690a934a9 | 566 | |
jah128 | 17:bf614e28668f | 567 | display.set_position(1,9+i); |
jah128 | 1:060690a934a9 | 568 | display.write_string("."); |
jah128 | 1:060690a934a9 | 569 | wait(0.2); |
jah128 | 1:060690a934a9 | 570 | store_illuminated_base_ir_values(); |
jah128 | 1:060690a934a9 | 571 | for(int k=0; k<5; k++) { |
jah128 | 1:060690a934a9 | 572 | white_background[k]+= get_background_base_ir_value(k); |
jah128 | 1:060690a934a9 | 573 | white_active[k] += get_illuminated_base_ir_value(k); |
jah128 | 1:060690a934a9 | 574 | } |
jah128 | 1:060690a934a9 | 575 | 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 | 576 | get_background_base_ir_value(0), get_illuminated_base_ir_value(0), |
jah128 | 1:060690a934a9 | 577 | get_background_base_ir_value(1), get_illuminated_base_ir_value(1), |
jah128 | 1:060690a934a9 | 578 | get_background_base_ir_value(2), get_illuminated_base_ir_value(2), |
jah128 | 1:060690a934a9 | 579 | get_background_base_ir_value(3), get_illuminated_base_ir_value(3), |
jah128 | 1:060690a934a9 | 580 | get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); |
jah128 | 0:d6269d17c8cf | 581 | } |
jah128 | 1:060690a934a9 | 582 | for(int k=0; k<5; k++) { |
jah128 | 0:d6269d17c8cf | 583 | white_background[k]/=5; |
jah128 | 0:d6269d17c8cf | 584 | white_active[k]/=5; |
jah128 | 0:d6269d17c8cf | 585 | } |
jah128 | 0:d6269d17c8cf | 586 | pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", |
jah128 | 1:060690a934a9 | 587 | white_background[0], white_active[0], |
jah128 | 1:060690a934a9 | 588 | white_background[1], white_active[1], |
jah128 | 1:060690a934a9 | 589 | white_background[2], white_active[2], |
jah128 | 1:060690a934a9 | 590 | white_background[3], white_active[3], |
jah128 | 1:060690a934a9 | 591 | white_background[4], white_active[4]); |
jah128 | 17:bf614e28668f | 592 | wait(0.25); |
jah128 | 17:bf614e28668f | 593 | char retake_sample = 1; |
jah128 | 17:bf614e28668f | 594 | if(test_colour_sensor) { |
jah128 | 17:bf614e28668f | 595 | display.clear_display(); |
jah128 | 17:bf614e28668f | 596 | display.write_string("Calibrating base"); |
jah128 | 17:bf614e28668f | 597 | display.set_position(1,0); |
jah128 | 17:bf614e28668f | 598 | display.write_string("colour sensor"); |
jah128 | 17:bf614e28668f | 599 | wait(0.2); |
jah128 | 17:bf614e28668f | 600 | char retake_white_count = 0; |
jah128 | 17:bf614e28668f | 601 | |
jah128 | 17:bf614e28668f | 602 | while(retake_sample == 1 && retake_white_count<3) { |
jah128 | 17:bf614e28668f | 603 | wait(0.2); |
jah128 | 17:bf614e28668f | 604 | led.set_base_led(1); |
jah128 | 17:bf614e28668f | 605 | colour.enable_base_colour_sensor(); |
jah128 | 17:bf614e28668f | 606 | wait(0.03); |
jah128 | 17:bf614e28668f | 607 | colour.read_base_colour_sensor_values( white_colour ); |
jah128 | 17:bf614e28668f | 608 | colour.disable_base_colour_sensor(); |
jah128 | 17:bf614e28668f | 609 | led.set_base_led(0); |
jah128 | 17:bf614e28668f | 610 | if(white_colour[1] < 1 || white_colour[1] > 1022 || white_colour[2] < 1 || white_colour[2] > 1022 || white_colour[3] < 1 || white_colour[3] > 1022) { |
jah128 | 17:bf614e28668f | 611 | pc.printf("Warning: Colour tested exceeded expected range\n"); |
jah128 | 17:bf614e28668f | 612 | retake_white_count ++; |
jah128 | 17:bf614e28668f | 613 | } else retake_sample=0; |
jah128 | 17:bf614e28668f | 614 | } |
jah128 | 17:bf614e28668f | 615 | if(retake_white_count > 2) { |
jah128 | 17:bf614e28668f | 616 | test_colour_sensor = 0; |
jah128 | 17:bf614e28668f | 617 | pc.printf("The colour sensor test has produced results outside expected range; check gain settings and test setup"); |
jah128 | 17:bf614e28668f | 618 | } else { |
jah128 | 17:bf614e28668f | 619 | wait(0.1); |
jah128 | 17:bf614e28668f | 620 | pc.printf("\n\nWhite Surface Colour Sensor Results:\n"); |
jah128 | 17:bf614e28668f | 621 | pc.printf("BRIGHTNESS:%4d RED:%4d GREEN:%4d BLUE:%d\n",white_colour[0],white_colour[1],white_colour[2],white_colour[3]); |
jah128 | 17:bf614e28668f | 622 | } |
jah128 | 17:bf614e28668f | 623 | } |
jah128 | 17:bf614e28668f | 624 | wait(0.5); |
jah128 | 1:060690a934a9 | 625 | |
jah128 | 0:d6269d17c8cf | 626 | display.clear_display(); |
jah128 | 0:d6269d17c8cf | 627 | display.write_string("Place robot on"); |
jah128 | 0:d6269d17c8cf | 628 | display.set_position(1,0); |
jah128 | 0:d6269d17c8cf | 629 | display.write_string("black surface"); |
jah128 | 17:bf614e28668f | 630 | wait(3.5); |
jah128 | 1:060690a934a9 | 631 | |
jah128 | 0:d6269d17c8cf | 632 | display.clear_display(); |
jah128 | 0:d6269d17c8cf | 633 | display.write_string("Calibrating base"); |
jah128 | 0:d6269d17c8cf | 634 | display.set_position(1,0); |
jah128 | 0:d6269d17c8cf | 635 | display.write_string("IR sensor"); |
jah128 | 0:d6269d17c8cf | 636 | wait(0.5); |
jah128 | 17:bf614e28668f | 637 | pc.printf("\nBlack Surface Results:\n"); |
jah128 | 1:060690a934a9 | 638 | |
jah128 | 1:060690a934a9 | 639 | for(int i=0; i<5; i++) { |
jah128 | 1:060690a934a9 | 640 | wait(0.2); |
jah128 | 0:d6269d17c8cf | 641 | |
jah128 | 1:060690a934a9 | 642 | store_background_base_ir_values(); |
jah128 | 1:060690a934a9 | 643 | display.set_position(1,9); |
jah128 | 1:060690a934a9 | 644 | display.write_string("."); |
jah128 | 1:060690a934a9 | 645 | wait(0.2); |
jah128 | 1:060690a934a9 | 646 | store_illuminated_base_ir_values(); |
jah128 | 1:060690a934a9 | 647 | for(int k=0; k<5; k++) { |
jah128 | 1:060690a934a9 | 648 | black_background[k]+= get_background_base_ir_value(k); |
jah128 | 1:060690a934a9 | 649 | black_active[k] += get_illuminated_base_ir_value(k); |
jah128 | 1:060690a934a9 | 650 | } |
jah128 | 1:060690a934a9 | 651 | 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 | 652 | get_background_base_ir_value(0), get_illuminated_base_ir_value(0), |
jah128 | 1:060690a934a9 | 653 | get_background_base_ir_value(1), get_illuminated_base_ir_value(1), |
jah128 | 1:060690a934a9 | 654 | get_background_base_ir_value(2), get_illuminated_base_ir_value(2), |
jah128 | 1:060690a934a9 | 655 | get_background_base_ir_value(3), get_illuminated_base_ir_value(3), |
jah128 | 1:060690a934a9 | 656 | get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); |
jah128 | 1:060690a934a9 | 657 | } |
jah128 | 1:060690a934a9 | 658 | for(int k=0; k<5; k++) { |
jah128 | 0:d6269d17c8cf | 659 | black_background[k]/=5; |
jah128 | 0:d6269d17c8cf | 660 | black_active[k]/=5; |
jah128 | 0:d6269d17c8cf | 661 | } |
jah128 | 1:060690a934a9 | 662 | pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", |
jah128 | 1:060690a934a9 | 663 | black_background[0], black_active[0], |
jah128 | 1:060690a934a9 | 664 | black_background[1], black_active[1], |
jah128 | 1:060690a934a9 | 665 | black_background[2], black_active[2], |
jah128 | 1:060690a934a9 | 666 | black_background[3], black_active[3], |
jah128 | 1:060690a934a9 | 667 | black_background[4], black_active[4]); |
jah128 | 17:bf614e28668f | 668 | wait(0.25); |
jah128 | 17:bf614e28668f | 669 | if(test_colour_sensor) { |
jah128 | 17:bf614e28668f | 670 | display.clear_display(); |
jah128 | 17:bf614e28668f | 671 | display.write_string("Calibrating base"); |
jah128 | 17:bf614e28668f | 672 | display.set_position(1,0); |
jah128 | 17:bf614e28668f | 673 | display.write_string("colour sensor"); |
jah128 | 17:bf614e28668f | 674 | wait(0.2); |
jah128 | 17:bf614e28668f | 675 | char retake_black_count = 0; |
jah128 | 17:bf614e28668f | 676 | retake_sample = 1; |
jah128 | 17:bf614e28668f | 677 | while(retake_sample == 1 && retake_black_count<3) { |
jah128 | 17:bf614e28668f | 678 | wait(0.2); |
jah128 | 17:bf614e28668f | 679 | led.set_base_led(1); |
jah128 | 17:bf614e28668f | 680 | colour.enable_base_colour_sensor(); |
jah128 | 17:bf614e28668f | 681 | wait(0.03); |
jah128 | 17:bf614e28668f | 682 | colour.read_base_colour_sensor_values( black_colour ); |
jah128 | 17:bf614e28668f | 683 | colour.disable_base_colour_sensor(); |
jah128 | 17:bf614e28668f | 684 | led.set_base_led(0); |
jah128 | 17:bf614e28668f | 685 | if(black_colour[1] < 1 || black_colour[1] > 1022 || black_colour[2] < 1 || black_colour[2] > 1022 || black_colour[3] < 1 || black_colour[3] > 1022) { |
jah128 | 17:bf614e28668f | 686 | pc.printf("Warning: Colour tested exceeded expected range\n"); |
jah128 | 17:bf614e28668f | 687 | retake_black_count ++; |
jah128 | 17:bf614e28668f | 688 | } else retake_sample=0; |
jah128 | 17:bf614e28668f | 689 | } |
jah128 | 17:bf614e28668f | 690 | if(retake_black_count > 2) { |
jah128 | 17:bf614e28668f | 691 | test_colour_sensor = 0; |
jah128 | 17:bf614e28668f | 692 | pc.printf("The colour sensor test has produced results outside expected range; check gain settings and test setup"); |
jah128 | 17:bf614e28668f | 693 | } else { |
jah128 | 17:bf614e28668f | 694 | wait(0.1); |
jah128 | 17:bf614e28668f | 695 | pc.printf("\n\nBlack Surface Colour Sensor Results:\n"); |
jah128 | 17:bf614e28668f | 696 | pc.printf("BRIGHTNESS:%4d RED:%4d GREEN:%4d BLUE:%d\n",black_colour[0],black_colour[1],black_colour[2],black_colour[3]); |
jah128 | 17:bf614e28668f | 697 | } |
jah128 | 17:bf614e28668f | 698 | } |
jah128 | 17:bf614e28668f | 699 | wait(1); |
jah128 | 17:bf614e28668f | 700 | while(i2c_setup.IF_get_switch_state() != 0) { |
jah128 | 17:bf614e28668f | 701 | display.clear_display(); |
jah128 | 17:bf614e28668f | 702 | display.set_position(0,0); |
jah128 | 17:bf614e28668f | 703 | display.write_string("RELEASE SWITCH!"); |
jah128 | 17:bf614e28668f | 704 | wait(0.1); |
jah128 | 17:bf614e28668f | 705 | } |
jah128 | 17:bf614e28668f | 706 | display.clear_display(); |
jah128 | 17:bf614e28668f | 707 | display.set_position(0,0); |
jah128 | 17:bf614e28668f | 708 | display.write_string("^ REJECT"); |
jah128 | 17:bf614e28668f | 709 | display.set_position(1,2); |
jah128 | 17:bf614e28668f | 710 | display.write_string("ACCEPT"); |
jah128 | 17:bf614e28668f | 711 | char switch_pos = i2c_setup.IF_get_switch_state(); |
jah128 | 17:bf614e28668f | 712 | while(switch_pos != 1 && switch_pos != 2) switch_pos = i2c_setup.IF_get_switch_state(); |
jah128 | 17:bf614e28668f | 713 | if(switch_pos == 2) { |
jah128 | 17:bf614e28668f | 714 | pc.printf("\nChanges accepted: Updating firmware values in EEPROM\n"); |
jah128 | 17:bf614e28668f | 715 | display.clear_display(); |
jah128 | 17:bf614e28668f | 716 | display.set_position(0,0); |
jah128 | 17:bf614e28668f | 717 | display.write_string("UPDATING"); |
jah128 | 17:bf614e28668f | 718 | display.set_position(1,0); |
jah128 | 17:bf614e28668f | 719 | display.write_string("FIRMWARE"); |
jah128 | 17:bf614e28668f | 720 | wait(0.5); |
jah128 | 17:bf614e28668f | 721 | pc.printf("- Updating bass IR sensor values\n"); |
jah128 | 17:bf614e28668f | 722 | eprom.IF_write_base_ir_calibration_values(white_active,black_active); |
jah128 | 17:bf614e28668f | 723 | if(test_colour_sensor == 1) { |
jah128 | 17:bf614e28668f | 724 | wait(0.5); |
jah128 | 17:bf614e28668f | 725 | pc.printf("- Updating bass colour sensor values\n"); |
jah128 | 17:bf614e28668f | 726 | eprom.IF_write_base_colour_calibration_values(white_colour,black_colour); |
jah128 | 17:bf614e28668f | 727 | } |
jah128 | 17:bf614e28668f | 728 | wait(1.5); |
jah128 | 17:bf614e28668f | 729 | } else { |
jah128 | 17:bf614e28668f | 730 | pc.printf("\nChanges rejected.\n"); |
jah128 | 17:bf614e28668f | 731 | } |
jah128 | 17:bf614e28668f | 732 | display.clear_display(); |
jah128 | 17:bf614e28668f | 733 | wait(0.5); |
jah128 | 17:bf614e28668f | 734 | pc.printf("Base sensor calibration routine complete\n\n"); |
jah128 | 0:d6269d17c8cf | 735 | } |
jah128 | 0:d6269d17c8cf | 736 | |
jah128 | 0:d6269d17c8cf | 737 | |
jah128 | 10:e58323951c08 | 738 | int Sensors::get_bearing_from_ir_array (unsigned short * ir_sensor_readings) |
jah128 | 1:060690a934a9 | 739 | { |
jah128 | 0:d6269d17c8cf | 740 | //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 | 741 | |
jah128 | 0:d6269d17c8cf | 742 | float degrees_per_radian = 57.295779513; |
jah128 | 1:060690a934a9 | 743 | |
jah128 | 0:d6269d17c8cf | 744 | // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors |
jah128 | 0:d6269d17c8cf | 745 | float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432}; |
jah128 | 0:d6269d17c8cf | 746 | float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533}; |
jah128 | 1:060690a934a9 | 747 | |
jah128 | 0:d6269d17c8cf | 748 | float sin_sum = 0; |
jah128 | 0:d6269d17c8cf | 749 | float cos_sum = 0; |
jah128 | 1:060690a934a9 | 750 | |
jah128 | 1:060690a934a9 | 751 | for(int i = 0; i < 8; i++) { |
jah128 | 0:d6269d17c8cf | 752 | // Use IR sensor reading to weight bearing vector |
jah128 | 0:d6269d17c8cf | 753 | sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i]; |
jah128 | 0:d6269d17c8cf | 754 | cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i]; |
jah128 | 0:d6269d17c8cf | 755 | } |
jah128 | 1:060690a934a9 | 756 | |
jah128 | 1:060690a934a9 | 757 | float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source |
jah128 | 0:d6269d17c8cf | 758 | bearing *= degrees_per_radian; // Convert to degrees |
jah128 | 0:d6269d17c8cf | 759 | |
jah128 | 0:d6269d17c8cf | 760 | //out("Sin sum:%f Cos sum:%f Bearing:%f\n",sin_sum,cos_sum,bearing); |
jah128 | 0:d6269d17c8cf | 761 | |
jah128 | 0:d6269d17c8cf | 762 | return (int) bearing; |
jah128 | 1:060690a934a9 | 763 | } |
jah128 | 0:d6269d17c8cf | 764 | |
jah128 | 17:bf614e28668f | 765 | void Sensors::IF_set_base_calibration_values(int bir1w, int bir2w, int bir3w, int bir4w, int bir5w, int bir1b, int bir2b, int bir3b, int bir4b, int bir5b) |
jah128 | 17:bf614e28668f | 766 | { |
jah128 | 17:bf614e28668f | 767 | cv_bir_w[0] = bir1w; |
jah128 | 17:bf614e28668f | 768 | cv_bir_w[1] = bir2w; |
jah128 | 17:bf614e28668f | 769 | cv_bir_w[2] = bir3w; |
jah128 | 17:bf614e28668f | 770 | cv_bir_w[3] = bir4w; |
jah128 | 17:bf614e28668f | 771 | cv_bir_w[4] = bir5w; |
jah128 | 17:bf614e28668f | 772 | cv_bir_b[0] = bir1b; |
jah128 | 17:bf614e28668f | 773 | cv_bir_b[1] = bir2b; |
jah128 | 17:bf614e28668f | 774 | cv_bir_b[2] = bir3b; |
jah128 | 17:bf614e28668f | 775 | cv_bir_b[3] = bir4b; |
jah128 | 17:bf614e28668f | 776 | cv_bir_b[4] = bir5b; |
jah128 | 17:bf614e28668f | 777 | } |