UKESF Headstart Summer School
Library for the PsiSwarm Robot for Headstart Lab - Version 0.5
Fork of
PsiSwarmLibrary
by 
James Hilder
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;
-}
+}