Library for the PsiSwarm Robot for Headstart Lab - Version 0.5
Fork of PsiSwarmLibrary by
Diff: sensors.cpp
- Revision:
- 1:060690a934a9
- Parent:
- 0:d6269d17c8cf
- Child:
- 2:c6986ee3c7c5
--- a/sensors.cpp Thu Feb 04 21:48:54 2016 +0000 +++ b/sensors.cpp Thu Mar 03 23:21:47 2016 +0000 @@ -1,5 +1,5 @@ /* University of York Robotics Laboratory PsiSwarm Library: Sensor Functions Source File - * + * * File: sensors.cpp * * (C) Dept. Electronics & Computer Science, University of York @@ -21,109 +21,133 @@ // The ultrasonic sensor needs a start command to be sent: this is done by calling update_ultrasonic_measure(). // It can be set to automatically refresh at 10Hz by called enable_ultrasonic_ticker and disable with disabled_ultrasonic_ticker -void enable_ultrasonic_ticker(){ - ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000); +void enable_ultrasonic_ticker() +{ + ultrasonic_ticker.attach_us(update_ultrasonic_measure,100000); } -void disable_ultrasonic_ticker(){ - ultrasonic_ticker.detach(); +void disable_ultrasonic_ticker() +{ + ultrasonic_ticker.detach(); } -void update_ultrasonic_measure(){ - if(waiting_for_ultrasonic == 0){ +void update_ultrasonic_measure() +{ + if(waiting_for_ultrasonic == 0) { waiting_for_ultrasonic = 1; - - char command[2]; - command[0] = 0x00; // Set the command register + if(has_ultrasonic_sensor) { + char command[2]; + command[0] = 0x00; // Set the command register command[1] = 0x51; // Get result is centimeters - primary_i2c.write(ULTRASONIC_ADDRESS, command, 2); // Send the command to start a ranging burst - - ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000); - }else{ - debug("WARNING: Ultrasonic sensor called too frequently.\n"); + primary_i2c.write(ULTRASONIC_ADDRESS, command, 2); // Send the command to start a ranging burst + } + ultrasonic_timeout.attach_us(IF_read_ultrasonic_measure,70000); + } else { + debug("WARNING: Ultrasonic sensor called too frequently.\n"); } } -void IF_read_ultrasonic_measure(){ - char command[1]; - char result[2]; - command[0] = 0x02; // The start address of measure result - primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1); // Send address to read a measure - primary_i2c.read(ULTRASONIC_ADDRESS, result, 2); // Read two byte of measure - ultrasonic_distance = (result[0]<<8)+result[1]; +void IF_read_ultrasonic_measure() +{ + if(has_ultrasonic_sensor) { + char command[1]; + char result[2]; + command[0] = 0x02; // The start address of measure result + primary_i2c.write(ULTRASONIC_ADDRESS, command, 1, 1); // Send address to read a measure + primary_i2c.read(ULTRASONIC_ADDRESS, result, 2); // Read two byte of measure + ultrasonic_distance = (result[0]<<8)+result[1]; + } else ultrasonic_distance = 0; ultrasonic_distance_updated = 1; waiting_for_ultrasonic = 0; - debug("US:%d cm\n",ultrasonic_distance); + //debug("US:%d cm\n",ultrasonic_distance); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Additional Sensing Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -float get_temperature(){ - return IF_read_from_temperature_sensor(); +float get_temperature() +{ + if(has_temperature_sensor)return IF_read_from_temperature_sensor(); + return 0; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Voltage Sensing Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -float get_battery_voltage (){ +float get_battery_voltage () +{ float raw_value = vin_battery.read(); return raw_value * 4.95; } -float get_current (){ +float get_current () +{ // Device uses a INA211 current sense monitor measuring voltage drop across a 2mOhm resistor // Device gain = 500 float raw_value = vin_current.read(); return raw_value * 3.3; } -float get_dc_voltage (){ +float get_dc_voltage () +{ float raw_value = vin_dc.read(); - return raw_value * 6.6; + return raw_value * 6.6; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // IR Sensor Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7). +// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7). // The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found. // NB This function is preserved for code-compatability and cases where only a single reading is needed // In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance() -float read_reflected_ir_distance ( char index ){ +float read_reflected_ir_distance ( char index ) +{ // Sanity check if(index>7) return 0.0; - + //1. Read the ADC value without IR emitter on; store in the background_ir_values[] array background_ir_values [index] = IF_read_IR_adc_value(1, index ); - + //2. Enable the relevant IR emitter by turning on its pulse output - switch(index){ - case 0: case 1: case 6: case 7: - IF_set_IR_emitter_output(0, 1); - break; - case 2: case 3: case 4: case 5: - IF_set_IR_emitter_output(1, 1); - break; + switch(index) { + case 0: + case 1: + case 6: + case 7: + IF_set_IR_emitter_output(0, 1); + break; + case 2: + case 3: + case 4: + case 5: + IF_set_IR_emitter_output(1, 1); + break; } - wait_us(ir_pulse_delay); - + wait_us(ir_pulse_delay); + //3. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array illuminated_ir_values [index] = IF_read_IR_adc_value (1, index ); - + //4. Turn off IR emitter - switch(index){ - case 0: case 1: case 6: case 7: - IF_set_IR_emitter_output(0, 0); - break; - case 2: case 3: case 4: case 5: - IF_set_IR_emitter_output(1, 0); - break; + switch(index) { + case 0: + case 1: + case 6: + case 7: + IF_set_IR_emitter_output(0, 0); + break; + case 2: + case 3: + case 4: + case 5: + IF_set_IR_emitter_output(1, 0); + break; } - + //5. Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances() reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]); ir_values_stored = 1; @@ -132,109 +156,130 @@ // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances -float get_reflected_ir_distance ( char index ){ +float get_reflected_ir_distance ( char index ) +{ // Sanity check: check range of index and that values have been stored if (index>7 || ir_values_stored == 0) return 0.0; - return reflected_ir_distances[index]; + return reflected_ir_distances[index]; } // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values -unsigned short get_background_raw_ir_value ( char index ){ - // Sanity check: check range of index and that values have been stored - if (index>7 || ir_values_stored == 0) return 0.0; - return background_ir_values[index]; -} - -// Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values -unsigned short get_illuminated_raw_ir_value ( char index ){ +unsigned short get_background_raw_ir_value ( char index ) +{ // Sanity check: check range of index and that values have been stored if (index>7 || ir_values_stored == 0) return 0.0; - return illuminated_ir_values[index]; + return background_ir_values[index]; } - + +// Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values +unsigned short get_illuminated_raw_ir_value ( char index ) +{ + // Sanity check: check range of index and that values have been stored + if (index>7 || ir_values_stored == 0) return 0.0; + return illuminated_ir_values[index]; +} + // Stores the reflected distances for all 8 IR sensors -void store_reflected_ir_distances ( void ){ +void store_reflected_ir_distances ( void ) +{ store_ir_values(); - for(int i=0;i<8;i++){ - reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]); - } + for(int i=0; i<8; i++) { + reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]); + } } // Stores the rbackground and illuminated values for all 8 IR sensors -void store_ir_values ( void ){ +void store_ir_values ( void ) +{ store_background_raw_ir_values(); store_illuminated_raw_ir_values(); } - + // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters -void store_background_raw_ir_values ( void ){ +void store_background_raw_ir_values ( void ) +{ ir_values_stored = 1; - for(int i=0;i<8;i++){ + for(int i=0; i<8; i++) { background_ir_values [i] = IF_read_IR_adc_value(1,i); } } - + // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse -void store_illuminated_raw_ir_values ( void ){ - //1. Enable the front IR emitters and store the values +void store_illuminated_raw_ir_values ( void ) +{ + //1. Enable the front IR emitters and store the values IF_set_IR_emitter_output(0, 1); - wait_us(ir_pulse_delay); + wait_us(ir_pulse_delay); illuminated_ir_values [0] = IF_read_IR_adc_value(1,0); illuminated_ir_values [1] = IF_read_IR_adc_value(1,1); illuminated_ir_values [6] = IF_read_IR_adc_value(1,6); illuminated_ir_values [7] = IF_read_IR_adc_value(1,7); IF_set_IR_emitter_output(0, 0); - - //2. Enable the rear+side IR emitters and store the values + + //2. Enable the rear+side IR emitters and store the values IF_set_IR_emitter_output(1, 1); - wait_us(ir_pulse_delay); + wait_us(ir_pulse_delay); illuminated_ir_values [2] = IF_read_IR_adc_value(1,2); illuminated_ir_values [3] = IF_read_IR_adc_value(1,3); illuminated_ir_values [4] = IF_read_IR_adc_value(1,4); illuminated_ir_values [5] = IF_read_IR_adc_value(1,5); IF_set_IR_emitter_output(1, 0); } - + // Converts a background and illuminated value into a distance -float calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ){ +float calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ) +{ float approximate_distance = 4000 + background_value - illuminated_value; if(approximate_distance < 0) approximate_distance = 0; - + // Very approximate: root value, divide by 2, approx distance in mm approximate_distance = sqrt (approximate_distance) / 2.0; - + // Then add adjustment value if value >27 if(approximate_distance > 27) { float shift = pow(approximate_distance - 27,3); approximate_distance += shift; } if(approximate_distance > 90) approximate_distance = 100.0; - return approximate_distance; + return approximate_distance; } // Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse -unsigned short read_illuminated_raw_ir_value ( char index ){ +unsigned short read_illuminated_raw_ir_value ( char index ) +{ //This function reads the IR strength when illuminated - used for PC system debugging purposes //1. Enable the relevant IR emitter by turning on its pulse output - switch(index){ - case 0: case 1: case 6: case 7: - IF_set_IR_emitter_output(0, 1); - break; - case 2: case 3: case 4: case 5: - IF_set_IR_emitter_output(1, 1); - break; + switch(index) { + case 0: + case 1: + case 6: + case 7: + IF_set_IR_emitter_output(0, 1); + break; + case 2: + case 3: + case 4: + case 5: + IF_set_IR_emitter_output(1, 1); + break; } - wait_us(ir_pulse_delay); + wait_us(ir_pulse_delay); //2. Read the ADC value now IR emitter is on unsigned short strong_value = IF_read_IR_adc_value( 1,index ); //3. Turn off IR emitter - switch(index){ - case 0: case 1: case 6: case 7: - IF_set_IR_emitter_output(0, 0); - break; - case 2: case 3: case 4: case 5: - IF_set_IR_emitter_output(1, 0); - break; + switch(index) { + case 0: + case 1: + case 6: + case 7: + IF_set_IR_emitter_output(0, 0); + break; + case 2: + case 3: + case 4: + case 5: + IF_set_IR_emitter_output(1, 0); + break; } return strong_value; } @@ -243,50 +288,56 @@ // Returns the stored value of the non-illuminated sensor based on last call of store_background_base_ir_values -unsigned short get_background_base_ir_value ( char index ){ +unsigned short get_background_base_ir_value ( char index ) +{ // Sanity check: check range of index and that values have been stored if (index>4 || base_ir_values_stored == 0) return 0.0; - return background_base_ir_values[index]; + return background_base_ir_values[index]; } - + // Returns the stored value of the illuminated sensor based on last call of store_illuminated_base_ir_values -unsigned short get_illuminated_base_ir_value ( char index ){ +unsigned short get_illuminated_base_ir_value ( char index ) +{ // Sanity check: check range of index and that values have been stored if (index>4 || base_ir_values_stored == 0) return 0.0; - return illuminated_base_ir_values[index]; + return illuminated_base_ir_values[index]; } - + // Stores the reflected distances for all 5 base IR sensors -void store_base_ir_values ( void ){ +void store_base_ir_values ( void ) +{ store_background_base_ir_values(); store_illuminated_base_ir_values(); //for(int i=0;i<5;i++){ - // reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]); - //} + // reflected_ir_distances [i] = calculate_reflected_distance( background_base_ir_values [i], illuminated_base_ir_values [i]); + //} } - + // Stores the raw ADC values for all 5 base IR sensors without enabling IR emitters -void store_background_base_ir_values ( void ){ +void store_background_base_ir_values ( void ) +{ base_ir_values_stored = 1; - for(int i=0;i<5;i++){ + for(int i=0; i<5; i++) { background_base_ir_values [i] = IF_read_IR_adc_value(2,i); } } - + // Stores the raw ADC values for all 5 base IR sensors with a 500us emitter pulse -void store_illuminated_base_ir_values ( void ){ - //1. Enable the base IR emitters and store the values +void store_illuminated_base_ir_values ( void ) +{ + //1. Enable the base IR emitters and store the values IF_set_IR_emitter_output(2, 1); - wait_us(base_ir_pulse_delay); - for(int i=0;i<5;i++){ - illuminated_base_ir_values [i] = IF_read_IR_adc_value(2,i); + wait_us(base_ir_pulse_delay); + for(int i=0; i<5; i++) { + illuminated_base_ir_values [i] = IF_read_IR_adc_value(2,i); } - + IF_set_IR_emitter_output(2, 0); } - + // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm -void store_line_position ( ){ +void store_line_position ( ) +{ // Store background and reflected base IR values store_base_ir_values(); int h_value[5]; @@ -295,49 +346,49 @@ char count = 0; line_found = 0; line_position = 0; - for(int i=0;i<5;i++){ + for(int i=0; i<5; i++) { if(get_background_base_ir_value(i) > get_illuminated_base_ir_value(i)) h_value[i]=0; else h_value[i] = get_illuminated_base_ir_value(i) - get_background_base_ir_value(i); if(h_value[i] < line_threshold) count++; - } - if(count == 1){ - line_found = 1; - if(h_value[0] < line_threshold) { - line_position = -1; - if(h_value[1] < line_threshold_hi) line_position = -0.8; - } - - if (h_value[1] < line_threshold) { - line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);; - } - if(h_value[2] < line_threshold) { + } + if(count == 1) { + line_found = 1; + if(h_value[0] < line_threshold) { + line_position = -1; + if(h_value[1] < line_threshold_hi) line_position = -0.8; + } + + if (h_value[1] < line_threshold) { + line_position = -0.5 + (0.00005 * h_value[0]) - (0.0001 * h_value[2]);; + } + if(h_value[2] < line_threshold) { line_position = (0.00005 * h_value[1]) - (0.0001 * h_value[3]); - } - if(h_value[3] < line_threshold) { - line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);; - } - if(h_value[4] < line_threshold) { - line_position = 1; - if(h_value[3] < line_threshold_hi) line_position = 0.8; - } + } + if(h_value[3] < line_threshold) { + line_position = 0.5 + (0.00005 * h_value[2]) - (0.0001 * h_value[4]);; + } + if(h_value[4] < line_threshold) { + line_position = 1; + if(h_value[3] < line_threshold_hi) line_position = 0.8; + } } - if(count == 2){ - if(h_value[0] && h_value[1] < line_threshold){ + if(count == 2) { + if(h_value[0] && h_value[1] < line_threshold) { line_found = 1; line_position = -0.6; } - - if(h_value[1] && h_value[2] < line_threshold){ + + if(h_value[1] && h_value[2] < line_threshold) { line_found = 1; line_position = -0.4; } - - if(h_value[2] && h_value[3] < line_threshold){ + + if(h_value[2] && h_value[3] < line_threshold) { line_found = 1; line_position = 0.4; } - - if(h_value[3] && h_value[4] < line_threshold){ + + if(h_value[3] && h_value[4] < line_threshold) { line_found = 1; line_position = 0.6; } @@ -346,17 +397,18 @@ -void calibrate_base_ir_sensors (void) { +void calibrate_base_ir_sensors (void) +{ short white_background[5]; short white_active[5]; short black_background[5]; short black_active[5]; - for(int k=0;k<5;k++){ + for(int k=0; k<5; k++) { white_background[k]=0; black_background[k]=0; white_active[k]=0; - black_active[k]=0; - } + black_active[k]=0; + } pc.printf("Base IR Calibration\n"); display.clear_display(); display.write_string("Calibrating base"); @@ -374,107 +426,107 @@ display.write_string("IR sensor"); wait(0.5); pc.printf("\nWhite Background Results:\n"); - - for(int i=0;i<5;i++){ - wait(0.2); - store_background_base_ir_values(); - - display.set_position(1,9); - display.write_string("."); - wait(0.2); - store_illuminated_base_ir_values(); - for(int k=0;k<5;k++){ - white_background[k]+= get_background_base_ir_value(k); - white_active[k] += get_illuminated_base_ir_value(k); - } - pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1), - get_background_base_ir_value(0), get_illuminated_base_ir_value(0), - get_background_base_ir_value(1), get_illuminated_base_ir_value(1), - get_background_base_ir_value(2), get_illuminated_base_ir_value(2), - get_background_base_ir_value(3), get_illuminated_base_ir_value(3), - get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); + + for(int i=0; i<5; i++) { + wait(0.2); + store_background_base_ir_values(); + + display.set_position(1,9); + display.write_string("."); + wait(0.2); + store_illuminated_base_ir_values(); + for(int k=0; k<5; k++) { + white_background[k]+= get_background_base_ir_value(k); + white_active[k] += get_illuminated_base_ir_value(k); + } + pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1), + get_background_base_ir_value(0), get_illuminated_base_ir_value(0), + get_background_base_ir_value(1), get_illuminated_base_ir_value(1), + get_background_base_ir_value(2), get_illuminated_base_ir_value(2), + get_background_base_ir_value(3), get_illuminated_base_ir_value(3), + get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); } - for(int k=0;k<5;k++){ + for(int k=0; k<5; k++) { white_background[k]/=5; white_active[k]/=5; } pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", - white_background[0], white_active[0], - white_background[1], white_active[1], - white_background[2], white_active[2], - white_background[3], white_active[3], - white_background[4], white_active[4]); - + white_background[0], white_active[0], + white_background[1], white_active[1], + white_background[2], white_active[2], + white_background[3], white_active[3], + white_background[4], white_active[4]); + display.clear_display(); display.write_string("Place robot on"); display.set_position(1,0); display.write_string("black surface"); - wait(3); - + wait(3); + display.clear_display(); display.write_string("Calibrating base"); display.set_position(1,0); display.write_string("IR sensor"); wait(0.5); pc.printf("\nBlack Background Results:\n"); - - for(int i=0;i<5;i++){ - wait(0.2); + + for(int i=0; i<5; i++) { + wait(0.2); - store_background_base_ir_values(); - display.set_position(1,9); - display.write_string("."); - wait(0.2); - store_illuminated_base_ir_values(); - for(int k=0;k<5;k++){ - black_background[k]+= get_background_base_ir_value(k); - black_active[k] += get_illuminated_base_ir_value(k); - } - pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1), - get_background_base_ir_value(0), get_illuminated_base_ir_value(0), - get_background_base_ir_value(1), get_illuminated_base_ir_value(1), - get_background_base_ir_value(2), get_illuminated_base_ir_value(2), - get_background_base_ir_value(3), get_illuminated_base_ir_value(3), - get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); - } - for(int k=0;k<5;k++){ + store_background_base_ir_values(); + display.set_position(1,9); + display.write_string("."); + wait(0.2); + store_illuminated_base_ir_values(); + for(int k=0; k<5; k++) { + black_background[k]+= get_background_base_ir_value(k); + black_active[k] += get_illuminated_base_ir_value(k); + } + pc.printf("Sample %d 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", (i+1), + get_background_base_ir_value(0), get_illuminated_base_ir_value(0), + get_background_base_ir_value(1), get_illuminated_base_ir_value(1), + get_background_base_ir_value(2), get_illuminated_base_ir_value(2), + get_background_base_ir_value(3), get_illuminated_base_ir_value(3), + get_background_base_ir_value(4), get_illuminated_base_ir_value(4)); + } + for(int k=0; k<5; k++) { black_background[k]/=5; black_active[k]/=5; } - pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", - black_background[0], black_active[0], - black_background[1], black_active[1], - black_background[2], black_active[2], - black_background[3], black_active[3], - black_background[4], black_active[4]); - + pc.printf("Mean results 1: %04d-%04d 2: %04d-%04d 3: %04d-%04d 4: %04d-%04d 5: %04d-%04d\n", + black_background[0], black_active[0], + black_background[1], black_active[1], + black_background[2], black_active[2], + black_background[3], black_active[3], + black_background[4], black_active[4]); + } -int get_bearing_from_ir_array (unsigned short * ir_sensor_readings){ +int get_bearing_from_ir_array (unsigned short * ir_sensor_readings) +{ //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]); - + float degrees_per_radian = 57.295779513; - + // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432}; float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533}; - + float sin_sum = 0; float cos_sum = 0; - - for(int i = 0; i < 8; i++) - { + + for(int i = 0; i < 8; i++) { // Use IR sensor reading to weight bearing vector sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i]; cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i]; } - - float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source + + float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source bearing *= degrees_per_radian; // Convert to degrees //out("Sin sum:%f Cos sum:%f Bearing:%f\n",sin_sum,cos_sum,bearing); return (int) bearing; -} +}