CLUE / Mbed 2 deprecated Mirror_Top_Indenter_ROS_Sync10Khz

Synchronous detection code with ROS communication for optic sensor

Dependencies:   FastAnalogIn mbed ros_lib_indigo

Fork of Mirror_Top_Indenter_ROS by CLUE

main.cpp@4:ec20064efef4, 2017-03-21 (annotated)

Committer:
Piachnp
Date:
Tue Mar 21 20:11:18 2017 +0000
Revision:
4:ec20064efef4
Parent:
3:2adce774a137 
Working code for Synchronous detection @10Khz with ROS communication enabled

Who changed what in which revision?

UserRevisionLine numberNew contents of line
keithbehrman 0:d83ac315a24c 1 #include "mbed.h"
keithbehrman 0:d83ac315a24c 2 #include "FastAnalogIn.h"
Piachnp 4:ec20064efef4 3 #include <ros.h>
Piachnp 4:ec20064efef4 4 #include <std_msgs/UInt16MultiArray.h>
Piachnp 4:ec20064efef4 5 #include <std_msgs/MultiArrayDimension.h>
Piachnp 4:ec20064efef4 6 #include <std_msgs/MultiArrayLayout.h>
Piachnp 4:ec20064efef4 7 #include <stdint.h>
Piachnp 4:ec20064efef4 8 #include <math.h>
keithbehrman 0:d83ac315a24c 9
Piachnp 4:ec20064efef4 10 #define BAUD_RATE 921600
Piachnp 4:ec20064efef4 11 #define NUM_SAMPLES 5
keithbehrman 0:d83ac315a24c 12
Piachnp 4:ec20064efef4 13 //Globals for ROS
Piachnp 4:ec20064efef4 14 ros::NodeHandle nh; //Node handle
Piachnp 4:ec20064efef4 15 std_msgs::UInt16MultiArray readings_msg; //Readings message structure is defined
Piachnp 4:ec20064efef4 16 std_msgs::MultiArrayDimension myDim; //MultiArrayDimension structure is defined
Piachnp 4:ec20064efef4 17 std_msgs::MultiArrayLayout myLayout; //MultiArrayLayout structure is defined
Piachnp 4:ec20064efef4 18 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
keithbehrman 1:8e7e9ef6b0bd 19
Piachnp 4:ec20064efef4 20 int number_of_leds= 6; //Up to 16
Piachnp 4:ec20064efef4 21 int number_of_diodes= 6; //Up to 16
Piachnp 4:ec20064efef4 22 int array_length = (number_of_leds+1)*number_of_diodes ;
keithbehrman 1:8e7e9ef6b0bd 23
keithbehrman 3:2adce774a137 24 //FastAnalogIn ain(p20); //Fast&Furious:Tokyo Drift Analog Input to PDmux
keithbehrman 3:2adce774a137 25 AnalogIn ain(p20); //Analog Input to PDmux
keithbehrman 1:8e7e9ef6b0bd 26
keithbehrman 0:d83ac315a24c 27 DigitalOut LEDout(p8); //5V output to LED mux
Piachnp 4:ec20064efef4 28
keithbehrman 0:d83ac315a24c 29 DigitalOut LEDmux0(p9); //s0
keithbehrman 0:d83ac315a24c 30 DigitalOut LEDmux1(p10); //s1
keithbehrman 0:d83ac315a24c 31 DigitalOut LEDmux2(p11); //s2
keithbehrman 0:d83ac315a24c 32 DigitalOut LEDmux3(p12); //s3
keithbehrman 0:d83ac315a24c 33
keithbehrman 0:d83ac315a24c 34 DigitalOut PDmux0(p14); //s0
keithbehrman 0:d83ac315a24c 35 DigitalOut PDmux1(p15); //s1
keithbehrman 0:d83ac315a24c 36 DigitalOut PDmux2(p16); //s2
keithbehrman 0:d83ac315a24c 37 DigitalOut PDmux3(p17); //s3
keithbehrman 1:8e7e9ef6b0bd 38 AnalogOut aout(p18);
keithbehrman 0:d83ac315a24c 39
Piachnp 4:ec20064efef4 40 DigitalOut myled(LED2); //To check programming
keithbehrman 1:8e7e9ef6b0bd 41
keithbehrman 0:d83ac315a24c 42
Piachnp 4:ec20064efef4 43 void Demux_LED(int);
Piachnp 4:ec20064efef4 44 void Demux_PD(int);
Piachnp 4:ec20064efef4 45 int median_of_array();
Piachnp 4:ec20064efef4 46 void bubble_sort(int[], int);
keithbehrman 0:d83ac315a24c 47
keithbehrman 1:8e7e9ef6b0bd 48
keithbehrman 0:d83ac315a24c 49 int main()
Piachnp 4:ec20064efef4 50 {
Piachnp 4:ec20064efef4 51 myled = 1;
Piachnp 4:ec20064efef4 52 double time=.000029; // ~10kHz
Piachnp 4:ec20064efef4 53 //Set up all ROS communication
Piachnp 4:ec20064efef4 54 nh.getHardware()->setBaud(BAUD_RATE);
Piachnp 4:ec20064efef4 55 nh.initNode();
Piachnp 4:ec20064efef4 56
Piachnp 4:ec20064efef4 57 //Setup all necessary fields of my MultiArray message (except data) >> See message structure commented before setup()
Piachnp 4:ec20064efef4 58 myDim.label = "readings";
Piachnp 4:ec20064efef4 59 myDim.size = array_length;
Piachnp 4:ec20064efef4 60 myDim.stride = array_length;
Piachnp 4:ec20064efef4 61 myLayout.dim = (std_msgs::MultiArrayDimension *) malloc(sizeof(std_msgs::MultiArrayDimension) * 1);
Piachnp 4:ec20064efef4 62 myLayout.dim[0] = myDim;
Piachnp 4:ec20064efef4 63 myLayout.data_offset = 0;
Piachnp 4:ec20064efef4 64 readings_msg.layout = myLayout;
Piachnp 4:ec20064efef4 65 readings_msg.data = (uint16_t *)malloc(sizeof(int)*array_length);
Piachnp 4:ec20064efef4 66 readings_msg.data_length = array_length;
Piachnp 4:ec20064efef4 67 nh.advertise(pub_sensor);
Piachnp 4:ec20064efef4 68
Piachnp 4:ec20064efef4 69 while(1)
Piachnp 4:ec20064efef4 70 {
Piachnp 4:ec20064efef4 71 //In switching mode, we just go through all the LEDs as fast as possible
Piachnp 4:ec20064efef4 72 //and take readings of the 8 diodes. We take 5 readings of a diode, and just send out the median value.
Piachnp 4:ec20064efef4 73 //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)
Piachnp 4:ec20064efef4 74 //The next 8 values correspond to the next 8 diodes values when LED #1 is ON, and so on....
Piachnp 4:ec20064efef4 75 int idx=0;
Piachnp 4:ec20064efef4 76 int measurements[5];
Piachnp 4:ec20064efef4 77 for(int i=-1;i<number_of_leds;i++) //Start from all LEDs OFF STATE.
Piachnp 4:ec20064efef4 78 {
Piachnp 4:ec20064efef4 79 for(int j=0;j<number_of_diodes;j++)
Piachnp 4:ec20064efef4 80 {
Piachnp 4:ec20064efef4 81 Demux_PD(j);
Piachnp 4:ec20064efef4 82 //Take 5 measurements for this LED/PD combo
Piachnp 4:ec20064efef4 83 for(int k=0;k<5;k++)
Piachnp 4:ec20064efef4 84 {
Piachnp 4:ec20064efef4 85 Demux_LED(i); //Turn on selected LED
Piachnp 4:ec20064efef4 86 measurements[k]= ain.read_u16();
keithbehrman 0:d83ac315a24c 87 wait(time);
Piachnp 4:ec20064efef4 88
Piachnp 4:ec20064efef4 89 Demux_LED(-1); //Turn off LED
Piachnp 4:ec20064efef4 90 ain.read_u16();
Piachnp 4:ec20064efef4 91 wait(time);
keithbehrman 0:d83ac315a24c 92 }
Piachnp 4:ec20064efef4 93 float voltageOut=(measurements[2]+measurements[3]+measurements[4])/3.0; //Average of last three values
Piachnp 4:ec20064efef4 94 aout=voltageOut/65535.0; //Sets Voltage out to Pin 18 for debugging on scope
Piachnp 4:ec20064efef4 95 readings_msg.data[idx] = (uint16_t)voltageOut; //load data field of my message
Piachnp 4:ec20064efef4 96 if(idx<array_length)
Piachnp 4:ec20064efef4 97 idx++;
Piachnp 4:ec20064efef4 98 }
keithbehrman 0:d83ac315a24c 99 }
Piachnp 4:ec20064efef4 100 Demux_LED(-1); //Turn off the LEDs while we put together the ROS package.
Piachnp 4:ec20064efef4 101 pub_sensor.publish(&readings_msg);
Piachnp 4:ec20064efef4 102 nh.spinOnce();
keithbehrman 0:d83ac315a24c 103 }
keithbehrman 0:d83ac315a24c 104 }
keithbehrman 1:8e7e9ef6b0bd 105
keithbehrman 1:8e7e9ef6b0bd 106
Piachnp 4:ec20064efef4 107
Piachnp 4:ec20064efef4 108 //int main()
Piachnp 4:ec20064efef4 109 //{
Piachnp 4:ec20064efef4 110 // Serial pc(USBTX, USBRX); // tx, rx
Piachnp 4:ec20064efef4 111 // pc.baud(115200);
Piachnp 4:ec20064efef4 112 // myled=1;
Piachnp 4:ec20064efef4 113 //
Piachnp 4:ec20064efef4 114 // int led=-1;
Piachnp 4:ec20064efef4 115 // while(1)
Piachnp 4:ec20064efef4 116 // {
Piachnp 4:ec20064efef4 117 // Demux_LED(led);
Piachnp 4:ec20064efef4 118 // for(int pd=0;pd<6;pd++)
Piachnp 4:ec20064efef4 119 // {
Piachnp 4:ec20064efef4 120 // Demux_PD(pd);
Piachnp 4:ec20064efef4 121 // pc.printf("For LED#%d and PD#%d >>> %f \n\r",led,pd,ain.read());
Piachnp 4:ec20064efef4 122 // }
Piachnp 4:ec20064efef4 123 // led++;
Piachnp 4:ec20064efef4 124 // wait(1);
Piachnp 4:ec20064efef4 125 // if(led>5)
Piachnp 4:ec20064efef4 126 // led=-1;
Piachnp 4:ec20064efef4 127 // }
Piachnp 4:ec20064efef4 128 //}
Piachnp 4:ec20064efef4 129
Piachnp 4:ec20064efef4 130
Piachnp 4:ec20064efef4 131 void Demux_LED(int input)
Piachnp 4:ec20064efef4 132 {
Piachnp 4:ec20064efef4 133 if(input>=0 && input<=15)
Piachnp 4:ec20064efef4 134 {
Piachnp 4:ec20064efef4 135 LEDout = 1;
Piachnp 4:ec20064efef4 136 LEDmux3=(input/8)%2; //LSB
Piachnp 4:ec20064efef4 137 LEDmux2=(input/4)%2;
Piachnp 4:ec20064efef4 138 LEDmux1=(input/2)%2;
Piachnp 4:ec20064efef4 139 LEDmux0=input%2; //MSB
Piachnp 4:ec20064efef4 140
Piachnp 4:ec20064efef4 141 }
Piachnp 4:ec20064efef4 142 else
Piachnp 4:ec20064efef4 143 {
Piachnp 4:ec20064efef4 144 LEDout = 0;
Piachnp 4:ec20064efef4 145 }
Piachnp 4:ec20064efef4 146 }
Piachnp 4:ec20064efef4 147
Piachnp 4:ec20064efef4 148 void Demux_PD(int input)
Piachnp 4:ec20064efef4 149 {
Piachnp 4:ec20064efef4 150 if(input>=0 && input<=15)
Piachnp 4:ec20064efef4 151 {
Piachnp 4:ec20064efef4 152 PDmux3=(input/8)%2; //LSB
Piachnp 4:ec20064efef4 153 PDmux2=(input/4)%2;
Piachnp 4:ec20064efef4 154 PDmux1=(input/2)%2;
Piachnp 4:ec20064efef4 155 PDmux0=input%2; //MSB
Piachnp 4:ec20064efef4 156 }
Piachnp 4:ec20064efef4 157 }
Piachnp 4:ec20064efef4 158
Piachnp 4:ec20064efef4 159
Piachnp 4:ec20064efef4 160 int median_of_array()
Piachnp 4:ec20064efef4 161 {
Piachnp 4:ec20064efef4 162 int measurements[NUM_SAMPLES];
Piachnp 4:ec20064efef4 163 for(int i=0;i<NUM_SAMPLES;i++)
Piachnp 4:ec20064efef4 164 {
Piachnp 4:ec20064efef4 165 measurements[i] = ain.read_u16();
Piachnp 4:ec20064efef4 166 }
Piachnp 4:ec20064efef4 167 bubble_sort(measurements,NUM_SAMPLES);
Piachnp 4:ec20064efef4 168 return measurements[(NUM_SAMPLES-1)/2];
Piachnp 4:ec20064efef4 169 }
Piachnp 4:ec20064efef4 170
Piachnp 4:ec20064efef4 171
Piachnp 4:ec20064efef4 172 //Sorting function for computing median values.
Piachnp 4:ec20064efef4 173 void bubble_sort(int array[], int size)
Piachnp 4:ec20064efef4 174 {
Piachnp 4:ec20064efef4 175 int i, j, temp;
Piachnp 4:ec20064efef4 176 for (i=0 ; i<(size-1); i++)
Piachnp 4:ec20064efef4 177 {
Piachnp 4:ec20064efef4 178 for (j=0 ; j< (size-i-1); j++)
Piachnp 4:ec20064efef4 179 {
Piachnp 4:ec20064efef4 180 if (array[j] > array[j+1])
Piachnp 4:ec20064efef4 181 {
Piachnp 4:ec20064efef4 182 /* Swapping */
Piachnp 4:ec20064efef4 183 temp = array[j];
Piachnp 4:ec20064efef4 184 array[j] = array[j+1];
Piachnp 4:ec20064efef4 185 array[j+1] = temp;
Piachnp 4:ec20064efef4 186 }
Piachnp 4:ec20064efef4 187 }
Piachnp 4:ec20064efef4 188 }
Piachnp 4:ec20064efef4 189 }