Psi Swarm Robot / PsiSwarmV7_CPP

C++ Library for the PsiSwarm Robot - Version 0.7

Fork of PsiSwarmV7 by Psi Swarm Robot

sensors.cpp@0:d6269d17c8cf, 2016-02-04 (annotated)

Committer:
jah128
Date:
Thu Feb 04 21:48:54 2016 +0000
Revision:
0:d6269d17c8cf
Child:
1:060690a934a9
PsiSwarm Library Version 0.4 - Initial Commit

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