File content as of revision 2:25f679fea061:

#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);

int LED; //counter for LEDs

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; //Sets Voltage out to Pin 18 for debugging on scope

            //send the diode readings to MATlab in format: LED,PD,Voltageout
            //I think this is where you want to report voltageOut to ROS for the PD/LED combo
            
            pc.printf("%d,%d,%f \n",LED,pd,voltageOut);



            //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;
                }
        



    }
}