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-08-05
Revision:
2:23fb8e32cf1b
Parent:
1:a5690556741c

File content as of revision 2:23fb8e32cf1b:

#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);
InterruptIn KITT(PTA4);

//KITT bouncing light effect
void scanner() {
    uint8_t r=127;
    uint8_t g=0;
    uint8_t b=0;
    uint8_t delay=50;
    int i, j, pos, dir;

    pos = 0;
    dir = 1;

    for (i=0; i<((strip.numPixels()-1) * 8); i++) {
        strip.setPixelColor(pos - 2, strip.Color(r/4, g/4, b/4));
        strip.setPixelColor(pos - 1, strip.Color(r/2, g/2, b/2));
        strip.setPixelColor(pos, strip.Color(r, g, b));
        strip.setPixelColor(pos + 1, strip.Color(r/2, g/2, b/2));
        strip.setPixelColor(pos + 2, strip.Color(r/4, g/4, b/4));

        strip.show();
        wait_ms(delay);

        for (j=-2; j<= 2; j++)
            strip.setPixelColor(pos+j, strip.Color(0,0,0));

        pos += dir;
        if (pos < 0) {
            pos = 1;
            dir = -dir;
        } else if (pos >= strip.numPixels()) {
            pos = strip.numPixels() - 2;
            dir = -dir;
        }
    }
}

int main() {
    KITT.rise(&scanner);
    
    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] = 30; //-1 for off.  30 for max brightness
                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; //36 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; 

        for(int bucket=0; bucket<length; bucket++) 
        {
            int bucketStart = bucketSize*bucket; 
            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];
                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; 
                if(score > 0)
                {
                    //creates non-linear scaling for LED brightness
                    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]);
            
            /*                  blue->red
            strip.setPixelColor(bucket, strip.Color(score_array[bucket],0,127-score_array[bucket]));
            */
         
            /*                  blue->green->red
            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();
        }
    }
}