File content as of revision 0:14b64813c04d:

#include "mbed.h"
#include <stdlib.h>
#include "LCD.h"
#include "MISC.h"
#include <math.h>
#include "ServoClass.h"
#include "Controller.h"
LocalFileSystem local("local");

MISC misc;
Controller controller_x(0);
Controller controller_y(0);

#include "FNCS.h"

DigitalOut myled(LED1);
DigitalOut led2(LED2);

DigitalIn mic1(p12);
DigitalIn mic2(p13);
DigitalIn mic3(p14);
DigitalIn mic4(p15);
DigitalOut resetFF(p16);
DigitalOut PowerOn12V(p17);



bool flag_mic1 = false;
bool flag_mic2 = false;
bool flag_mic3 = false;
bool flag_mic4 = false;

int difCntr_1 = 0;
int difCntr_2 = 0;
int mic1_val = 0;
int mic2_val = 0;
int mic3_val = 0;
int mic4_val = 0;

int valxAxis = 0;
int valyAxis = 0;


int data1[1001] = {0};
FILE *fp1;
int cntr1 = 0;

int data2[1001] = {0};
FILE *fp2;

int data3[100] = {0};
FILE *fp3;

int data4[100] = {0};
FILE *fp4;

int main()
{

    init();
    initServos();
    wait(2);
    PowerOn12V = 1;

    movePosTopSlow();
    wait(0.1);
    movePosBottomSlow();
    wait(0.1);
    movePosTopSlow();
    wait(0.1);
    movePosLowSlow();
    wait(0.1);
    movePosTopSlow();
    wait(0.1);


    while(cntr1 < 1001) {
        difCntr_1 = 0;
        difCntr_2 = 0;
        flag_mic1 = false;
        flag_mic2 = false;
        flag_mic3 = false;
        flag_mic4 = false;
        clearDispl();
        if(cntr1 < 2) {
            fastWriteString("ready.");
        } else {
            fastWriteString(misc.intToString(cntr1 - 1));
        }

        while(!flag_mic1 || !flag_mic2 || !flag_mic3 || !flag_mic4) {
            //loop until all mic signals got set.

            //check mic1
            if(mic1 && !flag_mic1) {
                //reset Counter if first
                if(!flag_mic2) {
                    difCntr_1 = 0;
                }
                //set the mic value to the current difCntr value.
                mic1_val = difCntr_1;
                flag_mic1 = true;
            }
            //check mic2
            if(mic2 && !flag_mic2) {
                //reset Counter if first
                if(!flag_mic1) {
                    difCntr_1 = 0;
                }
                //set the mic value to the current difCntr value.
                mic2_val = difCntr_1;
                flag_mic2 = true;
            }
            //check mic3
            if(mic3 && !flag_mic3) {
                //reset Counter if first
                if(!flag_mic4) {
                    difCntr_2 = 0;
                }
                //set the mic value to the current difCntr value.
                mic3_val = difCntr_2;
                flag_mic3 = true;
            }
            //check mic4
            if(mic4 && !flag_mic4) {
                //reset Counter if first
                if(!flag_mic3) {
                    difCntr_2 = 0;
                }
                //set the mic value to the current difCntr value.
                mic4_val = difCntr_2;
                flag_mic4 = true;
            }

            difCntr_1++;
            difCntr_2++;
        }

        resetCorrection();

        //valxAxis is + when the ball is near mic2 and - when the Ball is near mic1
        valxAxis = misc.ValCut(mic1_val) - misc.ValCut(mic2_val);
        //update the X-axis controller
        controller_x.updateData(valxAxis);
        correctPlateToRight(controller_x.getOutput());

        //valyAxis is + when the ball is near mic4 and - when the Ball is near mic3
        valyAxis = misc.ValCut(mic3_val) - misc.ValCut(mic4_val);
        //update the Y-axis controller
        controller_y.updateData(valyAxis);
        correctPlateToBack(controller_y.getOutput());

        movePosBottom();


        data1[cntr1] = valxAxis;
        data2[cntr1++] = valyAxis;

        clearDispl();
        //fastWriteString("mic1: ");
        fastWriteString(misc.intToString(mic1_val));
        setCursorPos(41);
        //fastWriteString("mic2: ");
        fastWriteString(misc.intToString(mic2_val));

        clearDispl();
        //fastWriteString("mic3: ");
        fastWriteString(misc.intToString(mic3_val));
        setCursorPos(41);
        //fastWriteString("mic4: ");
        fastWriteString(misc.intToString(mic4_val));

        wait(0.01);
        resetFF = 1;
        while(mic1 || mic2 || mic3 || mic4);
        resetFF = 0;
        movePosTop();
    }

    clearDispl();
    fastWriteString("cntr1 > 100");

    if ( NULL == (fp1 = fopen( "/local/S1.csv", "w" )) ) {
        error( "" );
    }
    for ( int i = 0; i < cntr1; i++ ) {
        fprintf( fp1, "%d\n", data1[i] );
    }
    fclose( fp1 );

    if ( NULL == (fp2 = fopen( "/local/S2.csv", "w" )) ) {
        error( "" );
    }
    for ( int i = 0; i < cntr1; i++ ) {
        fprintf( fp2, "%d\n", data2[i] );
    }
    fclose( fp2 );

    if ( NULL == (fp3 = fopen( "/local/S3.csv", "w" )) ) {
        error( "" );
    }
    for ( int i = 0; i < cntr1; i++ ) {
        fprintf( fp3, "%d\n", data3[i] );
    }
    fclose( fp3 );

    if ( NULL == (fp4 = fopen( "/local/S4.csv", "w" )) ) {
        error( "" );
    }
    for ( int i = 0; i < cntr1; i++ ) {
        fprintf( fp2, "%d\n", data4[i] );
    }
    fclose( fp4 );

    clearDispl();
    fastWriteString("data saved.");

    //END HERE
    while(1);



}