![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
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?
User | Revision | Line number | New 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 | } |