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@2:23fb8e32cf1b, 2016-08-05 (annotated)
- Committer:
- awatt196
- Date:
- Fri Aug 05 10:42:30 2016 +0000
- Revision:
- 2:23fb8e32cf1b
- Parent:
- 1:a5690556741c
APM version
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 | 1:a5690556741c | 10 | LPD8806 strip = LPD8806(36); |
awatt196 | 0:8c8d92233d08 | 11 | |
awatt196 | 0:8c8d92233d08 | 12 | Serial pc(USBTX, USBRX); |
awatt196 | 2:23fb8e32cf1b | 13 | InterruptIn KITT(PTA4); |
awatt196 | 2:23fb8e32cf1b | 14 | |
awatt196 | 2:23fb8e32cf1b | 15 | //KITT bouncing light effect |
awatt196 | 2:23fb8e32cf1b | 16 | void scanner() { |
awatt196 | 2:23fb8e32cf1b | 17 | uint8_t r=127; |
awatt196 | 2:23fb8e32cf1b | 18 | uint8_t g=0; |
awatt196 | 2:23fb8e32cf1b | 19 | uint8_t b=0; |
awatt196 | 2:23fb8e32cf1b | 20 | uint8_t delay=50; |
awatt196 | 2:23fb8e32cf1b | 21 | int i, j, pos, dir; |
awatt196 | 2:23fb8e32cf1b | 22 | |
awatt196 | 2:23fb8e32cf1b | 23 | pos = 0; |
awatt196 | 2:23fb8e32cf1b | 24 | dir = 1; |
awatt196 | 2:23fb8e32cf1b | 25 | |
awatt196 | 2:23fb8e32cf1b | 26 | for (i=0; i<((strip.numPixels()-1) * 8); i++) { |
awatt196 | 2:23fb8e32cf1b | 27 | strip.setPixelColor(pos - 2, strip.Color(r/4, g/4, b/4)); |
awatt196 | 2:23fb8e32cf1b | 28 | strip.setPixelColor(pos - 1, strip.Color(r/2, g/2, b/2)); |
awatt196 | 2:23fb8e32cf1b | 29 | strip.setPixelColor(pos, strip.Color(r, g, b)); |
awatt196 | 2:23fb8e32cf1b | 30 | strip.setPixelColor(pos + 1, strip.Color(r/2, g/2, b/2)); |
awatt196 | 2:23fb8e32cf1b | 31 | strip.setPixelColor(pos + 2, strip.Color(r/4, g/4, b/4)); |
awatt196 | 2:23fb8e32cf1b | 32 | |
awatt196 | 2:23fb8e32cf1b | 33 | strip.show(); |
awatt196 | 2:23fb8e32cf1b | 34 | wait_ms(delay); |
awatt196 | 2:23fb8e32cf1b | 35 | |
awatt196 | 2:23fb8e32cf1b | 36 | for (j=-2; j<= 2; j++) |
awatt196 | 2:23fb8e32cf1b | 37 | strip.setPixelColor(pos+j, strip.Color(0,0,0)); |
awatt196 | 2:23fb8e32cf1b | 38 | |
awatt196 | 2:23fb8e32cf1b | 39 | pos += dir; |
awatt196 | 2:23fb8e32cf1b | 40 | if (pos < 0) { |
awatt196 | 2:23fb8e32cf1b | 41 | pos = 1; |
awatt196 | 2:23fb8e32cf1b | 42 | dir = -dir; |
awatt196 | 2:23fb8e32cf1b | 43 | } else if (pos >= strip.numPixels()) { |
awatt196 | 2:23fb8e32cf1b | 44 | pos = strip.numPixels() - 2; |
awatt196 | 2:23fb8e32cf1b | 45 | dir = -dir; |
awatt196 | 2:23fb8e32cf1b | 46 | } |
awatt196 | 2:23fb8e32cf1b | 47 | } |
awatt196 | 2:23fb8e32cf1b | 48 | } |
awatt196 | 0:8c8d92233d08 | 49 | |
awatt196 | 0:8c8d92233d08 | 50 | int main() { |
awatt196 | 2:23fb8e32cf1b | 51 | KITT.rise(&scanner); |
awatt196 | 2:23fb8e32cf1b | 52 | |
awatt196 | 0:8c8d92233d08 | 53 | EthernetInterface eth; |
awatt196 | 0:8c8d92233d08 | 54 | eth.init(); //Use DHCP |
awatt196 | 0:8c8d92233d08 | 55 | eth.connect(); |
awatt196 | 0:8c8d92233d08 | 56 | pc.printf("\nServer IP Address is %s\n", eth.getIPAddress()); |
awatt196 | 0:8c8d92233d08 | 57 | UDPSocket server; |
awatt196 | 0:8c8d92233d08 | 58 | server.bind(ECHO_SERVER_PORT); |
awatt196 | 0:8c8d92233d08 | 59 | Endpoint client; |
awatt196 | 0:8c8d92233d08 | 60 | char buffer[5000]; //buffer to hold the depth data (640 data points) |
awatt196 | 0:8c8d92233d08 | 61 | |
awatt196 | 0:8c8d92233d08 | 62 | // Start up the LED strip |
awatt196 | 0:8c8d92233d08 | 63 | strip.begin(); |
awatt196 | 0:8c8d92233d08 | 64 | |
awatt196 | 0:8c8d92233d08 | 65 | // Update the strip, to start they are all 'off' |
awatt196 | 0:8c8d92233d08 | 66 | strip.show(); |
awatt196 | 0:8c8d92233d08 | 67 | while (1) { |
awatt196 | 0:8c8d92233d08 | 68 | //printf("\nWaiting for UDP packet...\n"); |
awatt196 | 0:8c8d92233d08 | 69 | int n = server.receiveFrom(client, buffer, sizeof(buffer)); |
awatt196 | 0:8c8d92233d08 | 70 | buffer[n] = '\0'; |
awatt196 | 0:8c8d92233d08 | 71 | |
awatt196 | 0:8c8d92233d08 | 72 | //printf("Received packet from: %s\n", client.get_address()); |
awatt196 | 0:8c8d92233d08 | 73 | //printf("Packet contents : '%s'\n",buffer); |
awatt196 | 0:8c8d92233d08 | 74 | |
awatt196 | 0:8c8d92233d08 | 75 | string data_string = ""; |
awatt196 | 0:8c8d92233d08 | 76 | int rawdata_array[640]; //holds raw int data |
awatt196 | 0:8c8d92233d08 | 77 | int index=0; |
awatt196 | 0:8c8d92233d08 | 78 | |
awatt196 | 0:8c8d92233d08 | 79 | for(int i=0; i<n; i++) |
awatt196 | 0:8c8d92233d08 | 80 | { |
awatt196 | 0:8c8d92233d08 | 81 | if(buffer[i] >= 48 && buffer[i] <= 57) //selects 0-9 |
awatt196 | 0:8c8d92233d08 | 82 | { |
awatt196 | 0:8c8d92233d08 | 83 | data_string+=buffer[i]; |
awatt196 | 0:8c8d92233d08 | 84 | } |
awatt196 | 0:8c8d92233d08 | 85 | else if(buffer[i] == 120) //x = invalid data |
awatt196 | 0:8c8d92233d08 | 86 | { |
awatt196 | 2:23fb8e32cf1b | 87 | rawdata_array[index] = 30; //-1 for off. 30 for max brightness |
awatt196 | 0:8c8d92233d08 | 88 | index++; |
awatt196 | 0:8c8d92233d08 | 89 | data_string = ""; //clear string |
awatt196 | 0:8c8d92233d08 | 90 | } |
awatt196 | 0:8c8d92233d08 | 91 | else if(buffer[i] == 44 || buffer[i] == 93) //reached a comma or end bracket |
awatt196 | 0:8c8d92233d08 | 92 | { |
awatt196 | 0:8c8d92233d08 | 93 | if (data_string.length() != 0) |
awatt196 | 0:8c8d92233d08 | 94 | { |
awatt196 | 0:8c8d92233d08 | 95 | int int_data = atoi(data_string.c_str()); |
awatt196 | 0:8c8d92233d08 | 96 | rawdata_array[index] = int_data; |
awatt196 | 0:8c8d92233d08 | 97 | data_string = ""; //clear string |
awatt196 | 0:8c8d92233d08 | 98 | index++; |
awatt196 | 0:8c8d92233d08 | 99 | } |
awatt196 | 0:8c8d92233d08 | 100 | } |
awatt196 | 0:8c8d92233d08 | 101 | else |
awatt196 | 0:8c8d92233d08 | 102 | { |
awatt196 | 0:8c8d92233d08 | 103 | //do nothing |
awatt196 | 0:8c8d92233d08 | 104 | } |
awatt196 | 0:8c8d92233d08 | 105 | } |
awatt196 | 0:8c8d92233d08 | 106 | |
awatt196 | 2:23fb8e32cf1b | 107 | int length = 36; //36 LEDs on strip |
awatt196 | 2:23fb8e32cf1b | 108 | int m_defaultRange = -1; |
awatt196 | 0:8c8d92233d08 | 109 | int m_minRange = 30; |
awatt196 | 0:8c8d92233d08 | 110 | int m_maxRange = 250; |
awatt196 | 0:8c8d92233d08 | 111 | double m_minData = 127.0; |
awatt196 | 0:8c8d92233d08 | 112 | double m_maxData = 0.0; |
awatt196 | 0:8c8d92233d08 | 113 | double m_defaultData = 0.0; |
awatt196 | 0:8c8d92233d08 | 114 | |
awatt196 | 2:23fb8e32cf1b | 115 | double bucketSize = 640 / length; |
awatt196 | 0:8c8d92233d08 | 116 | |
awatt196 | 2:23fb8e32cf1b | 117 | for(int bucket=0; bucket<length; bucket++) |
awatt196 | 0:8c8d92233d08 | 118 | { |
awatt196 | 2:23fb8e32cf1b | 119 | int bucketStart = bucketSize*bucket; |
awatt196 | 0:8c8d92233d08 | 120 | int bucketEnd = bucketSize*(bucket+1) - 1; |
awatt196 | 0:8c8d92233d08 | 121 | |
awatt196 | 0:8c8d92233d08 | 122 | double accumulator = 0; |
awatt196 | 0:8c8d92233d08 | 123 | int points = 0; |
awatt196 | 0:8c8d92233d08 | 124 | for(int index=bucketStart; index<=bucketEnd; index++) |
awatt196 | 0:8c8d92233d08 | 125 | { |
awatt196 | 0:8c8d92233d08 | 126 | double value = (double)rawdata_array[index]; |
awatt196 | 0:8c8d92233d08 | 127 | if(value != m_defaultRange) |
awatt196 | 0:8c8d92233d08 | 128 | { |
awatt196 | 0:8c8d92233d08 | 129 | accumulator += value; |
awatt196 | 0:8c8d92233d08 | 130 | points++; |
awatt196 | 0:8c8d92233d08 | 131 | } |
awatt196 | 0:8c8d92233d08 | 132 | } |
awatt196 | 0:8c8d92233d08 | 133 | |
awatt196 | 0:8c8d92233d08 | 134 | // Calc average of points or default value if no data |
awatt196 | 0:8c8d92233d08 | 135 | double average = m_defaultRange; |
awatt196 | 0:8c8d92233d08 | 136 | if(points != 0) |
awatt196 | 0:8c8d92233d08 | 137 | { |
awatt196 | 0:8c8d92233d08 | 138 | average = accumulator / points; |
awatt196 | 0:8c8d92233d08 | 139 | } |
awatt196 | 0:8c8d92233d08 | 140 | |
awatt196 | 1:a5690556741c | 141 | int score_array[length]; |
awatt196 | 0:8c8d92233d08 | 142 | double score = m_defaultData; |
awatt196 | 0:8c8d92233d08 | 143 | if ((average != m_defaultRange) && (average >= m_minRange) && (average <= m_maxRange)) |
awatt196 | 0:8c8d92233d08 | 144 | { |
awatt196 | 2:23fb8e32cf1b | 145 | score = average * (m_maxData - m_minData) / (m_maxRange - m_minRange) + m_minData; |
awatt196 | 0:8c8d92233d08 | 146 | if(score > 0) |
awatt196 | 0:8c8d92233d08 | 147 | { |
awatt196 | 2:23fb8e32cf1b | 148 | //creates non-linear scaling for LED brightness |
awatt196 | 0:8c8d92233d08 | 149 | float x_scale = 18.142857; |
awatt196 | 0:8c8d92233d08 | 150 | float x = score/x_scale; |
awatt196 | 0:8c8d92233d08 | 151 | float y = pow(2, x); |
awatt196 | 0:8c8d92233d08 | 152 | if(y==128) y=127; |
awatt196 | 0:8c8d92233d08 | 153 | score_array[bucket] = (int)y; |
awatt196 | 0:8c8d92233d08 | 154 | } |
awatt196 | 0:8c8d92233d08 | 155 | } |
awatt196 | 0:8c8d92233d08 | 156 | else //out of range |
awatt196 | 0:8c8d92233d08 | 157 | { |
awatt196 | 0:8c8d92233d08 | 158 | score = 0; |
awatt196 | 0:8c8d92233d08 | 159 | score_array[bucket] = score; |
awatt196 | 0:8c8d92233d08 | 160 | } |
awatt196 | 0:8c8d92233d08 | 161 | //printf("bucket:%i, score=%i\r\n", bucket, score_array[bucket]); |
awatt196 | 2:23fb8e32cf1b | 162 | |
awatt196 | 2:23fb8e32cf1b | 163 | /* blue->red |
awatt196 | 2:23fb8e32cf1b | 164 | strip.setPixelColor(bucket, strip.Color(score_array[bucket],0,127-score_array[bucket])); |
awatt196 | 2:23fb8e32cf1b | 165 | */ |
awatt196 | 0:8c8d92233d08 | 166 | |
awatt196 | 2:23fb8e32cf1b | 167 | /* blue->green->red |
awatt196 | 0:8c8d92233d08 | 168 | int cyan_start=33; |
awatt196 | 0:8c8d92233d08 | 169 | int green_start=66; |
awatt196 | 0:8c8d92233d08 | 170 | int yellow_start=99; |
awatt196 | 0:8c8d92233d08 | 171 | |
awatt196 | 0:8c8d92233d08 | 172 | if(score == 0) //off |
awatt196 | 0:8c8d92233d08 | 173 | { |
awatt196 | 0:8c8d92233d08 | 174 | strip.setPixelColor(bucket, strip.Color(0,0,0)); |
awatt196 | 0:8c8d92233d08 | 175 | } |
awatt196 | 0:8c8d92233d08 | 176 | |
awatt196 | 0:8c8d92233d08 | 177 | else if(score >=1 && score <cyan_start) //blue to cyan |
awatt196 | 0:8c8d92233d08 | 178 | { |
awatt196 | 0:8c8d92233d08 | 179 | strip.setPixelColor(bucket, strip.Color(0,(int)(128.0/cyan_start),127)); |
awatt196 | 0:8c8d92233d08 | 180 | } |
awatt196 | 0:8c8d92233d08 | 181 | else if(score >=cyan_start && score <green_start) //cyan to green |
awatt196 | 0:8c8d92233d08 | 182 | { |
awatt196 | 0:8c8d92233d08 | 183 | strip.setPixelColor(bucket, strip.Color(0,127,(int)(128.0/score-cyan_start))); |
awatt196 | 0:8c8d92233d08 | 184 | } |
awatt196 | 0:8c8d92233d08 | 185 | else if(score >=green_start && score <=yellow_start) //green to yellow |
awatt196 | 0:8c8d92233d08 | 186 | { |
awatt196 | 0:8c8d92233d08 | 187 | strip.setPixelColor(bucket, strip.Color((int)(128.0/score-green_start),127,0)); |
awatt196 | 0:8c8d92233d08 | 188 | } |
awatt196 | 0:8c8d92233d08 | 189 | else //(score >=yellow_start) //yellow to red |
awatt196 | 0:8c8d92233d08 | 190 | { |
awatt196 | 0:8c8d92233d08 | 191 | strip.setPixelColor(bucket, strip.Color(127,(int)(128.0/score-96.0),0)); |
awatt196 | 0:8c8d92233d08 | 192 | } |
awatt196 | 0:8c8d92233d08 | 193 | |
awatt196 | 0:8c8d92233d08 | 194 | if(score >=100) //red |
awatt196 | 0:8c8d92233d08 | 195 | { |
awatt196 | 0:8c8d92233d08 | 196 | strip.setPixelColor(bucket, strip.Color(127,0,0)); |
awatt196 | 0:8c8d92233d08 | 197 | } |
awatt196 | 0:8c8d92233d08 | 198 | */ |
awatt196 | 0:8c8d92233d08 | 199 | |
awatt196 | 0:8c8d92233d08 | 200 | strip.setPixelColor(bucket, strip.Color(score_array[bucket],0,0)); |
awatt196 | 0:8c8d92233d08 | 201 | strip.show(); |
awatt196 | 0:8c8d92233d08 | 202 | } |
awatt196 | 0:8c8d92233d08 | 203 | } |
awatt196 | 0:8c8d92233d08 | 204 | } |