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

Committer:
keithbehrman
Date:
2017-02-17
Revision:
0:d83ac315a24c
Child:
1:8e7e9ef6b0bd

File content as of revision 0:d83ac315a24c:

#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++;
        }

    }
}