Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
Diff: communications.cpp
- Revision:
- 3:4c0f2f3de33e
- Parent:
- 2:0a739218ab11
- Child:
- 4:52b3e4c5a425
--- a/communications.cpp Sun Feb 02 20:37:48 2014 +0000 +++ b/communications.cpp Sun Feb 02 21:18:05 2014 +0000 @@ -410,15 +410,47 @@ response[1] = sgyro % 256; break; } - case 10: // Request background IR reading + case 10: { // Request background IR reading + response_length = 16; + response = new char[16]; + for(int sensor = 0; sensor < 8; sensor ++){ + int offset = sensor * 2; + unsigned short m_val = piswarm.read_adc_value(sensor); + response[offset]=m_val >> 8; + response[offset+1]=m_val % 256; + } break; - case 11: // Request illuminated IR reading + } + case 11: { // Request illuminated IR reading + response_length = 16; + response = new char[16]; + for(int sensor = 0; sensor < 8; sensor ++){ + int offset = sensor * 2; + unsigned short m_val = piswarm.read_illuminated_raw_ir_value(sensor); + response[offset]=m_val >> 8; + response[offset+1]=m_val % 256; + } break; - case 12: // Request line-tracking IR reading + } + + case 12: { // Request distance IR reading + response_length = 16; + response = new char[16]; + for(int sensor = 0; sensor < 8; sensor ++){ + int offset = sensor * 2; + float f_val = piswarm.read_reflected_ir_distance(sensor); + //f_val ranges from 0 to 100.0; + f_val *= 655; + unsigned short m_val = (unsigned short) f_val; + response[offset]=m_val >> 8; + response[offset+1]=m_val % 256; + } break; - case 13: // Request uptime + } + + case 13: // Request line-tracking IR reading break; - case 14: // + case 14: // Request uptime break; case 15: // break; @@ -983,6 +1015,11 @@ swarm[i].battery = 0.0; swarm[i].light_sensor = 0.0; swarm[i].gyro = 0.0; + for(int k=0; k<8; k++) { + swarm[i].background_ir[k] = 0; + swarm[i].reflected_ir[k] = 0; + swarm[i].distance_ir[k] = 0; + } } } @@ -1214,6 +1251,12 @@ if(success == 1) { if(length == 16) { swarm[sender].status_rf_request_background_ir = 2; + for(int i=0;i<8;i++){ + int offset = i * 2; + unsigned short value = (unsigned short) data[offset] << 8; + value += data[offset+1]; + swarm[sender].background_ir[i]=value; + } outcome = 1; } else { swarm[sender].status_rf_request_background_ir = 3; @@ -1232,6 +1275,12 @@ if(success == 1) { if(length == 16) { swarm[sender].status_rf_request_reflected_ir = 2; + for(int i=0;i<8;i++){ + int offset = i * 2; + unsigned short value = (unsigned short) data[offset] << 8; + value += data[offset+1]; + swarm[sender].reflected_ir[i]=value; + } outcome = 1; } else { swarm[sender].status_rf_request_reflected_ir = 3; @@ -1250,6 +1299,13 @@ if(success == 1) { if(length == 16) { swarm[sender].status_rf_request_distance_ir = 2; + for(int i=0;i<8;i++){ + int offset = i * 2; + unsigned short value = (unsigned short) data[offset] << 8; + value += data[offset+1]; + float adjusted = (float) value / 655.0; + swarm[sender].distance_ir[i]=adjusted; + } outcome = 1; } else { swarm[sender].status_rf_request_distance_ir = 3;