Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
Diff: PsiSwarm/sensors.cpp
- Revision:
- 3:cd048f6e544e
- Parent:
- 2:a6214fd156ff
- Child:
- 6:ff3c66f7372b
diff -r a6214fd156ff -r cd048f6e544e PsiSwarm/sensors.cpp --- a/PsiSwarm/sensors.cpp Mon Oct 05 14:42:16 2015 +0000 +++ b/PsiSwarm/sensors.cpp Mon Oct 05 20:42:37 2015 +0000 @@ -284,13 +284,66 @@ } // Routine to store detected line position in a similar format to the used on 3Pi\m3Pi\PiSwarm -void store_line_position ( void ){ +void store_line_position ( ){ // Store background and reflected base IR values store_base_ir_values(); - + int h_value[5]; + int line_threshold = 1000; + int line_threshold_hi = 2000; + char count = 0; + line_found = 0; + line_position = 0; + 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) { + 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(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){ + line_found = 1; + line_position = -0.4; + } + + 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){ + line_found = 1; + line_position = 0.6; + } + } } + void calibrate_base_ir_sensors (void) { short white_background[5]; short white_active[5];