Radar effect using depth data sent over the network from an Xbox 360 Kinect which controls an LED strip
Dependencies: EthernetInterface LPD8806 mbed-rtos mbed
main.cpp@0:8c8d92233d08, 2016-07-13 (annotated)
- Committer:
- awatt196
- Date:
- Wed Jul 13 16:36:21 2016 +0000
- Revision:
- 0:8c8d92233d08
- Child:
- 1:a5690556741c
Fully working over the network. Option for multi-coloured LEDS. Data is scaled non-linearly
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
awatt196 | 0:8c8d92233d08 | 1 | #include "LPD8806.h" |
awatt196 | 0:8c8d92233d08 | 2 | #include "EthernetInterface.h" |
awatt196 | 0:8c8d92233d08 | 3 | #include <string> |
awatt196 | 0:8c8d92233d08 | 4 | #include <math.h> |
awatt196 | 0:8c8d92233d08 | 5 | |
awatt196 | 0:8c8d92233d08 | 6 | using namespace std; |
awatt196 | 0:8c8d92233d08 | 7 | |
awatt196 | 0:8c8d92233d08 | 8 | #define ECHO_SERVER_PORT 4548 |
awatt196 | 0:8c8d92233d08 | 9 | |
awatt196 | 0:8c8d92233d08 | 10 | LPD8806 strip = LPD8806(32); |
awatt196 | 0:8c8d92233d08 | 11 | |
awatt196 | 0:8c8d92233d08 | 12 | Serial pc(USBTX, USBRX); |
awatt196 | 0:8c8d92233d08 | 13 | |
awatt196 | 0:8c8d92233d08 | 14 | int main() { |
awatt196 | 0:8c8d92233d08 | 15 | EthernetInterface eth; |
awatt196 | 0:8c8d92233d08 | 16 | eth.init(); //Use DHCP |
awatt196 | 0:8c8d92233d08 | 17 | eth.connect(); |
awatt196 | 0:8c8d92233d08 | 18 | pc.printf("\nServer IP Address is %s\n", eth.getIPAddress()); |
awatt196 | 0:8c8d92233d08 | 19 | UDPSocket server; |
awatt196 | 0:8c8d92233d08 | 20 | server.bind(ECHO_SERVER_PORT); |
awatt196 | 0:8c8d92233d08 | 21 | Endpoint client; |
awatt196 | 0:8c8d92233d08 | 22 | char buffer[5000]; //buffer to hold the depth data (640 data points) |
awatt196 | 0:8c8d92233d08 | 23 | |
awatt196 | 0:8c8d92233d08 | 24 | // Start up the LED strip |
awatt196 | 0:8c8d92233d08 | 25 | strip.begin(); |
awatt196 | 0:8c8d92233d08 | 26 | |
awatt196 | 0:8c8d92233d08 | 27 | // Update the strip, to start they are all 'off' |
awatt196 | 0:8c8d92233d08 | 28 | strip.show(); |
awatt196 | 0:8c8d92233d08 | 29 | while (1) { |
awatt196 | 0:8c8d92233d08 | 30 | //printf("\nWaiting for UDP packet...\n"); |
awatt196 | 0:8c8d92233d08 | 31 | int n = server.receiveFrom(client, buffer, sizeof(buffer)); |
awatt196 | 0:8c8d92233d08 | 32 | buffer[n] = '\0'; |
awatt196 | 0:8c8d92233d08 | 33 | |
awatt196 | 0:8c8d92233d08 | 34 | //printf("Received packet from: %s\n", client.get_address()); |
awatt196 | 0:8c8d92233d08 | 35 | //printf("Packet contents : '%s'\n",buffer); |
awatt196 | 0:8c8d92233d08 | 36 | |
awatt196 | 0:8c8d92233d08 | 37 | string data_string = ""; |
awatt196 | 0:8c8d92233d08 | 38 | int rawdata_array[640]; //holds raw int data |
awatt196 | 0:8c8d92233d08 | 39 | int index=0; |
awatt196 | 0:8c8d92233d08 | 40 | |
awatt196 | 0:8c8d92233d08 | 41 | for(int i=0; i<n; i++) |
awatt196 | 0:8c8d92233d08 | 42 | { |
awatt196 | 0:8c8d92233d08 | 43 | if(buffer[i] >= 48 && buffer[i] <= 57) //selects 0-9 |
awatt196 | 0:8c8d92233d08 | 44 | { |
awatt196 | 0:8c8d92233d08 | 45 | data_string+=buffer[i]; |
awatt196 | 0:8c8d92233d08 | 46 | } |
awatt196 | 0:8c8d92233d08 | 47 | else if(buffer[i] == 120) //x = invalid data |
awatt196 | 0:8c8d92233d08 | 48 | { |
awatt196 | 0:8c8d92233d08 | 49 | rawdata_array[index] = -1; |
awatt196 | 0:8c8d92233d08 | 50 | index++; |
awatt196 | 0:8c8d92233d08 | 51 | data_string = ""; //clear string |
awatt196 | 0:8c8d92233d08 | 52 | } |
awatt196 | 0:8c8d92233d08 | 53 | else if(buffer[i] == 44 || buffer[i] == 93) //reached a comma or end bracket |
awatt196 | 0:8c8d92233d08 | 54 | { |
awatt196 | 0:8c8d92233d08 | 55 | if (data_string.length() != 0) |
awatt196 | 0:8c8d92233d08 | 56 | { |
awatt196 | 0:8c8d92233d08 | 57 | int int_data = atoi(data_string.c_str()); |
awatt196 | 0:8c8d92233d08 | 58 | rawdata_array[index] = int_data; |
awatt196 | 0:8c8d92233d08 | 59 | data_string = ""; //clear string |
awatt196 | 0:8c8d92233d08 | 60 | index++; |
awatt196 | 0:8c8d92233d08 | 61 | } |
awatt196 | 0:8c8d92233d08 | 62 | } |
awatt196 | 0:8c8d92233d08 | 63 | else |
awatt196 | 0:8c8d92233d08 | 64 | { |
awatt196 | 0:8c8d92233d08 | 65 | //do nothing |
awatt196 | 0:8c8d92233d08 | 66 | } |
awatt196 | 0:8c8d92233d08 | 67 | } |
awatt196 | 0:8c8d92233d08 | 68 | |
awatt196 | 0:8c8d92233d08 | 69 | int length = 32; //32 LEDs on strip |
awatt196 | 0:8c8d92233d08 | 70 | int m_defaultRange = -1; |
awatt196 | 0:8c8d92233d08 | 71 | int m_minRange = 30; |
awatt196 | 0:8c8d92233d08 | 72 | int m_maxRange = 250; |
awatt196 | 0:8c8d92233d08 | 73 | double m_minData = 127.0; |
awatt196 | 0:8c8d92233d08 | 74 | double m_maxData = 0.0; |
awatt196 | 0:8c8d92233d08 | 75 | double m_defaultData = 0.0; |
awatt196 | 0:8c8d92233d08 | 76 | |
awatt196 | 0:8c8d92233d08 | 77 | double bucketSize = 640 / length; //double bucketSize = values.size() / length(); = 20 |
awatt196 | 0:8c8d92233d08 | 78 | |
awatt196 | 0:8c8d92233d08 | 79 | for(int bucket=0; bucket<length; bucket++) //for(int bucket=0; bucket<length(); bucket++) |
awatt196 | 0:8c8d92233d08 | 80 | { |
awatt196 | 0:8c8d92233d08 | 81 | int bucketStart = bucketSize*bucket; //0-620 = |
awatt196 | 0:8c8d92233d08 | 82 | int bucketEnd = bucketSize*(bucket+1) - 1; |
awatt196 | 0:8c8d92233d08 | 83 | |
awatt196 | 0:8c8d92233d08 | 84 | double accumulator = 0; |
awatt196 | 0:8c8d92233d08 | 85 | int points = 0; |
awatt196 | 0:8c8d92233d08 | 86 | for(int index=bucketStart; index<=bucketEnd; index++) |
awatt196 | 0:8c8d92233d08 | 87 | { |
awatt196 | 0:8c8d92233d08 | 88 | double value = (double)rawdata_array[index]; |
awatt196 | 0:8c8d92233d08 | 89 | //printf("index=%i\tvalue=%f\n", index, value); |
awatt196 | 0:8c8d92233d08 | 90 | if(value != m_defaultRange) |
awatt196 | 0:8c8d92233d08 | 91 | { |
awatt196 | 0:8c8d92233d08 | 92 | accumulator += value; |
awatt196 | 0:8c8d92233d08 | 93 | points++; |
awatt196 | 0:8c8d92233d08 | 94 | } |
awatt196 | 0:8c8d92233d08 | 95 | } |
awatt196 | 0:8c8d92233d08 | 96 | |
awatt196 | 0:8c8d92233d08 | 97 | // Calc average of points or default value if no data |
awatt196 | 0:8c8d92233d08 | 98 | double average = m_defaultRange; |
awatt196 | 0:8c8d92233d08 | 99 | if(points != 0) |
awatt196 | 0:8c8d92233d08 | 100 | { |
awatt196 | 0:8c8d92233d08 | 101 | average = accumulator / points; |
awatt196 | 0:8c8d92233d08 | 102 | } |
awatt196 | 0:8c8d92233d08 | 103 | |
awatt196 | 0:8c8d92233d08 | 104 | int score_array[32]; |
awatt196 | 0:8c8d92233d08 | 105 | double score = m_defaultData; |
awatt196 | 0:8c8d92233d08 | 106 | if ((average != m_defaultRange) && (average >= m_minRange) && (average <= m_maxRange)) |
awatt196 | 0:8c8d92233d08 | 107 | { |
awatt196 | 0:8c8d92233d08 | 108 | score = average * (m_maxData - m_minData) / (m_maxRange - m_minRange) + m_minData; //from 0-127 |
awatt196 | 0:8c8d92233d08 | 109 | if(score > 0) |
awatt196 | 0:8c8d92233d08 | 110 | { |
awatt196 | 0:8c8d92233d08 | 111 | float x_scale = 18.142857; |
awatt196 | 0:8c8d92233d08 | 112 | float x = score/x_scale; |
awatt196 | 0:8c8d92233d08 | 113 | float y = pow(2, x); |
awatt196 | 0:8c8d92233d08 | 114 | if(y==128) y=127; |
awatt196 | 0:8c8d92233d08 | 115 | score_array[bucket] = (int)y; |
awatt196 | 0:8c8d92233d08 | 116 | } |
awatt196 | 0:8c8d92233d08 | 117 | } |
awatt196 | 0:8c8d92233d08 | 118 | else //out of range |
awatt196 | 0:8c8d92233d08 | 119 | { |
awatt196 | 0:8c8d92233d08 | 120 | score = 0; |
awatt196 | 0:8c8d92233d08 | 121 | score_array[bucket] = score; |
awatt196 | 0:8c8d92233d08 | 122 | } |
awatt196 | 0:8c8d92233d08 | 123 | //printf("bucket:%i, score=%i\r\n", bucket, score_array[bucket]); |
awatt196 | 0:8c8d92233d08 | 124 | |
awatt196 | 0:8c8d92233d08 | 125 | /* //for using the full colour spectrum |
awatt196 | 0:8c8d92233d08 | 126 | int cyan_start=33; |
awatt196 | 0:8c8d92233d08 | 127 | int green_start=66; |
awatt196 | 0:8c8d92233d08 | 128 | int yellow_start=99; |
awatt196 | 0:8c8d92233d08 | 129 | |
awatt196 | 0:8c8d92233d08 | 130 | if(score == 0) //off |
awatt196 | 0:8c8d92233d08 | 131 | { |
awatt196 | 0:8c8d92233d08 | 132 | strip.setPixelColor(bucket, strip.Color(0,0,0)); |
awatt196 | 0:8c8d92233d08 | 133 | } |
awatt196 | 0:8c8d92233d08 | 134 | |
awatt196 | 0:8c8d92233d08 | 135 | else if(score >=1 && score <cyan_start) //blue to cyan |
awatt196 | 0:8c8d92233d08 | 136 | { |
awatt196 | 0:8c8d92233d08 | 137 | strip.setPixelColor(bucket, strip.Color(0,(int)(128.0/cyan_start),127)); |
awatt196 | 0:8c8d92233d08 | 138 | } |
awatt196 | 0:8c8d92233d08 | 139 | else if(score >=cyan_start && score <green_start) //cyan to green |
awatt196 | 0:8c8d92233d08 | 140 | { |
awatt196 | 0:8c8d92233d08 | 141 | strip.setPixelColor(bucket, strip.Color(0,127,(int)(128.0/score-cyan_start))); |
awatt196 | 0:8c8d92233d08 | 142 | } |
awatt196 | 0:8c8d92233d08 | 143 | else if(score >=green_start && score <=yellow_start) //green to yellow |
awatt196 | 0:8c8d92233d08 | 144 | { |
awatt196 | 0:8c8d92233d08 | 145 | strip.setPixelColor(bucket, strip.Color((int)(128.0/score-green_start),127,0)); |
awatt196 | 0:8c8d92233d08 | 146 | } |
awatt196 | 0:8c8d92233d08 | 147 | else //(score >=yellow_start) //yellow to red |
awatt196 | 0:8c8d92233d08 | 148 | { |
awatt196 | 0:8c8d92233d08 | 149 | strip.setPixelColor(bucket, strip.Color(127,(int)(128.0/score-96.0),0)); |
awatt196 | 0:8c8d92233d08 | 150 | } |
awatt196 | 0:8c8d92233d08 | 151 | |
awatt196 | 0:8c8d92233d08 | 152 | if(score >=100) //red |
awatt196 | 0:8c8d92233d08 | 153 | { |
awatt196 | 0:8c8d92233d08 | 154 | strip.setPixelColor(bucket, strip.Color(127,0,0)); |
awatt196 | 0:8c8d92233d08 | 155 | } |
awatt196 | 0:8c8d92233d08 | 156 | */ |
awatt196 | 0:8c8d92233d08 | 157 | |
awatt196 | 0:8c8d92233d08 | 158 | strip.setPixelColor(bucket, strip.Color(score_array[bucket],0,0)); |
awatt196 | 0:8c8d92233d08 | 159 | strip.show(); |
awatt196 | 0:8c8d92233d08 | 160 | } |
awatt196 | 0:8c8d92233d08 | 161 | } |
awatt196 | 0:8c8d92233d08 | 162 | } |