James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/sensors.cpp@2:a6214fd156ff, 2015-10-05 (annotated)

Committer:
jah128
Date:
Mon Oct 05 14:42:16 2015 +0000
Revision:
2:a6214fd156ff
Parent:
0:8a5497a2e366
Child:
3:cd048f6e544e
Changed nothing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a5497a2e366 1 /* University of York Robotics Laboratory PsiSwarm Library: Sensor Functions Source File
jah128 0:8a5497a2e366 2 *
jah128 0:8a5497a2e366 3 * File: sensors.cpp
jah128 0:8a5497a2e366 4 *
jah128 0:8a5497a2e366 5 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
jah128 0:8a5497a2e366 6 *
jah128 0:8a5497a2e366 7 * PsiSwarm Library Version: 0.2
jah128 0:8a5497a2e366 8 *
jah128 0:8a5497a2e366 9 * September 2015
jah128 0:8a5497a2e366 10 *
jah128 0:8a5497a2e366 11 */
jah128 0:8a5497a2e366 12
jah128 0:8a5497a2e366 13 #include "psiswarm.h"
jah128 0:8a5497a2e366 14
jah128 0:8a5497a2e366 15 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 16 // Ultrasonic Sensor (SRF02) Functions
jah128 0:8a5497a2e366 17 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 18
jah128 0:8a5497a2e366 19 // The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure().
jah128 0:8a5497a2e366 20 // It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker
jah128 0:8a5497a2e366 21
jah128 0:8a5497a2e366 22 void enable_ultrasonic_ticker(){
jah128 0:8a5497a2e366 23 ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000);
jah128 0:8a5497a2e366 24 }
jah128 0:8a5497a2e366 25
jah128 0:8a5497a2e366 26 void disable_ultrasonic_ticker(){
jah128 0:8a5497a2e366 27 ultrasonic_ticker.detach();
jah128 0:8a5497a2e366 28 }
jah128 0:8a5497a2e366 29
jah128 0:8a5497a2e366 30 void update_ultrasonic_measure(){
jah128 0:8a5497a2e366 31 if(waiting_for_ultrasonic == 0){
jah128 0:8a5497a2e366 32 waiting_for_ultrasonic = 1;
jah128 0:8a5497a2e366 33
jah128 0:8a5497a2e366 34 char command[2];
jah128 0:8a5497a2e366 35 command[0] = 0x00; // Set the command register
jah128 0:8a5497a2e366 36 command[1] = 0x51; // Get result is centimeters
jah128 0:8a5497a2e366 37 primary_i2c.write(ULTRASONIC_ADDRESS, command, 2); // Send the command to start a ranging burst
jah128 0:8a5497a2e366 38
jah128 0:8a5497a2e366 39 ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000);
jah128 0:8a5497a2e366 40 }else{
jah128 0:8a5497a2e366 41 debug("WARNING: Ultrasonic sensor called too frequently.\n");
jah128 0:8a5497a2e366 42 }
jah128 0:8a5497a2e366 43 }
jah128 0:8a5497a2e366 44
jah128 0:8a5497a2e366 45 void IF_read_ultrasonic_measure(){
jah128 0:8a5497a2e366 46 char command[1];
jah128 0:8a5497a2e366 47 char result[2];
jah128 0:8a5497a2e366 48 command[0] = 0x02; // The start address of measure result
jah128 0:8a5497a2e366 49 primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1); // Send address to read a measure
jah128 0:8a5497a2e366 50 primary_i2c.read(ULTRASONIC_ADDRESS, result, 2); // Read two byte of measure
jah128 0:8a5497a2e366 51 ultrasonic_distance = (result[0]<<8)+result[1];
jah128 0:8a5497a2e366 52 ultrasonic_distance_updated = 1;
jah128 0:8a5497a2e366 53 waiting_for_ultrasonic = 0;
jah128 0:8a5497a2e366 54 debug("US:%d cm\n",ultrasonic_distance);
jah128 0:8a5497a2e366 55 }
jah128 0:8a5497a2e366 56
jah128 0:8a5497a2e366 57 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 58 // Additional Sensing Functions
jah128 0:8a5497a2e366 59 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 60
jah128 0:8a5497a2e366 61 float get_temperature(){
jah128 0:8a5497a2e366 62 return IF_read_from_temperature_sensor();
jah128 0:8a5497a2e366 63 }
jah128 0:8a5497a2e366 64
jah128 0:8a5497a2e366 65 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 66 // Voltage Sensing Functions
jah128 0:8a5497a2e366 67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 68
jah128 0:8a5497a2e366 69 float get_battery_voltage (){
jah128 0:8a5497a2e366 70 float raw_value = vin_battery.read();
jah128 0:8a5497a2e366 71 return raw_value * 4.95;
jah128 0:8a5497a2e366 72 }
jah128 0:8a5497a2e366 73
jah128 0:8a5497a2e366 74 float get_current (){
jah128 0:8a5497a2e366 75 // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor
jah128 0:8a5497a2e366 76 // Device gain = 500
jah128 0:8a5497a2e366 77 float raw_value = vin_current.read();
jah128 0:8a5497a2e366 78 return raw_value * 3.3;
jah128 0:8a5497a2e366 79 }
jah128 0:8a5497a2e366 80
jah128 0:8a5497a2e366 81 float get_dc_voltage (){
jah128 0:8a5497a2e366 82 float raw_value = vin_dc.read();
jah128 0:8a5497a2e366 83 return raw_value * 6.6;
jah128 0:8a5497a2e366 84 }
jah128 0:8a5497a2e366 85
jah128 0:8a5497a2e366 86 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 87 // IR Sensor Functions
jah128 0:8a5497a2e366 88 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 0:8a5497a2e366 89
jah128 0:8a5497a2e366 90 // Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).
jah128 0:8a5497a2e366 91 // The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
jah128 0:8a5497a2e366 92 // NB This function is preserved for code-compatability and cases where only a single reading is needed
jah128 0:8a5497a2e366 93 // 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 0:8a5497a2e366 94 float read_reflected_ir_distance ( char index ){
jah128 0:8a5497a2e366 95 // Sanity check
jah128 0:8a5497a2e366 96 if(index>7) return 0.0;
jah128 0:8a5497a2e366 97
jah128 0:8a5497a2e366 98 //1. Read the ADC value without IR emitter on; store in the background_ir_values[] array
jah128 0:8a5497a2e366 99 background_ir_values [index] = IF_read_IR_adc_value(1, index );
jah128 0:8a5497a2e366 100
jah128 0:8a5497a2e366 101 //2. Enable the relevant IR emitter by turning on its pulse output
jah128 0:8a5497a2e366 102 switch(index){
jah128 0:8a5497a2e366 103 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 104 IF_set_IR_emitter_output(0, 1);
jah128 0:8a5497a2e366 105 break;
jah128 0:8a5497a2e366 106 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 107 IF_set_IR_emitter_output(1, 1);
jah128 0:8a5497a2e366 108 break;
jah128 0:8a5497a2e366 109 }
jah128 0:8a5497a2e366 110 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 111
jah128 0:8a5497a2e366 112 //3. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array
jah128 0:8a5497a2e366 113 illuminated_ir_values [index] = IF_read_IR_adc_value (1, index );
jah128 0:8a5497a2e366 114
jah128 0:8a5497a2e366 115 //4. Turn off IR emitter
jah128 0:8a5497a2e366 116 switch(index){
jah128 0:8a5497a2e366 117 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 118 IF_set_IR_emitter_output(0, 0);
jah128 0:8a5497a2e366 119 break;
jah128 0:8a5497a2e366 120 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 121 IF_set_IR_emitter_output(1, 0);
jah128 0:8a5497a2e366 122 break;
jah128 0:8a5497a2e366 123 }
jah128 0:8a5497a2e366 124
jah128 0:8a5497a2e366 125 //5. Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances()
jah128 0:8a5497a2e366 126 reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]);
jah128 0:8a5497a2e366 127 ir_values_stored = 1;
jah128 0:8a5497a2e366 128 return reflected_ir_distances [index];
jah128 0:8a5497a2e366 129 }
jah128 0:8a5497a2e366 130
jah128 0:8a5497a2e366 131
jah128 0:8a5497a2e366 132 // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances
jah128 0:8a5497a2e366 133 float get_reflected_ir_distance ( char index ){
jah128 0:8a5497a2e366 134 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 135 if (index>7 || ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 136 return reflected_ir_distances[index];
jah128 0:8a5497a2e366 137 }
jah128 0:8a5497a2e366 138
jah128 0:8a5497a2e366 139 // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
jah128 0:8a5497a2e366 140 unsigned short get_background_raw_ir_value ( char index ){
jah128 0:8a5497a2e366 141 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 142 if (index>7 || ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 143 return background_ir_values[index];
jah128 0:8a5497a2e366 144 }
jah128 0:8a5497a2e366 145
jah128 0:8a5497a2e366 146 // Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values
jah128 0:8a5497a2e366 147 unsigned short get_illuminated_raw_ir_value ( char index ){
jah128 0:8a5497a2e366 148 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 149 if (index>7 || ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 150 return illuminated_ir_values[index];
jah128 0:8a5497a2e366 151 }
jah128 0:8a5497a2e366 152
jah128 0:8a5497a2e366 153 // Stores the reflected distances for all 8 IR sensors
jah128 0:8a5497a2e366 154 void store_reflected_ir_distances ( void ){
jah128 0:8a5497a2e366 155 store_ir_values();
jah128 0:8a5497a2e366 156 for(int i=0;i<8;i++){
jah128 0:8a5497a2e366 157 reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
jah128 0:8a5497a2e366 158 }
jah128 0:8a5497a2e366 159 }
jah128 0:8a5497a2e366 160
jah128 0:8a5497a2e366 161 // Stores the rbackground and illuminated values for all 8 IR sensors
jah128 0:8a5497a2e366 162 void store_ir_values ( void ){
jah128 0:8a5497a2e366 163 store_background_raw_ir_values();
jah128 0:8a5497a2e366 164 store_illuminated_raw_ir_values();
jah128 0:8a5497a2e366 165 }
jah128 0:8a5497a2e366 166
jah128 0:8a5497a2e366 167 // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters
jah128 0:8a5497a2e366 168 void store_background_raw_ir_values ( void ){
jah128 0:8a5497a2e366 169 ir_values_stored = 1;
jah128 0:8a5497a2e366 170 for(int i=0;i<8;i++){
jah128 0:8a5497a2e366 171 background_ir_values [i] = IF_read_IR_adc_value(1,i);
jah128 0:8a5497a2e366 172 }
jah128 0:8a5497a2e366 173 }
jah128 0:8a5497a2e366 174
jah128 0:8a5497a2e366 175 // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse
jah128 0:8a5497a2e366 176 void store_illuminated_raw_ir_values ( void ){
jah128 0:8a5497a2e366 177 //1. Enable the front IR emitters and store the values
jah128 0:8a5497a2e366 178 IF_set_IR_emitter_output(0, 1);
jah128 0:8a5497a2e366 179 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 180 illuminated_ir_values [0] = IF_read_IR_adc_value(1,0);
jah128 0:8a5497a2e366 181 illuminated_ir_values [1] = IF_read_IR_adc_value(1,1);
jah128 0:8a5497a2e366 182 illuminated_ir_values [6] = IF_read_IR_adc_value(1,6);
jah128 0:8a5497a2e366 183 illuminated_ir_values [7] = IF_read_IR_adc_value(1,7);
jah128 0:8a5497a2e366 184 IF_set_IR_emitter_output(0, 0);
jah128 0:8a5497a2e366 185
jah128 0:8a5497a2e366 186 //2. Enable the rear+side IR emitters and store the values
jah128 0:8a5497a2e366 187 IF_set_IR_emitter_output(1, 1);
jah128 0:8a5497a2e366 188 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 189 illuminated_ir_values [2] = IF_read_IR_adc_value(1,2);
jah128 0:8a5497a2e366 190 illuminated_ir_values [3] = IF_read_IR_adc_value(1,3);
jah128 0:8a5497a2e366 191 illuminated_ir_values [4] = IF_read_IR_adc_value(1,4);
jah128 0:8a5497a2e366 192 illuminated_ir_values [5] = IF_read_IR_adc_value(1,5);
jah128 0:8a5497a2e366 193 IF_set_IR_emitter_output(1, 0);
jah128 0:8a5497a2e366 194 }
jah128 0:8a5497a2e366 195
jah128 0:8a5497a2e366 196 // Converts a background and illuminated value into a distance
jah128 0:8a5497a2e366 197 float calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ){
jah128 0:8a5497a2e366 198 float approximate_distance = 4000 + background_value - illuminated_value;
jah128 0:8a5497a2e366 199 if(approximate_distance < 0) approximate_distance = 0;
jah128 0:8a5497a2e366 200
jah128 0:8a5497a2e366 201 // Very approximate: root value, divide by 2, approx distance in mm
jah128 0:8a5497a2e366 202 approximate_distance = sqrt (approximate_distance) / 2.0;
jah128 0:8a5497a2e366 203
jah128 0:8a5497a2e366 204 // Then add adjustment value if value >27
jah128 0:8a5497a2e366 205 if(approximate_distance > 27) {
jah128 0:8a5497a2e366 206 float shift = pow(approximate_distance - 27,3);
jah128 0:8a5497a2e366 207 approximate_distance += shift;
jah128 0:8a5497a2e366 208 }
jah128 0:8a5497a2e366 209 if(approximate_distance > 90) approximate_distance = 100.0;
jah128 0:8a5497a2e366 210 return approximate_distance;
jah128 0:8a5497a2e366 211 }
jah128 0:8a5497a2e366 212
jah128 0:8a5497a2e366 213 // 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 0:8a5497a2e366 214 unsigned short read_illuminated_raw_ir_value ( char index ){
jah128 0:8a5497a2e366 215 //This function reads the IR strength when illuminated - used for PC system debugging purposes
jah128 0:8a5497a2e366 216 //1. Enable the relevant IR emitter by turning on its pulse output
jah128 0:8a5497a2e366 217 switch(index){
jah128 0:8a5497a2e366 218 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 219 IF_set_IR_emitter_output(0, 1);
jah128 0:8a5497a2e366 220 break;
jah128 0:8a5497a2e366 221 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 222 IF_set_IR_emitter_output(1, 1);
jah128 0:8a5497a2e366 223 break;
jah128 0:8a5497a2e366 224 }
jah128 0:8a5497a2e366 225 wait_us(ir_pulse_delay);
jah128 0:8a5497a2e366 226 //2. Read the ADC value now IR emitter is on
jah128 0:8a5497a2e366 227 unsigned short strong_value = IF_read_IR_adc_value( 1,index );
jah128 0:8a5497a2e366 228 //3. Turn off IR emitter
jah128 0:8a5497a2e366 229 switch(index){
jah128 0:8a5497a2e366 230 case 0: case 1: case 6: case 7:
jah128 0:8a5497a2e366 231 IF_set_IR_emitter_output(0, 0);
jah128 0:8a5497a2e366 232 break;
jah128 0:8a5497a2e366 233 case 2: case 3: case 4: case 5:
jah128 0:8a5497a2e366 234 IF_set_IR_emitter_output(1, 0);
jah128 0:8a5497a2e366 235 break;
jah128 0:8a5497a2e366 236 }
jah128 0:8a5497a2e366 237 return strong_value;
jah128 0:8a5497a2e366 238 }
jah128 0:8a5497a2e366 239
jah128 0:8a5497a2e366 240 // Base IR sensor functions
jah128 0:8a5497a2e366 241
jah128 0:8a5497a2e366 242
jah128 0:8a5497a2e366 243 // Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values
jah128 0:8a5497a2e366 244 unsigned short get_background_base_ir_value ( char index ){
jah128 0:8a5497a2e366 245 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 246 if (index>4 || base_ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 247 return background_base_ir_values[index];
jah128 0:8a5497a2e366 248 }
jah128 0:8a5497a2e366 249
jah128 0:8a5497a2e366 250 // Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values
jah128 0:8a5497a2e366 251 unsigned short get_illuminated_base_ir_value ( char index ){
jah128 0:8a5497a2e366 252 // Sanity check: check range of index and that values have been stored
jah128 0:8a5497a2e366 253 if (index>4 || base_ir_values_stored == 0) return 0.0;
jah128 0:8a5497a2e366 254 return illuminated_base_ir_values[index];
jah128 0:8a5497a2e366 255 }
jah128 0:8a5497a2e366 256
jah128 0:8a5497a2e366 257 // Stores the reflected distances for all 5 base IR sensors
jah128 0:8a5497a2e366 258 void store_base_ir_values ( void ){
jah128 0:8a5497a2e366 259 store_background_base_ir_values();
jah128 0:8a5497a2e366 260 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 261 //for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 262 // reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]);
jah128 0:8a5497a2e366 263 //}
jah128 0:8a5497a2e366 264 }
jah128 0:8a5497a2e366 265
jah128 0:8a5497a2e366 266 // Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters
jah128 0:8a5497a2e366 267 void store_background_base_ir_values ( void ){
jah128 0:8a5497a2e366 268 base_ir_values_stored = 1;
jah128 0:8a5497a2e366 269 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 270 background_base_ir_values [i] = IF_read_IR_adc_value(2,i);
jah128 0:8a5497a2e366 271 }
jah128 0:8a5497a2e366 272 }
jah128 0:8a5497a2e366 273
jah128 0:8a5497a2e366 274 // Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse
jah128 0:8a5497a2e366 275 void store_illuminated_base_ir_values ( void ){
jah128 0:8a5497a2e366 276 //1. Enable the base IR emitters and store the values
jah128 0:8a5497a2e366 277 IF_set_IR_emitter_output(2, 1);
jah128 0:8a5497a2e366 278 wait_us(base_ir_pulse_delay);
jah128 0:8a5497a2e366 279 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 280 illuminated_base_ir_values [i] = IF_read_IR_adc_value(2,i);
jah128 0:8a5497a2e366 281 }
jah128 0:8a5497a2e366 282
jah128 0:8a5497a2e366 283 IF_set_IR_emitter_output(2, 0);
jah128 0:8a5497a2e366 284 }
jah128 0:8a5497a2e366 285
jah128 2:a6214fd156ff 286 // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm
jah128 2:a6214fd156ff 287 void store_line_position ( void ){
jah128 2:a6214fd156ff 288 // Store background and reflected base IR values
jah128 2:a6214fd156ff 289 store_base_ir_values();
jah128 2:a6214fd156ff 290
jah128 2:a6214fd156ff 291 }
jah128 2:a6214fd156ff 292
jah128 2:a6214fd156ff 293
jah128 0:8a5497a2e366 294 void calibrate_base_ir_sensors (void) {
jah128 0:8a5497a2e366 295 short white_background[5];
jah128 0:8a5497a2e366 296 short white_active[5];
jah128 0:8a5497a2e366 297 short black_background[5];
jah128 0:8a5497a2e366 298 short black_active[5];
jah128 0:8a5497a2e366 299 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 300 white_background[k]=0;
jah128 0:8a5497a2e366 301 black_background[k]=0;
jah128 0:8a5497a2e366 302 white_active[k]=0;
jah128 0:8a5497a2e366 303 black_active[k]=0;
jah128 0:8a5497a2e366 304 }
jah128 0:8a5497a2e366 305 pc.printf("Base IR Calibration\n");
jah128 0:8a5497a2e366 306 display.clear_display();
jah128 0:8a5497a2e366 307 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 308 display.set_position(1,0);
jah128 0:8a5497a2e366 309 display.write_string("IR sensor");
jah128 0:8a5497a2e366 310 wait(0.5);
jah128 0:8a5497a2e366 311 display.clear_display();
jah128 0:8a5497a2e366 312 display.write_string("Place robot on");
jah128 0:8a5497a2e366 313 display.set_position(1,0);
jah128 0:8a5497a2e366 314 display.write_string("white surface");
jah128 0:8a5497a2e366 315 wait(3);
jah128 0:8a5497a2e366 316 display.clear_display();
jah128 0:8a5497a2e366 317 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 318 display.set_position(1,0);
jah128 0:8a5497a2e366 319 display.write_string("IR sensor");
jah128 0:8a5497a2e366 320 wait(0.5);
jah128 0:8a5497a2e366 321 pc.printf("\nWhite Background Results:\n");
jah128 0:8a5497a2e366 322
jah128 0:8a5497a2e366 323 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 324 wait(0.2);
jah128 0:8a5497a2e366 325 store_background_base_ir_values();
jah128 0:8a5497a2e366 326
jah128 0:8a5497a2e366 327 display.set_position(1,9);
jah128 0:8a5497a2e366 328 display.write_string(".");
jah128 0:8a5497a2e366 329 wait(0.2);
jah128 0:8a5497a2e366 330 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 331 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 332 white_background[k]+= get_background_base_ir_value(k);
jah128 0:8a5497a2e366 333 white_active[k] += get_illuminated_base_ir_value(k);
jah128 0:8a5497a2e366 334 }
jah128 0:8a5497a2e366 335 pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1),
jah128 0:8a5497a2e366 336 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 0:8a5497a2e366 337 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 0:8a5497a2e366 338 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 0:8a5497a2e366 339 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 0:8a5497a2e366 340 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 0:8a5497a2e366 341 }
jah128 0:8a5497a2e366 342 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 343 white_background[k]/=5;
jah128 0:8a5497a2e366 344 white_active[k]/=5;
jah128 0:8a5497a2e366 345 }
jah128 0:8a5497a2e366 346 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 0:8a5497a2e366 347 white_background[0], white_active[0],
jah128 0:8a5497a2e366 348 white_background[1], white_active[1],
jah128 0:8a5497a2e366 349 white_background[2], white_active[2],
jah128 0:8a5497a2e366 350 white_background[3], white_active[3],
jah128 0:8a5497a2e366 351 white_background[4], white_active[4]);
jah128 0:8a5497a2e366 352
jah128 0:8a5497a2e366 353 display.clear_display();
jah128 0:8a5497a2e366 354 display.write_string("Place robot on");
jah128 0:8a5497a2e366 355 display.set_position(1,0);
jah128 0:8a5497a2e366 356 display.write_string("black surface");
jah128 0:8a5497a2e366 357 wait(3);
jah128 0:8a5497a2e366 358
jah128 0:8a5497a2e366 359 display.clear_display();
jah128 0:8a5497a2e366 360 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 361 display.set_position(1,0);
jah128 0:8a5497a2e366 362 display.write_string("IR sensor");
jah128 0:8a5497a2e366 363 wait(0.5);
jah128 0:8a5497a2e366 364 pc.printf("\nBlack Background Results:\n");
jah128 0:8a5497a2e366 365
jah128 0:8a5497a2e366 366 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 367 wait(0.2);
jah128 0:8a5497a2e366 368
jah128 0:8a5497a2e366 369 store_background_base_ir_values();
jah128 0:8a5497a2e366 370 display.set_position(1,9);
jah128 0:8a5497a2e366 371 display.write_string(".");
jah128 0:8a5497a2e366 372 wait(0.2);
jah128 0:8a5497a2e366 373 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 374 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 375 black_background[k]+= get_background_base_ir_value(k);
jah128 0:8a5497a2e366 376 black_active[k] += get_illuminated_base_ir_value(k);
jah128 0:8a5497a2e366 377 }
jah128 0:8a5497a2e366 378 pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1),
jah128 0:8a5497a2e366 379 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 0:8a5497a2e366 380 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 0:8a5497a2e366 381 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 0:8a5497a2e366 382 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 0:8a5497a2e366 383 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 0:8a5497a2e366 384 }
jah128 0:8a5497a2e366 385 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 386 black_background[k]/=5;
jah128 0:8a5497a2e366 387 black_active[k]/=5;
jah128 0:8a5497a2e366 388 }
jah128 0:8a5497a2e366 389 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 0:8a5497a2e366 390 black_background[0], black_active[0],
jah128 0:8a5497a2e366 391 black_background[1], black_active[1],
jah128 0:8a5497a2e366 392 black_background[2], black_active[2],
jah128 0:8a5497a2e366 393 black_background[3], black_active[3],
jah128 0:8a5497a2e366 394 black_background[4], black_active[4]);
jah128 0:8a5497a2e366 395
jah128 0:8a5497a2e366 396 }