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

Committer:
awatt196
Date:
2016-07-21
Revision:
1:a5690556741c
Parent:
0:8c8d92233d08
Child:
2:23fb8e32cf1b

File content as of revision 1:a5690556741c:

#include "LPD8806.h"
#include "EthernetInterface.h"
#include <string>
#include <math.h> 

using namespace std;

#define ECHO_SERVER_PORT   4548

LPD8806 strip = LPD8806(36);

Serial pc(USBTX, USBRX);

int main() {
    EthernetInterface eth;
    eth.init(); //Use DHCP
    eth.connect();
    pc.printf("\nServer IP Address is %s\n", eth.getIPAddress());
    UDPSocket server;
    server.bind(ECHO_SERVER_PORT);
    Endpoint client;
    char buffer[5000]; //buffer to hold the depth data (640 data points)
    
    // Start up the LED strip
    strip.begin();

    // Update the strip, to start they are all 'off'
    strip.show();
    while (1) {
        //printf("\nWaiting for UDP packet...\n");
        int n = server.receiveFrom(client, buffer, sizeof(buffer));
        buffer[n] = '\0';
        
        //printf("Received packet from: %s\n", client.get_address());
        //printf("Packet contents : '%s'\n",buffer);
        
        string data_string = "";
        int rawdata_array[640]; //holds raw int data
        int index=0;
        
        for(int i=0; i<n; i++)
        {
            if(buffer[i] >= 48 && buffer[i] <= 57) //selects 0-9
            {
                data_string+=buffer[i];   
            }      
            else if(buffer[i] == 120) //x = invalid data
            {
                rawdata_array[index] = -1;
                index++;
                data_string = ""; //clear string
            }
            else if(buffer[i] == 44 || buffer[i] == 93) //reached a comma or end bracket
            {
                if (data_string.length() != 0)
                {
                    int int_data = atoi(data_string.c_str());
                    rawdata_array[index] = int_data;
                    data_string = ""; //clear string
                    index++;
                }
            }
            else 
            {
                //do nothing
            }
        }
        
        int length = 36; //32 LEDs on strip
        int m_defaultRange = -1;
        int m_minRange = 30;
        int m_maxRange = 250;
        double m_minData = 127.0;
        double m_maxData = 0.0;
        double m_defaultData = 0.0;
    
        double bucketSize = 640 / length; //double bucketSize = values.size() / length(); = 20

        for(int bucket=0; bucket<length; bucket++) //for(int bucket=0; bucket<length(); bucket++)
        {
            int bucketStart = bucketSize*bucket; //0-620 = 
            int bucketEnd = bucketSize*(bucket+1) - 1; 

            double accumulator = 0;
            int points = 0;
            for(int index=bucketStart; index<=bucketEnd; index++)
            {
                double value = (double)rawdata_array[index];
                //printf("index=%i\tvalue=%f\n", index, value);
                if(value != m_defaultRange)
                {
                    accumulator +=  value;
                    points++;
                }
            }

            // Calc average of points or default value if no data
            double average = m_defaultRange;
            if(points != 0)
            {
                average = accumulator / points;
            }
            
            int score_array[length];
            double score = m_defaultData;
            if ((average != m_defaultRange) && (average >= m_minRange) && (average <= m_maxRange))
            {
                score = average * (m_maxData - m_minData) / (m_maxRange - m_minRange) + m_minData; //from 0-127
                if(score > 0)
                {
                    float x_scale = 18.142857;
                    float x = score/x_scale;
                    float y = pow(2, x);
                    if(y==128) y=127;
                    score_array[bucket] = (int)y;
                }
            }
            else //out of range
            {
                score = 0;
                score_array[bucket] = score;
            }
            //printf("bucket:%i, score=%i\r\n", bucket, score_array[bucket]);
         
            /* //for using the full colour spectrum
            int cyan_start=33;
            int green_start=66;
            int yellow_start=99;
        
            if(score ==  0) //off
            {
                strip.setPixelColor(bucket, strip.Color(0,0,0));
            }
            
            else if(score >=1 && score <cyan_start) //blue to cyan
            {
                strip.setPixelColor(bucket, strip.Color(0,(int)(128.0/cyan_start),127));
            }
            else if(score >=cyan_start && score <green_start) //cyan to green
            {
                strip.setPixelColor(bucket, strip.Color(0,127,(int)(128.0/score-cyan_start)));
            }
            else if(score >=green_start && score <=yellow_start) //green to yellow
            {
                strip.setPixelColor(bucket, strip.Color((int)(128.0/score-green_start),127,0));
            }
            else //(score >=yellow_start) //yellow to red
            {
                strip.setPixelColor(bucket, strip.Color(127,(int)(128.0/score-96.0),0));
            }
            
            if(score >=100) //red
            {
                strip.setPixelColor(bucket, strip.Color(127,0,0));
            }
            */       
                
            strip.setPixelColor(bucket, strip.Color(score_array[bucket],0,0));
            strip.show();
        }
    }
}