File content as of revision 4:ae2202b25d49:

#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 timing



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

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



    //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; //Average of last three values
            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;
            }


        }//end of PD for loop


        //LED multiplexing
        if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==0)) {
            LEDmux0=1;
            LED=1;
        } else if((LEDmux0==1) && (LEDmux1==0) && (LEDmux2==0)) {
            LEDmux0=0;
            LEDmux1=1;
            LED=2;
        } else if((LEDmux0==0) && (LEDmux1==1) && (LEDmux2==0)) {
            LEDmux0=1;
            LED=3;
        } else if((LEDmux0==1) && (LEDmux1==1) && (LEDmux2==0)) {
            LEDmux0=0;
            LEDmux1=0;
            LEDmux2=1;
            LED=4;
        } else if((LEDmux0==0) && (LEDmux1==0) && (LEDmux2==1)) {
            LEDmux0=1;
            LED=5;
        } else {
            LEDmux0=0;
            LEDmux2=0;
            LED=0;
        }

    }//end of While(1) loop
}