Psi Swarm Robot / PsiSwarmV9

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   PsiSwarm_V9_Blank

Fork of PsiSwarmV9 by James Hilder

sensors.cpp@10:e58323951c08, 2016-10-16 (annotated)

Committer:
jah128
Date:
Sun Oct 16 16:00:20 2016 +0000
Revision:
10:e58323951c08
Parent:
6:b340a527add9
Child:
11:312663037b8c
Added Sensors class

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