Synchronous detection code with ROS communication for optic sensor
Dependencies: FastAnalogIn mbed ros_lib_indigo
Fork of Mirror_Top_Indenter_ROS by
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?
User | Revision | Line number | New 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 | } |