Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

PsiSwarm/sensors.cpp@3:cd048f6e544e, 2015-10-05 (annotated)

Committer:
jah128
Date:
Mon Oct 05 20:42:37 2015 +0000
Revision:
3:cd048f6e544e
Parent:
2:a6214fd156ff
Child:
6:ff3c66f7372b
Added crude line following code

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 3:cd048f6e544e 287 void store_line_position ( ){
jah128 2:a6214fd156ff 288 // Store background and reflected base IR values
jah128 2:a6214fd156ff 289 store_base_ir_values();
jah128 3:cd048f6e544e 290 int h_value[5];
jah128 3:cd048f6e544e 291 int line_threshold = 1000;
jah128 3:cd048f6e544e 292 int line_threshold_hi = 2000;
jah128 3:cd048f6e544e 293 char count = 0;
jah128 3:cd048f6e544e 294 line_found = 0;
jah128 3:cd048f6e544e 295 line_position = 0;
jah128 3:cd048f6e544e 296 for(int i=0;i<5;i++){
jah128 3:cd048f6e544e 297 if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0;
jah128 3:cd048f6e544e 298 else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i);
jah128 3:cd048f6e544e 299 if(h_value[i] < line_threshold) count++;
jah128 3:cd048f6e544e 300 }
jah128 3:cd048f6e544e 301 if(count == 1){
jah128 3:cd048f6e544e 302 line_found = 1;
jah128 3:cd048f6e544e 303 if(h_value[0] < line_threshold) {
jah128 3:cd048f6e544e 304 line_position = -1;
jah128 3:cd048f6e544e 305 if(h_value[1] < line_threshold_hi) line_position = -0.8;
jah128 3:cd048f6e544e 306 }
jah128 3:cd048f6e544e 307
jah128 3:cd048f6e544e 308 if (h_value[1] < line_threshold) {
jah128 3:cd048f6e544e 309 line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);;
jah128 3:cd048f6e544e 310 }
jah128 3:cd048f6e544e 311 if(h_value[2] < line_threshold) {
jah128 3:cd048f6e544e 312 line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]);
jah128 3:cd048f6e544e 313 }
jah128 3:cd048f6e544e 314 if(h_value[3] < line_threshold) {
jah128 3:cd048f6e544e 315 line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);;
jah128 3:cd048f6e544e 316 }
jah128 3:cd048f6e544e 317 if(h_value[4] < line_threshold) {
jah128 3:cd048f6e544e 318 line_position = 1;
jah128 3:cd048f6e544e 319 if(h_value[3] < line_threshold_hi) line_position = 0.8;
jah128 3:cd048f6e544e 320 }
jah128 3:cd048f6e544e 321 }
jah128 3:cd048f6e544e 322 if(count == 2){
jah128 3:cd048f6e544e 323 if(h_value[0] && h_value[1] < line_threshold){
jah128 3:cd048f6e544e 324 line_found = 1;
jah128 3:cd048f6e544e 325 line_position = -0.6;
jah128 3:cd048f6e544e 326 }
jah128 3:cd048f6e544e 327
jah128 3:cd048f6e544e 328 if(h_value[1] && h_value[2] < line_threshold){
jah128 3:cd048f6e544e 329 line_found = 1;
jah128 3:cd048f6e544e 330 line_position = -0.4;
jah128 3:cd048f6e544e 331 }
jah128 3:cd048f6e544e 332
jah128 3:cd048f6e544e 333 if(h_value[2] && h_value[3] < line_threshold){
jah128 3:cd048f6e544e 334 line_found = 1;
jah128 3:cd048f6e544e 335 line_position = 0.4;
jah128 3:cd048f6e544e 336 }
jah128 3:cd048f6e544e 337
jah128 3:cd048f6e544e 338 if(h_value[3] && h_value[4] < line_threshold){
jah128 3:cd048f6e544e 339 line_found = 1;
jah128 3:cd048f6e544e 340 line_position = 0.6;
jah128 3:cd048f6e544e 341 }
jah128 3:cd048f6e544e 342 }
jah128 2:a6214fd156ff 343 }
jah128 2:a6214fd156ff 344
jah128 2:a6214fd156ff 345
jah128 3:cd048f6e544e 346
jah128 0:8a5497a2e366 347 void calibrate_base_ir_sensors (void) {
jah128 0:8a5497a2e366 348 short white_background[5];
jah128 0:8a5497a2e366 349 short white_active[5];
jah128 0:8a5497a2e366 350 short black_background[5];
jah128 0:8a5497a2e366 351 short black_active[5];
jah128 0:8a5497a2e366 352 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 353 white_background[k]=0;
jah128 0:8a5497a2e366 354 black_background[k]=0;
jah128 0:8a5497a2e366 355 white_active[k]=0;
jah128 0:8a5497a2e366 356 black_active[k]=0;
jah128 0:8a5497a2e366 357 }
jah128 0:8a5497a2e366 358 pc.printf("Base IR Calibration\n");
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 display.clear_display();
jah128 0:8a5497a2e366 365 display.write_string("Place robot on");
jah128 0:8a5497a2e366 366 display.set_position(1,0);
jah128 0:8a5497a2e366 367 display.write_string("white surface");
jah128 0:8a5497a2e366 368 wait(3);
jah128 0:8a5497a2e366 369 display.clear_display();
jah128 0:8a5497a2e366 370 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 371 display.set_position(1,0);
jah128 0:8a5497a2e366 372 display.write_string("IR sensor");
jah128 0:8a5497a2e366 373 wait(0.5);
jah128 0:8a5497a2e366 374 pc.printf("\nWhite Background Results:\n");
jah128 0:8a5497a2e366 375
jah128 0:8a5497a2e366 376 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 377 wait(0.2);
jah128 0:8a5497a2e366 378 store_background_base_ir_values();
jah128 0:8a5497a2e366 379
jah128 0:8a5497a2e366 380 display.set_position(1,9);
jah128 0:8a5497a2e366 381 display.write_string(".");
jah128 0:8a5497a2e366 382 wait(0.2);
jah128 0:8a5497a2e366 383 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 384 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 385 white_background[k]+= get_background_base_ir_value(k);
jah128 0:8a5497a2e366 386 white_active[k] += get_illuminated_base_ir_value(k);
jah128 0:8a5497a2e366 387 }
jah128 0:8a5497a2e366 388 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 389 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 0:8a5497a2e366 390 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 0:8a5497a2e366 391 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 0:8a5497a2e366 392 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 0:8a5497a2e366 393 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 0:8a5497a2e366 394 }
jah128 0:8a5497a2e366 395 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 396 white_background[k]/=5;
jah128 0:8a5497a2e366 397 white_active[k]/=5;
jah128 0:8a5497a2e366 398 }
jah128 0:8a5497a2e366 399 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 0:8a5497a2e366 400 white_background[0], white_active[0],
jah128 0:8a5497a2e366 401 white_background[1], white_active[1],
jah128 0:8a5497a2e366 402 white_background[2], white_active[2],
jah128 0:8a5497a2e366 403 white_background[3], white_active[3],
jah128 0:8a5497a2e366 404 white_background[4], white_active[4]);
jah128 0:8a5497a2e366 405
jah128 0:8a5497a2e366 406 display.clear_display();
jah128 0:8a5497a2e366 407 display.write_string("Place robot on");
jah128 0:8a5497a2e366 408 display.set_position(1,0);
jah128 0:8a5497a2e366 409 display.write_string("black surface");
jah128 0:8a5497a2e366 410 wait(3);
jah128 0:8a5497a2e366 411
jah128 0:8a5497a2e366 412 display.clear_display();
jah128 0:8a5497a2e366 413 display.write_string("Calibrating base");
jah128 0:8a5497a2e366 414 display.set_position(1,0);
jah128 0:8a5497a2e366 415 display.write_string("IR sensor");
jah128 0:8a5497a2e366 416 wait(0.5);
jah128 0:8a5497a2e366 417 pc.printf("\nBlack Background Results:\n");
jah128 0:8a5497a2e366 418
jah128 0:8a5497a2e366 419 for(int i=0;i<5;i++){
jah128 0:8a5497a2e366 420 wait(0.2);
jah128 0:8a5497a2e366 421
jah128 0:8a5497a2e366 422 store_background_base_ir_values();
jah128 0:8a5497a2e366 423 display.set_position(1,9);
jah128 0:8a5497a2e366 424 display.write_string(".");
jah128 0:8a5497a2e366 425 wait(0.2);
jah128 0:8a5497a2e366 426 store_illuminated_base_ir_values();
jah128 0:8a5497a2e366 427 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 428 black_background[k]+= get_background_base_ir_value(k);
jah128 0:8a5497a2e366 429 black_active[k] += get_illuminated_base_ir_value(k);
jah128 0:8a5497a2e366 430 }
jah128 0:8a5497a2e366 431 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 432 get_background_base_ir_value(0), get_illuminated_base_ir_value(0),
jah128 0:8a5497a2e366 433 get_background_base_ir_value(1), get_illuminated_base_ir_value(1),
jah128 0:8a5497a2e366 434 get_background_base_ir_value(2), get_illuminated_base_ir_value(2),
jah128 0:8a5497a2e366 435 get_background_base_ir_value(3), get_illuminated_base_ir_value(3),
jah128 0:8a5497a2e366 436 get_background_base_ir_value(4), get_illuminated_base_ir_value(4));
jah128 0:8a5497a2e366 437 }
jah128 0:8a5497a2e366 438 for(int k=0;k<5;k++){
jah128 0:8a5497a2e366 439 black_background[k]/=5;
jah128 0:8a5497a2e366 440 black_active[k]/=5;
jah128 0:8a5497a2e366 441 }
jah128 0:8a5497a2e366 442 pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n",
jah128 0:8a5497a2e366 443 black_background[0], black_active[0],
jah128 0:8a5497a2e366 444 black_background[1], black_active[1],
jah128 0:8a5497a2e366 445 black_background[2], black_active[2],
jah128 0:8a5497a2e366 446 black_background[3], black_active[3],
jah128 0:8a5497a2e366 447 black_background[4], black_active[4]);
jah128 0:8a5497a2e366 448
jah128 0:8a5497a2e366 449 }