Diff: main.cpp

Revision:

0:14b64813c04d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 29 00:02:00 2016 +0000
@@ -0,0 +1,236 @@
+#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);
+
+
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+