CLUE / Mbed 2 deprecated Mirror_Top_Indenter_ROS_NoSync

No Synchronous detection code with ROS communication enabled

Dependencies:   FastAnalogIn mbed ros_lib_indigo

Fork of Mirror_Top_Indenter_ROS by CLUE

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

Committer:
Piachnp
Date:
Tue Mar 21 14:16:05 2017 +0000
Revision:
4:a6db36142bf8
Parent:
3:2adce774a137 
Working version of ROS code for the Non-Synchronous detection case

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