Revision 4:a6db36142bf8, committed 2017-03-21

Comitter:

Piachnp

Date:

Tue Mar 21 14:16:05 2017 +0000

Parent:

3:2adce774a137

Commit message:

Working version of ROS code for the Non-Synchronous detection case

Changed in this revision

--- a/main.cpp	Wed Mar 08 18:17:12 2017 +0000
+++ b/main.cpp	Tue Mar 21 14:16:05 2017 +0000
@@ -1,14 +1,30 @@
 #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>
 
+#define BAUD_RATE 115200
+#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
 
-Serial pc(USBTX, USBRX); // tx, rx
+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
@@ -19,117 +35,141 @@
 DigitalOut PDmux2(p16); //s2
 DigitalOut PDmux3(p17); //s3
 AnalogOut aout(p18);
-
-int LED; //counter for LEDs
+DigitalOut myled(LED1);     //To check programming
 
-double voltageOut;
-double readIn[5];
-
-
-double alpha; //dummy variable for ADC
-
+void Demux_LED(int); 
+void Demux_PD(int); 
+int median_of_array();
+void bubble_sort(int[], int);
 
 
 int main()
-{
-    pc.baud(921600);
-    //pc.format();
-
-//   double ADCtime=.000000116; //1.6 us
-    double time=.000029;  // ~15kHz
-//    double time=.0000029; //100 kHz
-//    double time=.0003;
-
-
-    //counters for various while loops
-//    int mPD=0;
-//    int nPD=0;
-//    int mLED=0;
-//    int nLED=0;
-    //int pd=0;
-    //int i=0;
-
-    //boolean bits for multiplexing
-    LEDmux0=0;
-    LEDmux1=0;
-    LEDmux2=0;
-    LEDmux3=0; //MSB is always 0
-    PDmux0=0;
-    PDmux1=0;
-    PDmux2=0;
-    PDmux3=0;  //MSB is always 0
-
-
-    //while loop that runs continously through code to constantly give measuremtn while MCU is on
-    while(1) {
-
-
-        //loop to mux through photodiodes
-
-        for(int pd=0; pd<6; pd++) {
-            //loop will take 5 measurements for the selected LED/PD combo
-            for(int i=0; i<5; i++) {
-                LEDout = 1;
-                readIn[i]=ain.read();
-                wait(time);
-
-                //LEDout = 0;
-                //alpha=ain.read();
-                //wait(time);
-            }
-
-            voltageOut=(readIn[2]+readIn[3]+readIn[4])/3;
-            aout=voltageOut; //Sets Voltage out to Pin 18 for debugging on scope
-
-            //send the diode readings to MATlab in format: LED,PD,Voltageout
-            //I think this is where you want to report voltageOut to ROS for the PD/LED combo
-            
-            pc.printf("%d,%d,%f \n",LED,pd,voltageOut);
-
-
-
-            //PD multiplexing
-            if((PDmux0==0) && (PDmux1==0) && (PDmux2==0)) {
-                PDmux0=1;
-            } else if((PDmux0==1) && (PDmux1==0) && (PDmux2==0)) {
-                PDmux0=0;
-                PDmux1=1;
-            } else if((PDmux0==0) && (PDmux1==1) && (PDmux2==0)) {
-                PDmux0=1;
-            } else if((PDmux0==1) && (PDmux1==1) && (PDmux2==0)) {
-                PDmux0=0;
-                PDmux1=0;
-                PDmux2=1;
-            } else if((PDmux0==0) && (PDmux1==0) && (PDmux2==1)) {
-                PDmux0=1;
-            } else {
-                PDmux0=0;
-                PDmux2=0;
-            }
-
+{    
+    myled = 1;
+    //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;
+        for(int i=-1;i<number_of_leds;i++)  //Start from all LEDs OFF STATE.
+        {
+          Demux_LED(i);     //Turn on the appropriate LED
+          for(int j=0;j<number_of_diodes;j++)
+          {
+            Demux_PD(j);
+            readings_msg.data[idx] = (uint16_t)median_of_array(); //load data field of my message with the median value of my readings
+            //readings_msg.data[idx] = 100;
+            aout=readings_msg.data[idx];
+            if(idx<array_length)
+              idx++;
+          }
         }
-                if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==0)) {
-                    LEDmux0=1;
-                } else if((LEDmux0==1) && (LEDmux1==0) && (LEDmux2==0)) {
-                    LEDmux0=0;
-                    LEDmux1=1;
-                } else if((LEDmux0==0) && (LEDmux1==1) && (LEDmux2==0)) {
-                    LEDmux0=1;
-                } else if((LEDmux0==1) && (LEDmux1==1) && (LEDmux2==0)) {
-                    LEDmux0=0;
-                    LEDmux1=0;
-                    LEDmux2=1;
-                } else if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==1)) {
-                    LEDmux0=1;
-                } else {
-                    LEDmux0=0;
-                    LEDmux2=0;
-                }
-        
-
-
-
+        Demux_LED(-1);          //Turn off the LEDs while we put together the ROS package.
+        pub_sensor.publish(&readings_msg);
+        nh.spinOnce();  
+        wait_ms(10);
     }
 }
 
 
+
+//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;
+      }
+    }
+  }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_indigo.lib	Tue Mar 21 14:16:05 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/garyservin/code/ros_lib_indigo/#fd24f7ca9688