Tobias Kuhn / Mbed 2 deprecated LCD_XpressoBoard_3_SoundSens

program for ping pong robot

Dependencies:   mbed

main.cpp@0:14b64813c04d, 2016-06-29 (annotated)

Committer:
pnpako
Date:
Wed Jun 29 00:02:00 2016 +0000
Revision:
0:14b64813c04d
ping pong robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pnpako 0:14b64813c04d 1 #include "mbed.h"
pnpako 0:14b64813c04d 2 #include <stdlib.h>
pnpako 0:14b64813c04d 3 #include "LCD.h"
pnpako 0:14b64813c04d 4 #include "MISC.h"
pnpako 0:14b64813c04d 5 #include <math.h>
pnpako 0:14b64813c04d 6 #include "ServoClass.h"
pnpako 0:14b64813c04d 7 #include "Controller.h"
pnpako 0:14b64813c04d 8 LocalFileSystem local("local");
pnpako 0:14b64813c04d 9
pnpako 0:14b64813c04d 10 MISC misc;
pnpako 0:14b64813c04d 11 Controller controller_x(0);
pnpako 0:14b64813c04d 12 Controller controller_y(0);
pnpako 0:14b64813c04d 13
pnpako 0:14b64813c04d 14 #include "FNCS.h"
pnpako 0:14b64813c04d 15
pnpako 0:14b64813c04d 16 DigitalOut myled(LED1);
pnpako 0:14b64813c04d 17 DigitalOut led2(LED2);
pnpako 0:14b64813c04d 18
pnpako 0:14b64813c04d 19 DigitalIn mic1(p12);
pnpako 0:14b64813c04d 20 DigitalIn mic2(p13);
pnpako 0:14b64813c04d 21 DigitalIn mic3(p14);
pnpako 0:14b64813c04d 22 DigitalIn mic4(p15);
pnpako 0:14b64813c04d 23 DigitalOut resetFF(p16);
pnpako 0:14b64813c04d 24 DigitalOut PowerOn12V(p17);
pnpako 0:14b64813c04d 25
pnpako 0:14b64813c04d 26
pnpako 0:14b64813c04d 27
pnpako 0:14b64813c04d 28 bool flag_mic1 = false;
pnpako 0:14b64813c04d 29 bool flag_mic2 = false;
pnpako 0:14b64813c04d 30 bool flag_mic3 = false;
pnpako 0:14b64813c04d 31 bool flag_mic4 = false;
pnpako 0:14b64813c04d 32
pnpako 0:14b64813c04d 33 int difCntr_1 = 0;
pnpako 0:14b64813c04d 34 int difCntr_2 = 0;
pnpako 0:14b64813c04d 35 int mic1_val = 0;
pnpako 0:14b64813c04d 36 int mic2_val = 0;
pnpako 0:14b64813c04d 37 int mic3_val = 0;
pnpako 0:14b64813c04d 38 int mic4_val = 0;
pnpako 0:14b64813c04d 39
pnpako 0:14b64813c04d 40 int valxAxis = 0;
pnpako 0:14b64813c04d 41 int valyAxis = 0;
pnpako 0:14b64813c04d 42
pnpako 0:14b64813c04d 43
pnpako 0:14b64813c04d 44 int data1[1001] = {0};
pnpako 0:14b64813c04d 45 FILE *fp1;
pnpako 0:14b64813c04d 46 int cntr1 = 0;
pnpako 0:14b64813c04d 47
pnpako 0:14b64813c04d 48 int data2[1001] = {0};
pnpako 0:14b64813c04d 49 FILE *fp2;
pnpako 0:14b64813c04d 50
pnpako 0:14b64813c04d 51 int data3[100] = {0};
pnpako 0:14b64813c04d 52 FILE *fp3;
pnpako 0:14b64813c04d 53
pnpako 0:14b64813c04d 54 int data4[100] = {0};
pnpako 0:14b64813c04d 55 FILE *fp4;
pnpako 0:14b64813c04d 56
pnpako 0:14b64813c04d 57 int main()
pnpako 0:14b64813c04d 58 {
pnpako 0:14b64813c04d 59
pnpako 0:14b64813c04d 60 init();
pnpako 0:14b64813c04d 61 initServos();
pnpako 0:14b64813c04d 62 wait(2);
pnpako 0:14b64813c04d 63 PowerOn12V = 1;
pnpako 0:14b64813c04d 64
pnpako 0:14b64813c04d 65 movePosTopSlow();
pnpako 0:14b64813c04d 66 wait(0.1);
pnpako 0:14b64813c04d 67 movePosBottomSlow();
pnpako 0:14b64813c04d 68 wait(0.1);
pnpako 0:14b64813c04d 69 movePosTopSlow();
pnpako 0:14b64813c04d 70 wait(0.1);
pnpako 0:14b64813c04d 71 movePosLowSlow();
pnpako 0:14b64813c04d 72 wait(0.1);
pnpako 0:14b64813c04d 73 movePosTopSlow();
pnpako 0:14b64813c04d 74 wait(0.1);
pnpako 0:14b64813c04d 75
pnpako 0:14b64813c04d 76
pnpako 0:14b64813c04d 77 while(cntr1 < 1001) {
pnpako 0:14b64813c04d 78 difCntr_1 = 0;
pnpako 0:14b64813c04d 79 difCntr_2 = 0;
pnpako 0:14b64813c04d 80 flag_mic1 = false;
pnpako 0:14b64813c04d 81 flag_mic2 = false;
pnpako 0:14b64813c04d 82 flag_mic3 = false;
pnpako 0:14b64813c04d 83 flag_mic4 = false;
pnpako 0:14b64813c04d 84 clearDispl();
pnpako 0:14b64813c04d 85 if(cntr1 < 2) {
pnpako 0:14b64813c04d 86 fastWriteString("ready.");
pnpako 0:14b64813c04d 87 } else {
pnpako 0:14b64813c04d 88 fastWriteString(misc.intToString(cntr1 - 1));
pnpako 0:14b64813c04d 89 }
pnpako 0:14b64813c04d 90
pnpako 0:14b64813c04d 91 while(!flag_mic1 || !flag_mic2 || !flag_mic3 || !flag_mic4) {
pnpako 0:14b64813c04d 92 //loop until all mic signals got set.
pnpako 0:14b64813c04d 93
pnpako 0:14b64813c04d 94 //check mic1
pnpako 0:14b64813c04d 95 if(mic1 && !flag_mic1) {
pnpako 0:14b64813c04d 96 //reset Counter if first
pnpako 0:14b64813c04d 97 if(!flag_mic2) {
pnpako 0:14b64813c04d 98 difCntr_1 = 0;
pnpako 0:14b64813c04d 99 }
pnpako 0:14b64813c04d 100 //set the mic value to the current difCntr value.
pnpako 0:14b64813c04d 101 mic1_val = difCntr_1;
pnpako 0:14b64813c04d 102 flag_mic1 = true;
pnpako 0:14b64813c04d 103 }
pnpako 0:14b64813c04d 104 //check mic2
pnpako 0:14b64813c04d 105 if(mic2 && !flag_mic2) {
pnpako 0:14b64813c04d 106 //reset Counter if first
pnpako 0:14b64813c04d 107 if(!flag_mic1) {
pnpako 0:14b64813c04d 108 difCntr_1 = 0;
pnpako 0:14b64813c04d 109 }
pnpako 0:14b64813c04d 110 //set the mic value to the current difCntr value.
pnpako 0:14b64813c04d 111 mic2_val = difCntr_1;
pnpako 0:14b64813c04d 112 flag_mic2 = true;
pnpako 0:14b64813c04d 113 }
pnpako 0:14b64813c04d 114 //check mic3
pnpako 0:14b64813c04d 115 if(mic3 && !flag_mic3) {
pnpako 0:14b64813c04d 116 //reset Counter if first
pnpako 0:14b64813c04d 117 if(!flag_mic4) {
pnpako 0:14b64813c04d 118 difCntr_2 = 0;
pnpako 0:14b64813c04d 119 }
pnpako 0:14b64813c04d 120 //set the mic value to the current difCntr value.
pnpako 0:14b64813c04d 121 mic3_val = difCntr_2;
pnpako 0:14b64813c04d 122 flag_mic3 = true;
pnpako 0:14b64813c04d 123 }
pnpako 0:14b64813c04d 124 //check mic4
pnpako 0:14b64813c04d 125 if(mic4 && !flag_mic4) {
pnpako 0:14b64813c04d 126 //reset Counter if first
pnpako 0:14b64813c04d 127 if(!flag_mic3) {
pnpako 0:14b64813c04d 128 difCntr_2 = 0;
pnpako 0:14b64813c04d 129 }
pnpako 0:14b64813c04d 130 //set the mic value to the current difCntr value.
pnpako 0:14b64813c04d 131 mic4_val = difCntr_2;
pnpako 0:14b64813c04d 132 flag_mic4 = true;
pnpako 0:14b64813c04d 133 }
pnpako 0:14b64813c04d 134
pnpako 0:14b64813c04d 135 difCntr_1++;
pnpako 0:14b64813c04d 136 difCntr_2++;
pnpako 0:14b64813c04d 137 }
pnpako 0:14b64813c04d 138
pnpako 0:14b64813c04d 139 resetCorrection();
pnpako 0:14b64813c04d 140
pnpako 0:14b64813c04d 141 //valxAxis is + when the ball is near mic2 and - when the Ball is near mic1
pnpako 0:14b64813c04d 142 valxAxis = misc.ValCut(mic1_val) - misc.ValCut(mic2_val);
pnpako 0:14b64813c04d 143 //update the X-axis controller
pnpako 0:14b64813c04d 144 controller_x.updateData(valxAxis);
pnpako 0:14b64813c04d 145 correctPlateToRight(controller_x.getOutput());
pnpako 0:14b64813c04d 146
pnpako 0:14b64813c04d 147 //valyAxis is + when the ball is near mic4 and - when the Ball is near mic3
pnpako 0:14b64813c04d 148 valyAxis = misc.ValCut(mic3_val) - misc.ValCut(mic4_val);
pnpako 0:14b64813c04d 149 //update the Y-axis controller
pnpako 0:14b64813c04d 150 controller_y.updateData(valyAxis);
pnpako 0:14b64813c04d 151 correctPlateToBack(controller_y.getOutput());
pnpako 0:14b64813c04d 152
pnpako 0:14b64813c04d 153 movePosBottom();
pnpako 0:14b64813c04d 154
pnpako 0:14b64813c04d 155
pnpako 0:14b64813c04d 156 data1[cntr1] = valxAxis;
pnpako 0:14b64813c04d 157 data2[cntr1++] = valyAxis;
pnpako 0:14b64813c04d 158
pnpako 0:14b64813c04d 159 clearDispl();
pnpako 0:14b64813c04d 160 //fastWriteString("mic1: ");
pnpako 0:14b64813c04d 161 fastWriteString(misc.intToString(mic1_val));
pnpako 0:14b64813c04d 162 setCursorPos(41);
pnpako 0:14b64813c04d 163 //fastWriteString("mic2: ");
pnpako 0:14b64813c04d 164 fastWriteString(misc.intToString(mic2_val));
pnpako 0:14b64813c04d 165
pnpako 0:14b64813c04d 166 clearDispl();
pnpako 0:14b64813c04d 167 //fastWriteString("mic3: ");
pnpako 0:14b64813c04d 168 fastWriteString(misc.intToString(mic3_val));
pnpako 0:14b64813c04d 169 setCursorPos(41);
pnpako 0:14b64813c04d 170 //fastWriteString("mic4: ");
pnpako 0:14b64813c04d 171 fastWriteString(misc.intToString(mic4_val));
pnpako 0:14b64813c04d 172
pnpako 0:14b64813c04d 173 wait(0.01);
pnpako 0:14b64813c04d 174 resetFF = 1;
pnpako 0:14b64813c04d 175 while(mic1 || mic2 || mic3 || mic4);
pnpako 0:14b64813c04d 176 resetFF = 0;
pnpako 0:14b64813c04d 177 movePosTop();
pnpako 0:14b64813c04d 178 }
pnpako 0:14b64813c04d 179
pnpako 0:14b64813c04d 180 clearDispl();
pnpako 0:14b64813c04d 181 fastWriteString("cntr1 > 100");
pnpako 0:14b64813c04d 182
pnpako 0:14b64813c04d 183 if ( NULL == (fp1 = fopen( "/local/S1.csv", "w" )) ) {
pnpako 0:14b64813c04d 184 error( "" );
pnpako 0:14b64813c04d 185 }
pnpako 0:14b64813c04d 186 for ( int i = 0; i < cntr1; i++ ) {
pnpako 0:14b64813c04d 187 fprintf( fp1, "%d\n", data1[i] );
pnpako 0:14b64813c04d 188 }
pnpako 0:14b64813c04d 189 fclose( fp1 );
pnpako 0:14b64813c04d 190
pnpako 0:14b64813c04d 191 if ( NULL == (fp2 = fopen( "/local/S2.csv", "w" )) ) {
pnpako 0:14b64813c04d 192 error( "" );
pnpako 0:14b64813c04d 193 }
pnpako 0:14b64813c04d 194 for ( int i = 0; i < cntr1; i++ ) {
pnpako 0:14b64813c04d 195 fprintf( fp2, "%d\n", data2[i] );
pnpako 0:14b64813c04d 196 }
pnpako 0:14b64813c04d 197 fclose( fp2 );
pnpako 0:14b64813c04d 198
pnpako 0:14b64813c04d 199 if ( NULL == (fp3 = fopen( "/local/S3.csv", "w" )) ) {
pnpako 0:14b64813c04d 200 error( "" );
pnpako 0:14b64813c04d 201 }
pnpako 0:14b64813c04d 202 for ( int i = 0; i < cntr1; i++ ) {
pnpako 0:14b64813c04d 203 fprintf( fp3, "%d\n", data3[i] );
pnpako 0:14b64813c04d 204 }
pnpako 0:14b64813c04d 205 fclose( fp3 );
pnpako 0:14b64813c04d 206
pnpako 0:14b64813c04d 207 if ( NULL == (fp4 = fopen( "/local/S4.csv", "w" )) ) {
pnpako 0:14b64813c04d 208 error( "" );
pnpako 0:14b64813c04d 209 }
pnpako 0:14b64813c04d 210 for ( int i = 0; i < cntr1; i++ ) {
pnpako 0:14b64813c04d 211 fprintf( fp2, "%d\n", data4[i] );
pnpako 0:14b64813c04d 212 }
pnpako 0:14b64813c04d 213 fclose( fp4 );
pnpako 0:14b64813c04d 214
pnpako 0:14b64813c04d 215 clearDispl();
pnpako 0:14b64813c04d 216 fastWriteString("data saved.");
pnpako 0:14b64813c04d 217
pnpako 0:14b64813c04d 218 //END HERE
pnpako 0:14b64813c04d 219 while(1);
pnpako 0:14b64813c04d 220
pnpako 0:14b64813c04d 221
pnpako 0:14b64813c04d 222
pnpako 0:14b64813c04d 223 }
pnpako 0:14b64813c04d 224
pnpako 0:14b64813c04d 225
pnpako 0:14b64813c04d 226
pnpako 0:14b64813c04d 227
pnpako 0:14b64813c04d 228
pnpako 0:14b64813c04d 229
pnpako 0:14b64813c04d 230
pnpako 0:14b64813c04d 231
pnpako 0:14b64813c04d 232
pnpako 0:14b64813c04d 233
pnpako 0:14b64813c04d 234
pnpako 0:14b64813c04d 235
pnpako 0:14b64813c04d 236