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@2:25f679fea061, 2017-03-08 (annotated)

Committer:
keithbehrman
Date:
Wed Mar 08 16:47:56 2017 +0000
Revision:
2:25f679fea061
Parent:
1:8e7e9ef6b0bd
Child:
3:2adce774a137
Ready for ROS;

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"
keithbehrman 0:d83ac315a24c 3
keithbehrman 0:d83ac315a24c 4
keithbehrman 1:8e7e9ef6b0bd 5
keithbehrman 1:8e7e9ef6b0bd 6 Serial pc(USBTX, USBRX); // tx, rx
keithbehrman 1:8e7e9ef6b0bd 7
keithbehrman 1:8e7e9ef6b0bd 8 FastAnalogIn ain(p20); //Fast&Furious:Tokyo Drift Analog Input to PDmux
keithbehrman 1:8e7e9ef6b0bd 9 //AnalogIn ain(p20); //Analog Input to PDmux
keithbehrman 1:8e7e9ef6b0bd 10
keithbehrman 0:d83ac315a24c 11 DigitalOut LEDout(p8); //5V output to LED mux
keithbehrman 0:d83ac315a24c 12 DigitalOut LEDmux0(p9); //s0
keithbehrman 0:d83ac315a24c 13 DigitalOut LEDmux1(p10); //s1
keithbehrman 0:d83ac315a24c 14 DigitalOut LEDmux2(p11); //s2
keithbehrman 0:d83ac315a24c 15 DigitalOut LEDmux3(p12); //s3
keithbehrman 0:d83ac315a24c 16
keithbehrman 0:d83ac315a24c 17 DigitalOut PDmux0(p14); //s0
keithbehrman 0:d83ac315a24c 18 DigitalOut PDmux1(p15); //s1
keithbehrman 0:d83ac315a24c 19 DigitalOut PDmux2(p16); //s2
keithbehrman 0:d83ac315a24c 20 DigitalOut PDmux3(p17); //s3
keithbehrman 1:8e7e9ef6b0bd 21 AnalogOut aout(p18);
keithbehrman 0:d83ac315a24c 22
keithbehrman 2:25f679fea061 23 int LED; //counter for LEDs
keithbehrman 0:d83ac315a24c 24
keithbehrman 1:8e7e9ef6b0bd 25 double voltageOut;
keithbehrman 1:8e7e9ef6b0bd 26 double readIn[5];
keithbehrman 1:8e7e9ef6b0bd 27
keithbehrman 0:d83ac315a24c 28
keithbehrman 0:d83ac315a24c 29 double alpha; //dummy variable for ADC
keithbehrman 0:d83ac315a24c 30
keithbehrman 0:d83ac315a24c 31
keithbehrman 1:8e7e9ef6b0bd 32
keithbehrman 0:d83ac315a24c 33 int main()
keithbehrman 0:d83ac315a24c 34 {
keithbehrman 1:8e7e9ef6b0bd 35 pc.baud(921600);
keithbehrman 1:8e7e9ef6b0bd 36 //pc.format();
keithbehrman 1:8e7e9ef6b0bd 37
keithbehrman 0:d83ac315a24c 38 // double ADCtime=.000000116; //1.6 us
keithbehrman 0:d83ac315a24c 39 double time=.000029; // ~15kHz
keithbehrman 0:d83ac315a24c 40 // double time=.0000029; //100 kHz
keithbehrman 1:8e7e9ef6b0bd 41 // double time=.0003;
keithbehrman 1:8e7e9ef6b0bd 42
keithbehrman 0:d83ac315a24c 43
keithbehrman 0:d83ac315a24c 44 //counters for various while loops
keithbehrman 1:8e7e9ef6b0bd 45 // int mPD=0;
keithbehrman 1:8e7e9ef6b0bd 46 // int nPD=0;
keithbehrman 1:8e7e9ef6b0bd 47 // int mLED=0;
keithbehrman 1:8e7e9ef6b0bd 48 // int nLED=0;
keithbehrman 1:8e7e9ef6b0bd 49 //int pd=0;
keithbehrman 1:8e7e9ef6b0bd 50 //int i=0;
keithbehrman 0:d83ac315a24c 51
keithbehrman 0:d83ac315a24c 52 //boolean bits for multiplexing
keithbehrman 0:d83ac315a24c 53 LEDmux0=0;
keithbehrman 0:d83ac315a24c 54 LEDmux1=0;
keithbehrman 0:d83ac315a24c 55 LEDmux2=0;
keithbehrman 0:d83ac315a24c 56 LEDmux3=0; //MSB is always 0
keithbehrman 0:d83ac315a24c 57 PDmux0=0;
keithbehrman 0:d83ac315a24c 58 PDmux1=0;
keithbehrman 0:d83ac315a24c 59 PDmux2=0;
keithbehrman 0:d83ac315a24c 60 PDmux3=0; //MSB is always 0
keithbehrman 0:d83ac315a24c 61
keithbehrman 0:d83ac315a24c 62
keithbehrman 0:d83ac315a24c 63 //while loop that runs continously through code to constantly give measuremtn while MCU is on
keithbehrman 0:d83ac315a24c 64 while(1) {
keithbehrman 0:d83ac315a24c 65
keithbehrman 0:d83ac315a24c 66
keithbehrman 1:8e7e9ef6b0bd 67 //loop to mux through photodiodes
keithbehrman 1:8e7e9ef6b0bd 68
keithbehrman 1:8e7e9ef6b0bd 69 for(int pd=0; pd<6; pd++) {
keithbehrman 0:d83ac315a24c 70 //loop will take 5 measurements for the selected LED/PD combo
keithbehrman 1:8e7e9ef6b0bd 71 for(int i=0; i<5; i++) {
keithbehrman 0:d83ac315a24c 72 LEDout = 1;
keithbehrman 1:8e7e9ef6b0bd 73 readIn[i]=ain.read();
keithbehrman 0:d83ac315a24c 74 wait(time);
keithbehrman 0:d83ac315a24c 75
keithbehrman 1:8e7e9ef6b0bd 76 //LEDout = 0;
keithbehrman 1:8e7e9ef6b0bd 77 //alpha=ain.read();
keithbehrman 1:8e7e9ef6b0bd 78 //wait(time);
keithbehrman 0:d83ac315a24c 79 }
keithbehrman 1:8e7e9ef6b0bd 80
keithbehrman 1:8e7e9ef6b0bd 81 voltageOut=(readIn[2]+readIn[3]+readIn[4])/3;
keithbehrman 2:25f679fea061 82 aout=voltageOut; //Sets Voltage out to Pin 18 for debugging on scope
keithbehrman 0:d83ac315a24c 83
keithbehrman 2:25f679fea061 84 //send the diode readings to MATlab in format: LED,PD,Voltageout
keithbehrman 2:25f679fea061 85 //I think this is where you want to report voltageOut to ROS for the PD/LED combo
keithbehrman 2:25f679fea061 86
keithbehrman 2:25f679fea061 87 pc.printf("%d,%d,%f \n",LED,pd,voltageOut);
keithbehrman 0:d83ac315a24c 88
keithbehrman 1:8e7e9ef6b0bd 89
keithbehrman 1:8e7e9ef6b0bd 90
keithbehrman 1:8e7e9ef6b0bd 91 //PD multiplexing
keithbehrman 1:8e7e9ef6b0bd 92 if((PDmux0==0) && (PDmux1==0) && (PDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 93 PDmux0=1;
keithbehrman 1:8e7e9ef6b0bd 94 } else if((PDmux0==1) && (PDmux1==0) && (PDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 95 PDmux0=0;
keithbehrman 1:8e7e9ef6b0bd 96 PDmux1=1;
keithbehrman 1:8e7e9ef6b0bd 97 } else if((PDmux0==0) && (PDmux1==1) && (PDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 98 PDmux0=1;
keithbehrman 1:8e7e9ef6b0bd 99 } else if((PDmux0==1) && (PDmux1==1) && (PDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 100 PDmux0=0;
keithbehrman 1:8e7e9ef6b0bd 101 PDmux1=0;
keithbehrman 1:8e7e9ef6b0bd 102 PDmux2=1;
keithbehrman 1:8e7e9ef6b0bd 103 } else if((PDmux0==0) && (PDmux1==0) && (PDmux2==1)) {
keithbehrman 1:8e7e9ef6b0bd 104 PDmux0=1;
keithbehrman 0:d83ac315a24c 105 } else {
keithbehrman 1:8e7e9ef6b0bd 106 PDmux0=0;
keithbehrman 1:8e7e9ef6b0bd 107 PDmux2=0;
keithbehrman 0:d83ac315a24c 108 }
keithbehrman 0:d83ac315a24c 109
keithbehrman 0:d83ac315a24c 110 }
keithbehrman 1:8e7e9ef6b0bd 111 if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 112 LEDmux0=1;
keithbehrman 1:8e7e9ef6b0bd 113 } else if((LEDmux0==1) && (LEDmux1==0) && (LEDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 114 LEDmux0=0;
keithbehrman 1:8e7e9ef6b0bd 115 LEDmux1=1;
keithbehrman 1:8e7e9ef6b0bd 116 } else if((LEDmux0==0) && (LEDmux1==1) && (LEDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 117 LEDmux0=1;
keithbehrman 1:8e7e9ef6b0bd 118 } else if((LEDmux0==1) && (LEDmux1==1) && (LEDmux2==0)) {
keithbehrman 1:8e7e9ef6b0bd 119 LEDmux0=0;
keithbehrman 1:8e7e9ef6b0bd 120 LEDmux1=0;
keithbehrman 1:8e7e9ef6b0bd 121 LEDmux2=1;
keithbehrman 1:8e7e9ef6b0bd 122 } else if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==1)) {
keithbehrman 1:8e7e9ef6b0bd 123 LEDmux0=1;
keithbehrman 1:8e7e9ef6b0bd 124 } else {
keithbehrman 1:8e7e9ef6b0bd 125 LEDmux0=0;
keithbehrman 1:8e7e9ef6b0bd 126 LEDmux2=0;
keithbehrman 1:8e7e9ef6b0bd 127 }
keithbehrman 1:8e7e9ef6b0bd 128
keithbehrman 0:d83ac315a24c 129
keithbehrman 1:8e7e9ef6b0bd 130
keithbehrman 0:d83ac315a24c 131
keithbehrman 0:d83ac315a24c 132 }
keithbehrman 0:d83ac315a24c 133 }
keithbehrman 1:8e7e9ef6b0bd 134
keithbehrman 1:8e7e9ef6b0bd 135