main.cpp

00001 #include "mbed.h"
00002 #include <stdlib.h>
00003 #include "LCD.h"
00004 #include "MISC.h"
00005 #include <math.h>
00006 #include "ServoClass.h"
00007 #include "Controller.h"
00008 LocalFileSystem local("local");
00009 
00010 MISC misc;
00011 Controller controller_x(0);
00012 Controller controller_y(0);
00013 
00014 #include "FNCS.h"
00015 
00016 DigitalOut myled(LED1);
00017 DigitalOut led2(LED2);
00018 
00019 DigitalIn mic1(p12);
00020 DigitalIn mic2(p13);
00021 DigitalIn mic3(p14);
00022 DigitalIn mic4(p15);
00023 DigitalOut resetFF(p16);
00024 DigitalOut PowerOn12V(p17);
00025 
00026 
00027 
00028 bool flag_mic1 = false;
00029 bool flag_mic2 = false;
00030 bool flag_mic3 = false;
00031 bool flag_mic4 = false;
00032 
00033 int difCntr_1 = 0;
00034 int difCntr_2 = 0;
00035 int mic1_val = 0;
00036 int mic2_val = 0;
00037 int mic3_val = 0;
00038 int mic4_val = 0;
00039 
00040 int valxAxis = 0;
00041 int valyAxis = 0;
00042 
00043 
00044 int data1[1001] = {0};
00045 FILE *fp1;
00046 int cntr1 = 0;
00047 
00048 int data2[1001] = {0};
00049 FILE *fp2;
00050 
00051 int data3[100] = {0};
00052 FILE *fp3;
00053 
00054 int data4[100] = {0};
00055 FILE *fp4;
00056 
00057 int main()
00058 {
00059 
00060     init();
00061     initServos();
00062     wait(2);
00063     PowerOn12V = 1;
00064 
00065     movePosTopSlow();
00066     wait(0.1);
00067     movePosBottomSlow();
00068     wait(0.1);
00069     movePosTopSlow();
00070     wait(0.1);
00071     movePosLowSlow();
00072     wait(0.1);
00073     movePosTopSlow();
00074     wait(0.1);
00075 
00076 
00077     while(cntr1 < 1001) {
00078         difCntr_1 = 0;
00079         difCntr_2 = 0;
00080         flag_mic1 = false;
00081         flag_mic2 = false;
00082         flag_mic3 = false;
00083         flag_mic4 = false;
00084         clearDispl();
00085         if(cntr1 < 2) {
00086             fastWriteString("ready.");
00087         } else {
00088             fastWriteString(misc.intToString(cntr1 - 1));
00089         }
00090 
00091         while(!flag_mic1 || !flag_mic2 || !flag_mic3 || !flag_mic4) {
00092             //loop until all mic signals got set.
00093 
00094             //check mic1
00095             if(mic1 && !flag_mic1) {
00096                 //reset Counter if first
00097                 if(!flag_mic2) {
00098                     difCntr_1 = 0;
00099                 }
00100                 //set the mic value to the current difCntr value.
00101                 mic1_val = difCntr_1;
00102                 flag_mic1 = true;
00103             }
00104             //check mic2
00105             if(mic2 && !flag_mic2) {
00106                 //reset Counter if first
00107                 if(!flag_mic1) {
00108                     difCntr_1 = 0;
00109                 }
00110                 //set the mic value to the current difCntr value.
00111                 mic2_val = difCntr_1;
00112                 flag_mic2 = true;
00113             }
00114             //check mic3
00115             if(mic3 && !flag_mic3) {
00116                 //reset Counter if first
00117                 if(!flag_mic4) {
00118                     difCntr_2 = 0;
00119                 }
00120                 //set the mic value to the current difCntr value.
00121                 mic3_val = difCntr_2;
00122                 flag_mic3 = true;
00123             }
00124             //check mic4
00125             if(mic4 && !flag_mic4) {
00126                 //reset Counter if first
00127                 if(!flag_mic3) {
00128                     difCntr_2 = 0;
00129                 }
00130                 //set the mic value to the current difCntr value.
00131                 mic4_val = difCntr_2;
00132                 flag_mic4 = true;
00133             }
00134 
00135             difCntr_1++;
00136             difCntr_2++;
00137         }
00138 
00139         resetCorrection();
00140 
00141         //valxAxis is + when the ball is near mic2 and - when the Ball is near mic1
00142         valxAxis = misc.ValCut(mic1_val) - misc.ValCut(mic2_val);
00143         //update the X-axis controller
00144         controller_x.updateData(valxAxis);
00145         correctPlateToRight(controller_x.getOutput());
00146 
00147         //valyAxis is + when the ball is near mic4 and - when the Ball is near mic3
00148         valyAxis = misc.ValCut(mic3_val) - misc.ValCut(mic4_val);
00149         //update the Y-axis controller
00150         controller_y.updateData(valyAxis);
00151         correctPlateToBack(controller_y.getOutput());
00152 
00153         movePosBottom();
00154 
00155 
00156         data1[cntr1] = valxAxis;
00157         data2[cntr1++] = valyAxis;
00158 
00159         clearDispl();
00160         //fastWriteString("mic1: ");
00161         fastWriteString(misc.intToString(mic1_val));
00162         setCursorPos(41);
00163         //fastWriteString("mic2: ");
00164         fastWriteString(misc.intToString(mic2_val));
00165 
00166         clearDispl();
00167         //fastWriteString("mic3: ");
00168         fastWriteString(misc.intToString(mic3_val));
00169         setCursorPos(41);
00170         //fastWriteString("mic4: ");
00171         fastWriteString(misc.intToString(mic4_val));
00172 
00173         wait(0.01);
00174         resetFF = 1;
00175         while(mic1 || mic2 || mic3 || mic4);
00176         resetFF = 0;
00177         movePosTop();
00178     }
00179 
00180     clearDispl();
00181     fastWriteString("cntr1 > 100");
00182 
00183     if ( NULL == (fp1 = fopen( "/local/S1.csv", "w" )) ) {
00184         error( "" );
00185     }
00186     for ( int i = 0; i < cntr1; i++ ) {
00187         fprintf( fp1, "%d\n", data1[i] );
00188     }
00189     fclose( fp1 );
00190 
00191     if ( NULL == (fp2 = fopen( "/local/S2.csv", "w" )) ) {
00192         error( "" );
00193     }
00194     for ( int i = 0; i < cntr1; i++ ) {
00195         fprintf( fp2, "%d\n", data2[i] );
00196     }
00197     fclose( fp2 );
00198 
00199     if ( NULL == (fp3 = fopen( "/local/S3.csv", "w" )) ) {
00200         error( "" );
00201     }
00202     for ( int i = 0; i < cntr1; i++ ) {
00203         fprintf( fp3, "%d\n", data3[i] );
00204     }
00205     fclose( fp3 );
00206 
00207     if ( NULL == (fp4 = fopen( "/local/S4.csv", "w" )) ) {
00208         error( "" );
00209     }
00210     for ( int i = 0; i < cntr1; i++ ) {
00211         fprintf( fp2, "%d\n", data4[i] );
00212     }
00213     fclose( fp4 );
00214 
00215     clearDispl();
00216     fastWriteString("data saved.");
00217 
00218     //END HERE
00219     while(1);
00220 
00221 
00222 
00223 }
00224 
00225 
00226 
00227 
00228 
00229 
00230 
00231 
00232 
00233 
00234 
00235 
00236