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-03-07
Revision:
1:8e7e9ef6b0bd
Parent:
0:d83ac315a24c
Child:
2:25f679fea061

File content as of revision 1:8e7e9ef6b0bd:

#include "mbed.h"
#include "FastAnalogIn.h"



Serial pc(USBTX, USBRX); // tx, rx

FastAnalogIn ain(p20); //Fast&Furious:Tokyo Drift Analog Input to PDmux
//AnalogIn 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
AnalogOut aout(p18);


double voltageOut;
double readIn[5];


double alpha; //dummy variable for ADC



int main()
{
    pc.baud(921600);
    //pc.format();

//   double ADCtime=.000000116; //1.6 us
    double time=.000029;  // ~15kHz
//    double time=.0000029; //100 kHz
//    double time=.0003;


    //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 photodiodes

        for(int pd=0; pd<6; pd++) {
            //loop will take 5 measurements for the selected LED/PD combo
            for(int i=0; i<5; i++) {
                LEDout = 1;
                readIn[i]=ain.read();
                wait(time);

                //LEDout = 0;
                //alpha=ain.read();
                //wait(time);
            }

            voltageOut=(readIn[2]+readIn[3]+readIn[4])/3;
            aout=voltageOut;

            //send the diode readings to Matlab
            if(pd==0) {
                pc.printf("1,%f \n", voltageOut);
            } else if(pd==1) {
                pc.printf("2,%f \n", voltageOut+10);
            } else if(pd==2) {
                pc.printf("3,%f \n", voltageOut+20);
            } else if(pd==3) {
                pc.printf("4,%f \n", voltageOut+30);
            } else if(pd==4) {
                pc.printf("5,%f \n", voltageOut+40);
            } else {
                pc.printf("6,%f \n", voltageOut+50);
            }



            //PD multiplexing
            if((PDmux0==0) && (PDmux1==0) && (PDmux2==0)) {
                PDmux0=1;
            } else if((PDmux0==1) && (PDmux1==0) && (PDmux2==0)) {
                PDmux0=0;
                PDmux1=1;
            } else if((PDmux0==0) && (PDmux1==1) && (PDmux2==0)) {
                PDmux0=1;
            } else if((PDmux0==1) && (PDmux1==1) && (PDmux2==0)) {
                PDmux0=0;
                PDmux1=0;
                PDmux2=1;
            } else if((PDmux0==0) && (PDmux1==0) && (PDmux2==1)) {
                PDmux0=1;
            } else {
                PDmux0=0;
                PDmux2=0;
            }

        }
                if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==0)) {
                    LEDmux0=1;
                } else if((LEDmux0==1) && (LEDmux1==0) && (LEDmux2==0)) {
                    LEDmux0=0;
                    LEDmux1=1;
                } else if((LEDmux0==0) && (LEDmux1==1) && (LEDmux2==0)) {
                    LEDmux0=1;
                } else if((LEDmux0==1) && (LEDmux1==1) && (LEDmux2==0)) {
                    LEDmux0=0;
                    LEDmux1=0;
                    LEDmux2=1;
                } else if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==1)) {
                    LEDmux0=1;
                } else {
                    LEDmux0=0;
                    LEDmux2=0;
                }
        



    }
}