James Hilder / PsiSwarmV9-Fork

Psi Swarm robot library version 0.9

Fork of PsiSwarmV9 by Psi Swarm Robot

sensors.cpp@20:1bc6c6dc477b, 2018-05-14 (annotated)

Committer:
jah128
Date:
Mon May 14 15:35:38 2018 +0000
Revision:
20:1bc6c6dc477b
Parent:
18:9204f74069b4 
Updated?

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 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 18:9204f74069b4 726 eprom.IF_write_base_colour_calibration_values(black_colour,white_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 }
jah128 18:9204f74069b4 778 char cv_description[105];
jah128 18:9204f74069b4 779 const char * Sensors::IF_get_base_calibration_values_string()
jah128 18:9204f74069b4 780 {
jah128 18:9204f74069b4 781
jah128 18:9204f74069b4 782 sprintf(cv_description,"[BLACK 1:%4d 2:%4d 3:%4d 4:%4d 5:%4d WHITE 1:%4d 2:%4d 3:%4d 4:%4d 5:%4d]",cv_bir_b[0],cv_bir_b[1],cv_bir_b[2],cv_bir_b[3],cv_bir_b[4],cv_bir_w[0],cv_bir_w[1],cv_bir_w[2],cv_bir_w[3],cv_bir_w[4]);
jah128 18:9204f74069b4 783 return cv_description;
jah128 18:9204f74069b4 784 }