File content as of revision 4:ec20064efef4:

#include "mbed.h"
#include "FastAnalogIn.h"
#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/MultiArrayLayout.h>
#include <stdint.h>
#include <math.h>

#define BAUD_RATE 921600
#define NUM_SAMPLES 5

//Globals for ROS 
ros::NodeHandle nh;                                             //Node handle 
std_msgs::UInt16MultiArray readings_msg;                        //Readings message structure is defined
std_msgs::MultiArrayDimension myDim;                            //MultiArrayDimension structure is defined
std_msgs::MultiArrayLayout myLayout;                            //MultiArrayLayout structure is defined
ros::Publisher pub_sensor("optic_sensor", &readings_msg);       //Publisher is defined, will publish to the topic named "optic_sensor" with a message of type Int16MultiArray

int number_of_leds= 6;          //Up to 16
int number_of_diodes= 6;        //Up to 16
int array_length = (number_of_leds+1)*number_of_diodes ;

//FastAnalogIn ain(p20); //Fast&Furious:Tokyo Drift Analog Input to PDmux
AnalogIn ain(p20); //Analog Input to PDmux

DigitalOut LEDout(p8); //5V output to LED mux

DigitalOut LEDmux0(p9); //s0
DigitalOut LEDmux1(p10); //s1
DigitalOut LEDmux2(p11); //s2
DigitalOut LEDmux3(p12); //s3

DigitalOut PDmux0(p14); //s0
DigitalOut PDmux1(p15); //s1
DigitalOut PDmux2(p16); //s2
DigitalOut PDmux3(p17); //s3
AnalogOut aout(p18);

DigitalOut myled(LED2);     //To check programming


void Demux_LED(int); 
void Demux_PD(int); 
int median_of_array();
void bubble_sort(int[], int);


int main()
{    
    myled = 1;
    double time=.000029;  // ~10kHz
    //Set up all ROS communication
    nh.getHardware()->setBaud(BAUD_RATE);
    nh.initNode();
    
    //Setup all necessary fields of my MultiArray message (except data) >> See message structure commented before setup()
    myDim.label = "readings";
    myDim.size = array_length;
    myDim.stride = array_length;
    myLayout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension) * 1);
    myLayout.dim[0] = myDim;
    myLayout.data_offset = 0;
    readings_msg.layout = myLayout;
    readings_msg.data = (uint16_t *)malloc(sizeof(int)*array_length);
    readings_msg.data_length = array_length;
    nh.advertise(pub_sensor);
    
    while(1) 
    {
        //In switching mode, we just go through all the LEDs as fast as possible
        //and take readings of the 8 diodes. We take 5 readings of a diode, and just send out the median value.
        //The array we sent out has batches of 8 numbers. The first 8 correspond to the 8 diodes when all LEDs are off (State 0)
        //The next 8 values correspond to the next 8 diodes values when LED #1 is ON, and so on....
        int idx=0;
        int measurements[5];
        for(int i=-1;i<number_of_leds;i++)  //Start from all LEDs OFF STATE.
        {
          for(int j=0;j<number_of_diodes;j++)
          {
            Demux_PD(j);
            //Take 5 measurements for this LED/PD combo
            for(int k=0;k<5;k++)
            {
                Demux_LED(i);       //Turn on selected LED
                measurements[k]= ain.read_u16();
                wait(time);
 
                Demux_LED(-1);      //Turn off LED
                ain.read_u16();
                wait(time);
            }
            float voltageOut=(measurements[2]+measurements[3]+measurements[4])/3.0; //Average of last three values
            aout=voltageOut/65535.0; //Sets Voltage out to Pin 18 for debugging on scope
            readings_msg.data[idx] = (uint16_t)voltageOut; //load data field of my message 
            if(idx<array_length)
              idx++;
          }
        }
        Demux_LED(-1);          //Turn off the LEDs while we put together the ROS package.
        pub_sensor.publish(&readings_msg);
        nh.spinOnce();  
    }
}



//int main()
//{
//    Serial pc(USBTX, USBRX); // tx, rx
//    pc.baud(115200);
//    myled=1;
//    
//    int led=-1;
//    while(1)
//    {
//        Demux_LED(led);
//        for(int pd=0;pd<6;pd++)
//        {
//            Demux_PD(pd);
//            pc.printf("For LED#%d and PD#%d >>> %f \n\r",led,pd,ain.read());
//        }
//        led++;
//        wait(1);
//        if(led>5)
//            led=-1;
//    }   
//}


void Demux_LED(int input) 
{
    if(input>=0 && input<=15) 
    {
        LEDout = 1;
        LEDmux3=(input/8)%2;  //LSB
        LEDmux2=(input/4)%2;
        LEDmux1=(input/2)%2;
        LEDmux0=input%2;      //MSB
        
    }
    else
    {
        LEDout = 0;
    }
}

void Demux_PD(int input) 
{
    if(input>=0 && input<=15) 
    {
        PDmux3=(input/8)%2;  //LSB
        PDmux2=(input/4)%2;
        PDmux1=(input/2)%2;
        PDmux0=input%2;      //MSB
    }
}
    
    
int median_of_array()
{
  int measurements[NUM_SAMPLES];
  for(int i=0;i<NUM_SAMPLES;i++)
  {
    measurements[i] = ain.read_u16();
  }
  bubble_sort(measurements,NUM_SAMPLES);
  return measurements[(NUM_SAMPLES-1)/2];
}


//Sorting function for computing median values.
void bubble_sort(int array[], int size)
{
  int  i, j, temp;
  for (i=0 ; i<(size-1); i++)
  {
    for (j=0 ; j< (size-i-1); j++)
    {
      if (array[j] > array[j+1])
      {
        /* Swapping */
        temp = array[j];
        array[j] = array[j+1];
        array[j+1] = temp;
      }
    }
  }
}