Synchronous detection code with ROS communication for optic sensor

Dependencies:   FastAnalogIn mbed ros_lib_indigo

Fork of Mirror_Top_Indenter_ROS by CLUE

Revision:
0:d83ac315a24c
Child:
1:8e7e9ef6b0bd
diff -r 000000000000 -r d83ac315a24c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 17 20:37:32 2017 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "FastAnalogIn.h"
+
+
+FastAnalogIn 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
+DigitalOut LEDmux3(p12); //s3
+
+DigitalOut PDmux0(p14); //s0
+DigitalOut PDmux1(p15); //s1
+DigitalOut PDmux2(p16); //s2
+DigitalOut PDmux3(p17); //s3
+
+
+
+double alpha; //dummy variable for ADC
+
+
+int main()
+{
+//   double ADCtime=.000000116; //1.6 us
+    double time=.000029;  // ~15kHz
+//    double time=.0000029; //100 kHz
+//    double time=.003;
+
+    //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 the 8 photodiodes
+        while(pd<8) {
+            //loop will take 5 measurements for the selected LED/PD combo
+            while(i<5) {
+                LEDout = 1;
+                alpha=ain.read();
+                wait(time);
+
+                LEDout = 0;
+                alpha=ain.read();
+                wait(time);
+                i++;
+            }
+            i=0;
+
+            PDmux0=!PDmux0; //flip LSB
+
+            //flips 2nd bit every other cycle
+            if (mPD==1) {
+                PDmux1=!PDmux1;
+                mPD=0;
+            } else {
+                mPD++;
+            }
+
+            //flips 3rd bit after every 4th cycle
+            if (nPD==3) {
+                PDmux2=!PDmux2;
+                nPD=0;
+            } else {
+                nPD++;
+            }
+            pd++;
+        }
+        pd=0;
+
+        //same multiplexing code above, but for LEDs
+        LEDmux0=!LEDmux0;
+
+        if (mLED==1) {
+            LEDmux1=!LEDmux1;
+            mLED=0;
+        } else {
+            mLED++;
+        }
+
+        if (nLED==3) {
+            LEDmux2=!LEDmux2;
+            nLED=0;
+        } else {
+            nLED++;
+        }
+
+    }
+}