Project bike
bamlor nuttymaisuay
Dependencies: mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "MPU9250.h" 00003 #include <iostream> 00004 #include <string> 00005 00006 00007 00008 string word; 00009 char alphabet; 00010 float x; 00011 float sum = 0; 00012 uint32_t sumCount = 0; 00013 char buffer[14]; 00014 int i = 0; 00015 00016 MPU9250 mpu9250; 00017 00018 Timer t; 00019 00020 Serial pc(PA_15, PB_7); // tx, rx 00021 float v, delt_t2 = 0, count2 = 0; 00022 int mode; 00023 int cmode; 00024 Serial bam(D1,D0); 00025 BusOut B(PA_0,PA_1,PA_4,PB_0,PC_1,PC_2,PC_3), A(PC_10,PC_12,PA_13,PA_14,PC_13,PC_11,PD_2), C(PA_5,PA_6,PA_7,PC_7,PB_2,PB_1,PB_15); 00026 00027 void Text (char text,float posA,float posB,float posC,int i ) 00028 { 00029 int n = i*60; 00030 if (text == ' '){ 00031 if (posA > 0+n && posA < 10+n) { A = 0x00; } 00032 else if(posA > 10+n && posA < 20+n ) { A = 0x00; } 00033 else if(posA > 20+n && posA < 30+n ) { A = 0x00; } 00034 else if(posA > 30+n && posA < 40+n ) { A = 0x00; } 00035 else if(posA > 40+n && posA < 50+n ) { A = 0x00; } 00036 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00037 00038 if (posB > 0+n && posB < 10+n) { B = 0x00; } 00039 else if(posB > 10+n && posB < 20+n ) { B = 0x00; } 00040 else if(posB > 20+n && posB < 30+n ) { B = 0x00; } 00041 else if(posB > 30+n && posB < 40+n ) { B = 0x00; } 00042 else if(posB > 40+n && posB < 50+n ) { B = 0x00; } 00043 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00044 00045 if (posC > 0+n && posC < 10+n) { C = 0x00; } 00046 else if(posC > 10+n && posC < 20+n ) { C = 0x00; } 00047 else if(posC > 20+n && posC < 30+n ) { C = 0x00; } 00048 else if(posC > 30+n && posC < 40+n ) { C = 0x00; } 00049 else if(posC > 40+n && posC < 50+n ) { C = 0x00; } 00050 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00051 } 00052 if (text == 'A'){ 00053 if (posA > 0+n && posA < 10+n) { A = 0x3F; } 00054 else if(posA > 10+n && posA < 20+n ) { A = 0x44; } 00055 else if(posA > 20+n && posA < 30+n ) { A = 0x44; } 00056 else if(posA > 30+n && posA < 40+n ) { A = 0x44; } 00057 else if(posA > 40+n && posA < 50+n ) { A = 0x3F; } 00058 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00059 00060 if (posB > 0+n && posB < 10+n) { B = 0x3F; } 00061 else if(posB > 10+n && posB < 20+n ) { B = 0x44; } 00062 else if(posB > 20+n && posB < 30+n ) { B = 0x44; } 00063 else if(posB > 30+n && posB < 40+n ) { B = 0x44; } 00064 else if(posB > 40+n && posB < 50+n ) { B = 0x3F; } 00065 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00066 00067 if (posC > 0+n && posC < 10+n) { C = 0x3F; } 00068 else if(posC > 10+n && posC < 20+n ) { C = 0x44; } 00069 else if(posC > 20+n && posC < 30+n ) { C = 0x44; } 00070 else if(posC > 30+n && posC < 40+n ) { C = 0x44; } 00071 else if(posC > 40+n && posC < 50+n ) { C = 0x3F; } 00072 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00073 } 00074 if (text == 'B'){ 00075 if (posA > 0+n && posA < 10+n) { A = 0x36; } 00076 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00077 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00078 else if(posA > 30+n && posA < 40+n ) { A = 0x49; } 00079 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00080 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00081 00082 if (posB > 0+n && posB < 10+n) { B = 0x36; } 00083 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00084 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00085 else if(posB > 30+n && posB < 40+n ) { B = 0x49; } 00086 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00087 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00088 00089 if (posC > 0+n && posC < 10+n) { C = 0x36; } 00090 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00091 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00092 else if(posC > 30+n && posC < 40+n ) { C = 0x49; } 00093 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00094 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00095 } 00096 if (text == 'C'){ 00097 if (posA > 0+n && posA < 10+n) { A = 0x22; } 00098 else if(posA > 10+n && posA < 20+n ) { A = 0x41; } 00099 else if(posA > 20+n && posA < 30+n ) { A = 0x41; } 00100 else if(posA > 30+n && posA < 40+n ) { A = 0x41; } 00101 else if(posA > 40+n && posA < 50+n ) { A = 0x3E; } 00102 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00103 00104 if (posB > 0+n && posB < 10+n) { B = 0x22; } 00105 else if(posB > 10+n && posB < 20+n ) { B = 0x41; } 00106 else if(posB > 20+n && posB < 30+n ) { B = 0x41; } 00107 else if(posB > 30+n && posB < 40+n ) { B = 0x41; } 00108 else if(posB > 40+n && posB < 50+n ) { B = 0x3E; } 00109 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00110 00111 if (posC > 0+n && posC < 10+n) { C = 0x22; } 00112 else if(posC > 10+n && posC < 20+n ) { C = 0x41; } 00113 else if(posC > 20+n && posC < 30+n ) { C = 0x41; } 00114 else if(posC > 30+n && posC < 40+n ) { C = 0x41; } 00115 else if(posC > 40+n && posC < 50+n ) { C = 0x3E; } 00116 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00117 } 00118 if (text == 'D'){ 00119 if (posA > 0+n && posA < 10+n) { A = 0x3E; } 00120 else if(posA > 10+n && posA < 20+n ) { A = 0x41; } 00121 else if(posA > 20+n && posA < 30+n ) { A = 0x41; } 00122 else if(posA > 30+n && posA < 40+n ) { A = 0x41; } 00123 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00124 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00125 00126 if (posB > 0+n && posB < 10+n) { B = 0x3E; } 00127 else if(posB > 10+n && posB < 20+n ) { B = 0x41; } 00128 else if(posB > 20+n && posB < 30+n ) { B = 0x41; } 00129 else if(posB > 30+n && posB < 40+n ) { B = 0x41; } 00130 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00131 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00132 00133 if (posC > 0+n && posC < 10+n) { C = 0x3E; } 00134 else if(posC > 10+n && posC < 20+n ) { C = 0x41; } 00135 else if(posC > 20+n && posC < 30+n ) { C = 0x41; } 00136 else if(posC > 30+n && posC < 40+n ) { C = 0x41; } 00137 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00138 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00139 } 00140 if (text == 'E'){ 00141 if (posA > 0+n && posA < 10+n) { A = 0x49; } 00142 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00143 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00144 else if(posA > 30+n && posA < 40+n ) { A = 0x49; } 00145 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00146 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00147 00148 if (posB > 0+n && posB < 10+n) { B = 0x49; } 00149 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00150 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00151 else if(posB > 30+n && posB < 40+n ) { B = 0x49; } 00152 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00153 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00154 00155 if (posC > 0+n && posC < 10+n) { C = 0x49; } 00156 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00157 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00158 else if(posC > 30+n && posC < 40+n ) { C = 0x49; } 00159 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00160 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00161 } 00162 if (text == 'F'){ 00163 if (posA > 0+n && posA < 10+n) { A = 0x40; } 00164 else if(posA > 10+n && posA < 20+n ) { A = 0x40; } 00165 else if(posA > 20+n && posA < 30+n ) { A = 0x48; } 00166 else if(posA > 30+n && posA < 40+n ) { A = 0x48; } 00167 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00168 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00169 00170 if (posB > 0+n && posB < 10+n) { B = 0x40; } 00171 else if(posB > 10+n && posB < 20+n ) { B = 0x40; } 00172 else if(posB > 20+n && posB < 30+n ) { B = 0x48; } 00173 else if(posB > 30+n && posB < 40+n ) { B = 0x48; } 00174 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00175 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00176 00177 if (posC > 0+n && posC < 10+n) { C = 0x40; } 00178 else if(posC > 10+n && posC < 20+n ) { C = 0x40; } 00179 else if(posC > 20+n && posC < 30+n ) { C = 0x48; } 00180 else if(posC > 30+n && posC < 40+n ) { C = 0x48; } 00181 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00182 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00183 } 00184 if (text == 'G'){ 00185 if (posA > 0+n && posA < 10+n) { A = 0x4F; } 00186 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00187 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00188 else if(posA > 30+n && posA < 40+n ) { A = 0x41; } 00189 else if(posA > 40+n && posA < 50+n ) { A = 0x3E; } 00190 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00191 00192 if (posB > 0+n && posB < 10+n) { B = 0x4F; } 00193 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00194 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00195 else if(posB > 30+n && posB < 40+n ) { B = 0x41; } 00196 else if(posB > 40+n && posB < 50+n ) { B = 0x3E; } 00197 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00198 00199 if (posC > 0+n && posC < 10+n) { C = 0x4F; } 00200 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00201 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00202 else if(posC > 30+n && posC < 40+n ) { C = 0x41; } 00203 else if(posC > 40+n && posC < 50+n ) { C = 0x3E; } 00204 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00205 } 00206 if (text == 'H'){ 00207 if (posA > 0+n && posA < 10+n) { A = 0x7F; } 00208 else if(posA > 10+n && posA < 20+n ) { A = 0x08; } 00209 else if(posA > 20+n && posA < 30+n ) { A = 0x08; } 00210 else if(posA > 30+n && posA < 40+n ) { A = 0x08; } 00211 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00212 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00213 00214 if (posB > 0+n && posB < 10+n) { B = 0x7F; } 00215 else if(posB > 10+n && posB < 20+n ) { B = 0x08; } 00216 else if(posB > 20+n && posB < 30+n ) { B = 0x08; } 00217 else if(posB > 30+n && posB < 40+n ) { B = 0x08; } 00218 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00219 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00220 00221 if (posC > 0+n && posC < 10+n) { C = 0x7F; } 00222 else if(posC > 10+n && posC < 20+n ) { C = 0x08; } 00223 else if(posC > 20+n && posC < 30+n ) { C = 0x08; } 00224 else if(posC > 30+n && posC < 40+n ) { C = 0x08; } 00225 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00226 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00227 } 00228 if (text == 'I'){ 00229 if (posA > 0+n && posA < 10+n) { A = 0x41; } 00230 else if(posA > 10+n && posA < 20+n ) { A = 0x41; } 00231 else if(posA > 20+n && posA < 30+n ) { A = 0x7F; } 00232 else if(posA > 30+n && posA < 40+n ) { A = 0x41; } 00233 else if(posA > 40+n && posA < 50+n ) { A = 0x41; } 00234 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00235 00236 if (posB > 0+n && posB < 10+n) { B = 0x41; } 00237 else if(posB > 10+n && posB < 20+n ) { B = 0x41; } 00238 else if(posB > 20+n && posB < 30+n ) { B = 0x7F; } 00239 else if(posB > 30+n && posB < 40+n ) { B = 0x41; } 00240 else if(posB > 40+n && posB < 50+n ) { B = 0x41; } 00241 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00242 00243 if (posC > 0+n && posC < 10+n) { C = 0x41; } 00244 else if(posC > 10+n && posC < 20+n ) { C = 0x41; } 00245 else if(posC > 20+n && posC < 30+n ) { C = 0x7F; } 00246 else if(posC > 30+n && posC < 40+n ) { C = 0x41; } 00247 else if(posC > 40+n && posC < 50+n ) { C = 0x41; } 00248 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00249 } 00250 if (text == 'J'){ 00251 if (posA > 0+n && posA < 10+n) { A = 0x7E; } 00252 else if(posA > 10+n && posA < 20+n ) { A = 0x41; } 00253 else if(posA > 20+n && posA < 30+n ) { A = 0x41; } 00254 else if(posA > 30+n && posA < 40+n ) { A = 0x01; } 00255 else if(posA > 40+n && posA < 50+n ) { A = 0x02; } 00256 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00257 00258 if (posB > 0+n && posB < 10+n) { B = 0x7E; } 00259 else if(posB > 10+n && posB < 20+n ) { B = 0x41; } 00260 else if(posB > 20+n && posB < 30+n ) { B = 0x41; } 00261 else if(posB > 30+n && posB < 40+n ) { B = 0x01; } 00262 else if(posB > 40+n && posB < 50+n ) { B = 0x02; } 00263 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00264 00265 if (posC > 0+n && posC < 10+n) { C = 0x7E; } 00266 else if(posC > 10+n && posC < 20+n ) { C = 0x41; } 00267 else if(posC > 20+n && posC < 30+n ) { C = 0x41; } 00268 else if(posC > 30+n && posC < 40+n ) { C = 0x01; } 00269 else if(posC > 40+n && posC < 50+n ) { C = 0x02; } 00270 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00271 } 00272 if (text == 'K'){ 00273 if (posA > 0+n && posA < 10+n) { A = 0x41; } 00274 else if(posA > 10+n && posA < 20+n ) { A = 0x22; } 00275 else if(posA > 20+n && posA < 30+n ) { A = 0x14; } 00276 else if(posA > 30+n && posA < 40+n ) { A = 0x08; } 00277 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00278 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00279 00280 if (posB > 0+n && posB < 10+n) { B = 0x41; } 00281 else if(posB > 10+n && posB < 20+n ) { B = 0x22; } 00282 else if(posB > 20+n && posB < 30+n ) { B = 0x14; } 00283 else if(posB > 30+n && posB < 40+n ) { B = 0x08; } 00284 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00285 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00286 00287 if (posC > 0+n && posC < 10+n) { C = 0x41; } 00288 else if(posC > 10+n && posC < 20+n ) { C = 0x22; } 00289 else if(posC > 20+n && posC < 30+n ) { C = 0x14; } 00290 else if(posC > 30+n && posC < 40+n ) { C = 0x08; } 00291 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00292 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00293 } 00294 if (text == 'L'){ 00295 if (posA > 0+n && posA < 10+n) { A = 0x01; } 00296 else if(posA > 10+n && posA < 20+n ) { A = 0x01; } 00297 else if(posA > 20+n && posA < 30+n ) { A = 0x01; } 00298 else if(posA > 30+n && posA < 40+n ) { A = 0x01; } 00299 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00300 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00301 00302 if (posB > 0+n && posB < 10+n) { B = 0x01; } 00303 else if(posB > 10+n && posB < 20+n ) { B = 0x01; } 00304 else if(posB > 20+n && posB < 30+n ) { B = 0x01; } 00305 else if(posB > 30+n && posB < 40+n ) { B = 0x01; } 00306 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00307 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00308 00309 if (posC > 0+n && posC < 10+n) { C = 0x01; } 00310 else if(posC > 10+n && posC < 20+n ) { C = 0x01; } 00311 else if(posC > 20+n && posC < 30+n ) { C = 0x01; } 00312 else if(posC > 30+n && posC < 40+n ) { C = 0x01; } 00313 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00314 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00315 } 00316 if (text == 'M'){ 00317 if (posA > 0+n && posA < 10+n) { A = 0x7F; } 00318 else if(posA > 10+n && posA < 20+n ) { A = 0x20; } 00319 else if(posA > 20+n && posA < 30+n ) { A = 0x10; } 00320 else if(posA > 30+n && posA < 40+n ) { A = 0x20; } 00321 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00322 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00323 00324 if (posB > 0+n && posB < 10+n) { B = 0x7F; } 00325 else if(posB > 10+n && posB < 20+n ) { B = 0x20; } 00326 else if(posB > 20+n && posB < 30+n ) { B = 0x10; } 00327 else if(posB > 30+n && posB < 40+n ) { B = 0x20; } 00328 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00329 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00330 00331 if (posC > 0+n && posC < 10+n) { C = 0x7F; } 00332 else if(posC > 10+n && posC < 20+n ) { C = 0x20; } 00333 else if(posC > 20+n && posC < 30+n ) { C = 0x10; } 00334 else if(posC > 30+n && posC < 40+n ) { C = 0x20; } 00335 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00336 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00337 } 00338 if (text == 'N'){ 00339 if (posA > 0+n && posA < 10+n) { A = 0x7F; } 00340 else if(posA > 10+n && posA < 20+n ) { A = 0x08; } 00341 else if(posA > 20+n && posA < 30+n ) { A = 0x10; } 00342 else if(posA > 30+n && posA < 40+n ) { A = 0x20; } 00343 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00344 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00345 00346 if (posB > 0+n && posB < 10+n) { B = 0x7F; } 00347 else if(posB > 10+n && posB < 20+n ) { B = 0x08; } 00348 else if(posB > 20+n && posB < 30+n ) { B = 0x10; } 00349 else if(posB > 30+n && posB < 40+n ) { B = 0x20; } 00350 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00351 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00352 00353 if (posC > 0+n && posC < 10+n) { C = 0x7F; } 00354 else if(posC > 10+n && posC < 20+n ) { C = 0x08; } 00355 else if(posC > 20+n && posC < 30+n ) { C = 0x10; } 00356 else if(posC > 30+n && posC < 40+n ) { C = 0x20; } 00357 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00358 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00359 } 00360 if (text == 'O'){ 00361 if (posA > 0+n && posA < 10+n) { A = 0x7F; } 00362 else if(posA > 10+n && posA < 20+n ) { A = 0x41; } 00363 else if(posA > 20+n && posA < 30+n ) { A = 0x41; } 00364 else if(posA > 30+n && posA < 40+n ) { A = 0x41; } 00365 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00366 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00367 00368 if (posB > 0+n && posB < 10+n) { B = 0x7F; } 00369 else if(posB > 10+n && posB < 20+n ) { B = 0x41; } 00370 else if(posB > 20+n && posB < 30+n ) { B = 0x41; } 00371 else if(posB > 30+n && posB < 40+n ) { B = 0x41; } 00372 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00373 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00374 00375 if (posC > 0+n && posC < 10+n) { C = 0x7F; } 00376 else if(posC > 10+n && posC < 20+n ) { C = 0x41; } 00377 else if(posC > 20+n && posC < 30+n ) { C = 0x41; } 00378 else if(posC > 30+n && posC < 40+n ) { C = 0x41; } 00379 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00380 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00381 } 00382 if (text == 'P'){ 00383 if (posA > 0+n && posA < 10+n) { A = 0x30; } 00384 else if(posA > 10+n && posA < 20+n ) { A = 0x48; } 00385 else if(posA > 20+n && posA < 30+n ) { A = 0x48; } 00386 else if(posA > 30+n && posA < 40+n ) { A = 0x48; } 00387 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00388 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00389 00390 if (posB > 0+n && posB < 10+n) { B = 0x30; } 00391 else if(posB > 10+n && posB < 20+n ) { B = 0x48; } 00392 else if(posB > 20+n && posB < 30+n ) { B = 0x48; } 00393 else if(posB > 30+n && posB < 40+n ) { B = 0x48; } 00394 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00395 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00396 00397 if (posC > 0+n && posC < 10+n) { C = 0x30; } 00398 else if(posC > 10+n && posC < 20+n ) { C = 0x48; } 00399 else if(posC > 20+n && posC < 30+n ) { C = 0x48; } 00400 else if(posC > 30+n && posC < 40+n ) { C = 0x48; } 00401 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00402 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00403 } 00404 if (text == 'Q'){ 00405 if (posA > 0+n && posA < 10+n) { A = 0x3D; } 00406 else if(posA > 10+n && posA < 20+n ) { A = 0x22; } 00407 else if(posA > 20+n && posA < 30+n ) { A = 0x22; } 00408 else if(posA > 30+n && posA < 40+n ) { A = 0x22; } 00409 else if(posA > 40+n && posA < 50+n ) { A = 0x3C; } 00410 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00411 00412 if (posB > 0+n && posB < 10+n) { B = 0x3D; } 00413 else if(posB > 10+n && posB < 20+n ) { B = 0x22; } 00414 else if(posB > 20+n && posB < 30+n ) { B = 0x22; } 00415 else if(posB > 30+n && posB < 40+n ) { B = 0x22; } 00416 else if(posB > 40+n && posB < 50+n ) { B = 0x3C; } 00417 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00418 00419 if (posC > 0+n && posC < 10+n) { C = 0x3D; } 00420 else if(posC > 10+n && posC < 20+n ) { C = 0x22; } 00421 else if(posC > 20+n && posC < 30+n ) { C = 0x22; } 00422 else if(posC > 30+n && posC < 40+n ) { C = 0x22; } 00423 else if(posC > 40+n && posC < 50+n ) { C = 0x3C; } 00424 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00425 } 00426 if (text == 'R'){ 00427 if (posA > 0+n && posA < 10+n) { A = 0x31; } 00428 else if(posA > 10+n && posA < 20+n ) { A = 0x4A; } 00429 else if(posA > 20+n && posA < 30+n ) { A = 0x4C; } 00430 else if(posA > 30+n && posA < 40+n ) { A = 0x48; } 00431 else if(posA > 40+n && posA < 50+n ) { A = 0x7F; } 00432 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00433 00434 if (posB > 0+n && posB < 10+n) { B = 0x31; } 00435 else if(posB > 10+n && posB < 20+n ) { B = 0x4A; } 00436 else if(posB > 20+n && posB < 30+n ) { B = 0x4C; } 00437 else if(posB > 30+n && posB < 40+n ) { B = 0x48; } 00438 else if(posB > 40+n && posB < 50+n ) { B = 0x7F; } 00439 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00440 00441 if (posC > 0+n && posC < 10+n) { C = 0x31; } 00442 else if(posC > 10+n && posC < 20+n ) { C = 0x4A; } 00443 else if(posC > 20+n && posC < 30+n ) { C = 0x4C; } 00444 else if(posC > 30+n && posC < 40+n ) { C = 0x48; } 00445 else if(posC > 40+n && posC < 50+n ) { C = 0x7F; } 00446 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00447 } 00448 if (text == 'S'){ 00449 if (posA > 0+n && posA < 10+n) { A = 0x46; } 00450 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00451 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00452 else if(posA > 30+n && posA < 40+n ) { A = 0x49; } 00453 else if(posA > 40+n && posA < 50+n ) { A = 0x31; } 00454 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00455 00456 if (posB > 0+n && posB < 10+n) { B = 0x46; } 00457 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00458 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00459 else if(posB > 30+n && posB < 40+n ) { B = 0x49; } 00460 else if(posB > 40+n && posB < 50+n ) { B = 0x31; } 00461 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00462 00463 if (posC > 0+n && posC < 10+n) { C = 0x46; } 00464 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00465 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00466 else if(posC > 30+n && posC < 40+n ) { C = 0x49; } 00467 else if(posC > 40+n && posC < 50+n ) { C = 0x31; } 00468 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00469 } 00470 if (text == 'T'){ 00471 if (posA > 0+n && posA < 10+n) { A = 0x40; } 00472 else if(posA > 10+n && posA < 20+n ) { A = 0x40; } 00473 else if(posA > 20+n && posA < 30+n ) { A = 0x7F; } 00474 else if(posA > 30+n && posA < 40+n ) { A = 0x40; } 00475 else if(posA > 40+n && posA < 50+n ) { A = 0x40; } 00476 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00477 00478 if (posB > 0+n && posB < 10+n) { B = 0x40; } 00479 else if(posB > 10+n && posB < 20+n ) { B = 0x40; } 00480 else if(posB > 20+n && posB < 30+n ) { B = 0x7F; } 00481 else if(posB > 30+n && posB < 40+n ) { B = 0x40; } 00482 else if(posB > 40+n && posB < 50+n ) { B = 0x40; } 00483 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00484 00485 if (posC > 0+n && posC < 10+n) { C = 0x40; } 00486 else if(posC > 10+n && posC < 20+n ) { C = 0x40; } 00487 else if(posC > 20+n && posC < 30+n ) { C = 0x7F; } 00488 else if(posC > 30+n && posC < 40+n ) { C = 0x40; } 00489 else if(posC > 40+n && posC < 50+n ) { C = 0x40; } 00490 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00491 } 00492 if (text == 'U'){ 00493 if (posA > 0+n && posA < 10+n) { A = 0x7E; } 00494 else if(posA > 10+n && posA < 20+n ) { A = 0x01; } 00495 else if(posA > 20+n && posA < 30+n ) { A = 0x01; } 00496 else if(posA > 30+n && posA < 40+n ) { A = 0x01; } 00497 else if(posA > 40+n && posA < 50+n ) { A = 0x7E; } 00498 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00499 00500 if (posB > 0+n && posB < 10+n) { B = 0x7E; } 00501 else if(posB > 10+n && posB < 20+n ) { B = 0x01; } 00502 else if(posB > 20+n && posB < 30+n ) { B = 0x01; } 00503 else if(posB > 30+n && posB < 40+n ) { B = 0x01; } 00504 else if(posB > 40+n && posB < 50+n ) { B = 0x7E; } 00505 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00506 00507 if (posC > 0+n && posC < 10+n) { C = 0x7E; } 00508 else if(posC > 10+n && posC < 20+n ) { C = 0x01; } 00509 else if(posC > 20+n && posC < 30+n ) { C = 0x01; } 00510 else if(posC > 30+n && posC < 40+n ) { C = 0x01; } 00511 else if(posC > 40+n && posC < 50+n ) { C = 0x7E; } 00512 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00513 } 00514 if (text == 'V'){ 00515 if (posA > 0+n && posA < 10+n) { A = 0x7C; } 00516 else if(posA > 10+n && posA < 20+n ) { A = 0x02; } 00517 else if(posA > 20+n && posA < 30+n ) { A = 0x01; } 00518 else if(posA > 30+n && posA < 40+n ) { A = 0x02; } 00519 else if(posA > 40+n && posA < 50+n ) { A = 0x7C; } 00520 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00521 00522 if (posB > 0+n && posB < 10+n) { B = 0x7C; } 00523 else if(posB > 10+n && posB < 20+n ) { B = 0x02; } 00524 else if(posB > 20+n && posB < 30+n ) { B = 0x01; } 00525 else if(posB > 30+n && posB < 40+n ) { B = 0x02; } 00526 else if(posB > 40+n && posB < 50+n ) { B = 0x7C; } 00527 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00528 00529 if (posC > 0+n && posC < 10+n) { C = 0x7C; } 00530 else if(posC > 10+n && posC < 20+n ) { C = 0x02; } 00531 else if(posC > 20+n && posC < 30+n ) { C = 0x01; } 00532 else if(posC > 30+n && posC < 40+n ) { C = 0x02; } 00533 else if(posC > 40+n && posC < 50+n ) { C = 0x7C; } 00534 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00535 } 00536 if (text == 'W'){ 00537 if (posA > 0+n && posA < 10+n) { A = 0x7E; } 00538 else if(posA > 10+n && posA < 20+n ) { A = 0x01; } 00539 else if(posA > 20+n && posA < 30+n ) { A = 0x0E; } 00540 else if(posA > 30+n && posA < 40+n ) { A = 0x01; } 00541 else if(posA > 40+n && posA < 50+n ) { A = 0x7E; } 00542 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00543 00544 if (posB > 0+n && posB < 10+n) { B = 0x7E; } 00545 else if(posB > 10+n && posB < 20+n ) { B = 0x01; } 00546 else if(posB > 20+n && posB < 30+n ) { B = 0x0E; } 00547 else if(posB > 30+n && posB < 40+n ) { B = 0x01; } 00548 else if(posB > 40+n && posB < 50+n ) { B = 0x7E; } 00549 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00550 00551 if (posC > 0+n && posC < 10+n) { C = 0x7E; } 00552 else if(posC > 10+n && posC < 20+n ) { C = 0x01; } 00553 else if(posC > 20+n && posC < 30+n ) { C = 0x0E; } 00554 else if(posC > 30+n && posC < 40+n ) { C = 0x01; } 00555 else if(posC > 40+n && posC < 50+n ) { C = 0x7E; } 00556 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00557 } 00558 if (text == 'X'){ 00559 if (posA > 0+n && posA < 10+n) { A = 0x77; } 00560 else if(posA > 10+n && posA < 20+n ) { A = 0x08; } 00561 else if(posA > 20+n && posA < 30+n ) { A = 0x08; } 00562 else if(posA > 30+n && posA < 40+n ) { A = 0x08; } 00563 else if(posA > 40+n && posA < 50+n ) { A = 0x77; } 00564 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00565 00566 if (posB > 0+n && posB < 10+n) { B = 0x77; } 00567 else if(posB > 10+n && posB < 20+n ) { B = 0x08; } 00568 else if(posB > 20+n && posB < 30+n ) { B = 0x08; } 00569 else if(posB > 30+n && posB < 40+n ) { B = 0x08; } 00570 else if(posB > 40+n && posB < 50+n ) { B = 0x77; } 00571 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00572 00573 if (posC > 0+n && posC < 10+n) { C = 0x77; } 00574 else if(posC > 10+n && posC < 20+n ) { C = 0x08; } 00575 else if(posC > 20+n && posC < 30+n ) { C = 0x08; } 00576 else if(posC > 30+n && posC < 40+n ) { C = 0x08; } 00577 else if(posC > 40+n && posC < 50+n ) { C = 0x77; } 00578 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00579 } 00580 if (text == 'Y'){ 00581 if (posA > 0+n && posA < 10+n) { A = 0x70; } 00582 else if(posA > 10+n && posA < 20+n ) { A = 0x08; } 00583 else if(posA > 20+n && posA < 30+n ) { A = 0x07; } 00584 else if(posA > 30+n && posA < 40+n ) { A = 0x08; } 00585 else if(posA > 40+n && posA < 50+n ) { A = 0x70; } 00586 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00587 00588 if (posB > 0+n && posB < 10+n) { B = 0x70; } 00589 else if(posB > 10+n && posB < 20+n ) { B = 0x08; } 00590 else if(posB > 20+n && posB < 30+n ) { B = 0x07; } 00591 else if(posB > 30+n && posB < 40+n ) { B = 0x08; } 00592 else if(posB > 40+n && posB < 50+n ) { B = 0x70; } 00593 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00594 00595 if (posC > 0+n && posC < 10+n) { C = 0x70; } 00596 else if(posC > 10+n && posC < 20+n ) { C = 0x08; } 00597 else if(posC > 20+n && posC < 30+n ) { C = 0x07; } 00598 else if(posC > 30+n && posC < 40+n ) { C = 0x08; } 00599 else if(posC > 40+n && posC < 50+n ) { C = 0x70; } 00600 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00601 } 00602 if (text == 'Z'){ 00603 if (posA > 0+n && posA < 10+n) { A = 0x61; } 00604 else if(posA > 10+n && posA < 20+n ) { A = 0x51; } 00605 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00606 else if(posA > 30+n && posA < 40+n ) { A = 0x45; } 00607 else if(posA > 40+n && posA < 50+n ) { A = 0x43; } 00608 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00609 00610 if (posB > 0+n && posB < 10+n) { B = 0x61; } 00611 else if(posB > 10+n && posB < 20+n ) { B = 0x51; } 00612 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00613 else if(posB > 30+n && posB < 40+n ) { B = 0x45; } 00614 else if(posB > 40+n && posB < 50+n ) { B = 0x43; } 00615 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00616 00617 if (posC > 0+n && posC < 10+n) { C = 0x61; } 00618 else if(posC > 10+n && posC < 20+n ) { C = 0x51; } 00619 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00620 else if(posC > 30+n && posC < 40+n ) { C = 0x45; } 00621 else if(posC > 40+n && posC < 50+n ) { C = 0x43; } 00622 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00623 } 00624 if (text == '0'){ 00625 if (posA > 0+n && posA < 10+n) { A = 0x3E; } 00626 else if(posA > 10+n && posA < 20+n ) { A = 0x41; } 00627 else if(posA > 20+n && posA < 30+n ) { A = 0x41; } 00628 else if(posA > 30+n && posA < 40+n ) { A = 0x41; } 00629 else if(posA > 40+n && posA < 50+n ) { A = 0x3E; } 00630 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00631 00632 if (posB > 0+n && posB < 10+n) { B = 0x3E; } 00633 else if(posB > 10+n && posB < 20+n ) { B = 0x41; } 00634 else if(posB > 20+n && posB < 30+n ) { B = 0x41; } 00635 else if(posB > 30+n && posB < 40+n ) { B = 0x41; } 00636 else if(posB > 40+n && posB < 50+n ) { B = 0x3E; } 00637 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00638 00639 if (posC > 0+n && posC < 10+n) { C = 0x3E; } 00640 else if(posC > 10+n && posC < 20+n ) { C = 0x41; } 00641 else if(posC > 20+n && posC < 30+n ) { C = 0x41; } 00642 else if(posC > 30+n && posC < 40+n ) { C = 0x41; } 00643 else if(posC > 40+n && posC < 50+n ) { C = 0x3E; } 00644 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00645 } 00646 if (text == '1'){ 00647 if (posA > 0+n && posA < 10+n) { A = 0x01; } 00648 else if(posA > 10+n && posA < 20+n ) { A = 0x01; } 00649 else if(posA > 20+n && posA < 30+n ) { A = 0x7F; } 00650 else if(posA > 30+n && posA < 40+n ) { A = 0x21; } 00651 else if(posA > 40+n && posA < 50+n ) { A = 0x21; } 00652 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00653 00654 if (posB > 0+n && posB < 10+n) { B = 0x01; } 00655 else if(posB > 10+n && posB < 20+n ) { B = 0x01; } 00656 else if(posB > 20+n && posB < 30+n ) { B = 0x7F; } 00657 else if(posB > 30+n && posB < 40+n ) { B = 0x21; } 00658 else if(posB > 40+n && posB < 50+n ) { B = 0x21; } 00659 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00660 00661 if (posC > 0+n && posC < 10+n) { C = 0x01; } 00662 else if(posC > 10+n && posC < 20+n ) { C = 0x01; } 00663 else if(posC > 20+n && posC < 30+n ) { C = 0x7F; } 00664 else if(posC > 30+n && posC < 40+n ) { C = 0x21; } 00665 else if(posC > 40+n && posC < 50+n ) { C = 0x21; } 00666 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00667 } 00668 if (text == '2'){ 00669 if (posA > 0+n && posA < 10+n) { A = 0x21; } 00670 else if(posA > 10+n && posA < 20+n ) { A = 0x51; } 00671 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00672 else if(posA > 30+n && posA < 40+n ) { A = 0x45; } 00673 else if(posA > 40+n && posA < 50+n ) { A = 0x23; } 00674 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00675 00676 if (posB > 0+n && posB < 10+n) { B = 0x21; } 00677 else if(posB > 10+n && posB < 20+n ) { B = 0x51; } 00678 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00679 else if(posB > 30+n && posB < 40+n ) { B = 0x45; } 00680 else if(posB > 40+n && posB < 50+n ) { B = 0x23; } 00681 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00682 00683 if (posC > 0+n && posC < 10+n) { C = 0x21; } 00684 else if(posC > 10+n && posC < 20+n ) { C = 0x51; } 00685 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00686 else if(posC > 30+n && posC < 40+n ) { C = 0x48; } 00687 else if(posC > 40+n && posC < 50+n ) { C = 0x23; } 00688 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00689 } 00690 if (text == '3'){ 00691 if (posA > 0+n && posA < 10+n) { A = 0x36; } 00692 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00693 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00694 else if(posA > 30+n && posA < 40+n ) { A = 0x41; } 00695 else if(posA > 40+n && posA < 50+n ) { A = 0x22; } 00696 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00697 00698 if (posB > 0+n && posB < 10+n) { B = 0x36; } 00699 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00700 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00701 else if(posB > 30+n && posB < 40+n ) { B = 0x41; } 00702 else if(posB > 40+n && posB < 50+n ) { B = 0x22; } 00703 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00704 00705 if (posC > 0+n && posC < 10+n) { C = 0x36; } 00706 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00707 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00708 else if(posC > 30+n && posC < 40+n ) { C = 0x41; } 00709 else if(posC > 40+n && posC < 50+n ) { C = 0x22; } 00710 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00711 } 00712 if (text == '4'){ 00713 if (posA > 0+n && posA < 10+n) { A = 0x04; } 00714 else if(posA > 10+n && posA < 20+n ) { A = 0x1F; } 00715 else if(posA > 20+n && posA < 30+n ) { A = 0x04; } 00716 else if(posA > 30+n && posA < 40+n ) { A = 0x04; } 00717 else if(posA > 40+n && posA < 50+n ) { A = 0x7C; } 00718 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00719 00720 if (posB > 0+n && posB < 10+n) { B = 0x04; } 00721 else if(posB > 10+n && posB < 20+n ) { B = 0x1F; } 00722 else if(posB > 20+n && posB < 30+n ) { B = 0x04; } 00723 else if(posB > 30+n && posB < 40+n ) { B = 0x04; } 00724 else if(posB > 40+n && posB < 50+n ) { B = 0x7C; } 00725 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00726 00727 if (posC > 0+n && posC < 10+n) { C = 0x04; } 00728 else if(posC > 10+n && posC < 20+n ) { C = 0x1F; } 00729 else if(posC > 20+n && posC < 30+n ) { C = 0x04; } 00730 else if(posC > 30+n && posC < 40+n ) { C = 0x04; } 00731 else if(posC > 40+n && posC < 50+n ) { C = 0x7C; } 00732 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00733 } 00734 if (text == '5'){ 00735 if (posA > 0+n && posA < 10+n) { A = 0x46; } 00736 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00737 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00738 else if(posA > 30+n && posA < 40+n ) { A = 0x49; } 00739 else if(posA > 40+n && posA < 50+n ) { A = 0x79; } 00740 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00741 00742 if (posB > 0+n && posB < 10+n) { B = 0x46; } 00743 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00744 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00745 else if(posB > 30+n && posB < 40+n ) { B = 0x49; } 00746 else if(posB > 40+n && posB < 50+n ) { B = 0x79; } 00747 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00748 00749 if (posC > 0+n && posC < 10+n) { C = 0x46; } 00750 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00751 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00752 else if(posC > 30+n && posC < 40+n ) { C = 0x49; } 00753 else if(posC > 40+n && posC < 50+n ) { C = 0x79; } 00754 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00755 } 00756 if (text == '6'){ 00757 if (posA > 0+n && posA < 10+n) { A = 0x26; } 00758 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00759 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00760 else if(posA > 30+n && posA < 40+n ) { A = 0x49; } 00761 else if(posA > 40+n && posA < 50+n ) { A = 0x3E; } 00762 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00763 00764 if (posB > 0+n && posB < 10+n) { B = 0x26; } 00765 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00766 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00767 else if(posB > 30+n && posB < 40+n ) { B = 0x49; } 00768 else if(posB > 40+n && posB < 50+n ) { B = 0x3E; } 00769 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00770 00771 if (posC > 0+n && posC < 10+n) { C = 0x26; } 00772 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00773 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00774 else if(posC > 30+n && posC < 40+n ) { C = 0x49; } 00775 else if(posC > 40+n && posC < 50+n ) { C = 0x3E; } 00776 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00777 } 00778 if (text == '7'){ 00779 if (posA > 0+n && posA < 10+n) { A = 0x60; } 00780 else if(posA > 10+n && posA < 20+n ) { A = 0x50; } 00781 else if(posA > 20+n && posA < 30+n ) { A = 0x47; } 00782 else if(posA > 30+n && posA < 40+n ) { A = 0x40; } 00783 else if(posA > 40+n && posA < 50+n ) { A = 0x40; } 00784 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00785 00786 if (posB > 0+n && posB < 10+n) { B = 0x60; } 00787 else if(posB > 10+n && posB < 20+n ) { B = 0x50; } 00788 else if(posB > 20+n && posB < 30+n ) { B = 0x47; } 00789 else if(posB > 30+n && posB < 40+n ) { B = 0x40; } 00790 else if(posB > 40+n && posB < 50+n ) { B = 0x40; } 00791 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00792 00793 if (posC > 0+n && posC < 10+n) { C = 0x60; } 00794 else if(posC > 10+n && posC < 20+n ) { C = 0x50; } 00795 else if(posC > 20+n && posC < 30+n ) { C = 0x47; } 00796 else if(posC > 30+n && posC < 40+n ) { C = 0x40; } 00797 else if(posC > 40+n && posC < 50+n ) { C = 0x40; } 00798 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00799 } 00800 if (text == '8'){ 00801 if (posA > 0+n && posA < 10+n) { A = 0x36; } 00802 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00803 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00804 else if(posA > 30+n && posA < 40+n ) { A = 0x49; } 00805 else if(posA > 40+n && posA < 50+n ) { A = 0x36; } 00806 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00807 00808 if (posB > 0+n && posB < 10+n) { B = 0x36; } 00809 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00810 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00811 else if(posB > 30+n && posB < 40+n ) { B = 0x49; } 00812 else if(posB > 40+n && posB < 50+n ) { B = 0x36; } 00813 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00814 00815 if (posC > 0+n && posC < 10+n) { C = 0x36; } 00816 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00817 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00818 else if(posC > 30+n && posC < 40+n ) { C = 0x49; } 00819 else if(posC > 40+n && posC < 50+n ) { C = 0x36; } 00820 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00821 } 00822 if (text == '9'){ 00823 if (posA > 0+n && posA < 10+n) { A = 0x3E; } 00824 else if(posA > 10+n && posA < 20+n ) { A = 0x49; } 00825 else if(posA > 20+n && posA < 30+n ) { A = 0x49; } 00826 else if(posA > 30+n && posA < 40+n ) { A = 0x49; } 00827 else if(posA > 40+n && posA < 50+n ) { A = 0x32; } 00828 else if(posA > 50+n && posA < 60+n ) { A = 0x00; } 00829 00830 if (posB > 0+n && posB < 10+n) { B = 0x3E; } 00831 else if(posB > 10+n && posB < 20+n ) { B = 0x49; } 00832 else if(posB > 20+n && posB < 30+n ) { B = 0x49; } 00833 else if(posB > 30+n && posB < 40+n ) { B = 0x49; } 00834 else if(posB > 40+n && posB < 50+n ) { B = 0x32; } 00835 else if(posB > 50+n && posB < 60+n ) { B = 0x00; } 00836 00837 if (posC > 0+n && posC < 10+n) { C = 0x3E; } 00838 else if(posC > 10+n && posC < 20+n ) { C = 0x49; } 00839 else if(posC > 20+n && posC < 30+n ) { C = 0x49; } 00840 else if(posC > 30+n && posC < 40+n ) { C = 0x49; } 00841 else if(posC > 40+n && posC < 50+n ) { C = 0x32; } 00842 else if(posC > 50+n && posC < 60+n ) { C = 0x00; } 00843 } 00844 } 00845 void Number(char text,float posA,float posB,float posC,int i) 00846 { 00847 int n = i*50; 00848 if (i ==1 || i ==2) 00849 {n=(i*50)+15;} 00850 if (text == '0'){ 00851 if (posA > 0+n && posA < 9+n) { A = 0x7F; } 00852 else if(posA > 9+n && posA < 18+n ) { A = 0x41; } 00853 else if(posA > 18+n && posA < 27+n ) { A = 0x41; } 00854 else if(posA > 27+n && posA < 36+n ) { A = 0x41; } 00855 else if(posA > 36+n && posA < 45+n ) { A = 0x7F; } 00856 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 00857 00858 if (posB > 0+n && posB < 9+n) { B = 0x7F; } 00859 else if(posB > 9+n && posB < 18+n ) { B = 0x41; } 00860 else if(posB > 18+n && posB < 27+n ) { B = 0x41; } 00861 else if(posB > 27+n && posB < 36+n ) { B = 0x41; } 00862 else if(posB > 36+n && posB < 45+n ) { B = 0x7F; } 00863 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 00864 00865 if (posC > 0+n && posC < 9+n) { C = 0x7F; } 00866 else if(posC > 9+n && posC < 18+n ) { C = 0x41; } 00867 else if(posC > 18+n && posC < 27+n ) { C = 0x41; } 00868 else if(posC > 27+n && posC < 36+n ) { C = 0x41; } 00869 else if(posC > 36+n && posC < 45+n ) { C = 0x7F; } 00870 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 00871 } 00872 if (text == '1'){ 00873 if (posA > 0+n && posA < 9+n) { A = 0x01; } 00874 else if(posA > 9+n && posA < 18+n ) { A = 0x01; } 00875 else if(posA > 18+n && posA < 27+n ) { A = 0x7F; } 00876 else if(posA > 27+n && posA < 36+n ) { A = 0x21; } 00877 else if(posA > 36+n && posA < 45+n ) { A = 0x21; } 00878 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 00879 00880 if (posB > 0+n && posB < 9+n) { B = 0x01; } 00881 else if(posB > 9+n && posB < 18+n ) { B = 0x01; } 00882 else if(posB > 18+n && posB < 27+n ) { B = 0x7F; } 00883 else if(posB > 27+n && posB < 36+n ) { B = 0x21; } 00884 else if(posB > 36+n && posB < 45+n ) { B = 0x21; } 00885 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 00886 00887 if (posC > 0+n && posC < 9+n) { C = 0x01; } 00888 else if(posC > 9+n && posC < 18+n ) { C = 0x01; } 00889 else if(posC > 18+n && posC < 27+n ) { C = 0x7F; } 00890 else if(posC > 27+n && posC < 36+n ) { C = 0x21; } 00891 else if(posC > 36+n && posC < 45+n ) { C = 0x21; } 00892 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 00893 } 00894 if (text == '2'){ 00895 if (posA > 0+n && posA < 9+n) { A = 0x21; } 00896 else if(posA > 9+n && posA < 18+n ) { A = 0x51; } 00897 else if(posA > 18+n && posA < 27+n ) { A = 0x49; } 00898 else if(posA > 27+n && posA < 36+n ) { A = 0x45; } 00899 else if(posA > 36+n && posA < 45+n ) { A = 0x23; } 00900 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 00901 00902 if (posB > 0+n && posB < 9+n) { B = 0x21; } 00903 else if(posB > 9+n && posB < 18+n ) { B = 0x51; } 00904 else if(posB > 18+n && posB < 27+n ) { B = 0x49; } 00905 else if(posB > 27+n && posB < 36+n ) { B = 0x45; } 00906 else if(posB > 36+n && posB < 45+n ) { B = 0x23; } 00907 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 00908 00909 if (posC > 0+n && posC < 9+n) { C = 0x21; } 00910 else if(posC > 9+n && posC < 18+n ) { C = 0x51; } 00911 else if(posC > 18+n && posC < 27+n ) { C = 0x49; } 00912 else if(posC > 27+n && posC < 36+n ) { C = 0x45; } 00913 else if(posC > 36+n && posC < 45+n ) { C = 0x23; } 00914 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 00915 } 00916 if (text == '3'){ 00917 if (posA > 0+n && posA < 9+n) { A = 0x36; } 00918 else if(posA > 9+n && posA < 18+n ) { A = 0x49; } 00919 else if(posA > 18+n && posA < 27+n ) { A = 0x49; } 00920 else if(posA > 27+n && posA < 36+n ) { A = 0x41; } 00921 else if(posA > 36+n && posA < 45+n ) { A = 0x22; } 00922 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 00923 00924 if (posB > 0+n && posB < 9+n) { B = 0x36; } 00925 else if(posB > 9+n && posB < 18+n ) { B = 0x49; } 00926 else if(posB > 18+n && posB < 27+n ) { B = 0x49; } 00927 else if(posB > 27+n && posB < 36+n ) { B = 0x41; } 00928 else if(posB > 36+n && posB < 45+n ) { B = 0x22; } 00929 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 00930 00931 if (posC > 0+n && posC < 9+n) { C = 0x36; } 00932 else if(posC > 9+n && posC < 18+n ) { C = 0x49; } 00933 else if(posC > 18+n && posC < 27+n ) { C = 0x49; } 00934 else if(posC > 27+n && posC < 36+n ) { C = 0x41; } 00935 else if(posC > 36+n && posC < 45+n ) { C = 0x22; } 00936 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 00937 } 00938 if (text == '4'){ 00939 if (posA > 0+n && posA < 9+n) { A = 0x04; } 00940 else if(posA > 9+n && posA < 18+n ) { A = 0x1F; } 00941 else if(posA > 18+n && posA < 27+n ) { A = 0x04; } 00942 else if(posA > 27+n && posA < 36+n ) { A = 0x04; } 00943 else if(posA > 36+n && posA < 45+n ) { A = 0x7C; } 00944 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 00945 00946 if (posB > 0+n && posB < 9+n) { B = 0x04; } 00947 else if(posB > 9+n && posB < 18+n ) { B = 0x1F; } 00948 else if(posB > 18+n && posB < 27+n ) { B = 0x04; } 00949 else if(posB > 27+n && posB < 36+n ) { B = 0x04; } 00950 else if(posB > 36+n && posB < 45+n ) { B = 0x7C; } 00951 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 00952 00953 if (posC > 0+n && posC < 9+n) { C = 0x04; } 00954 else if(posC > 9+n && posC < 18+n ) { C = 0x1F; } 00955 else if(posC > 18+n && posC < 27+n ) { C = 0x04; } 00956 else if(posC > 27+n && posC < 36+n ) { C = 0x04; } 00957 else if(posC > 36+n && posC < 45+n ) { C = 0x7C; } 00958 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 00959 } 00960 if (text == '5'){ 00961 if (posA > 0+n && posA < 9+n) { A = 0x46; } 00962 else if(posA > 9+n && posA < 18+n ) { A = 0x49; } 00963 else if(posA > 18+n && posA < 27+n ) { A = 0x49; } 00964 else if(posA > 27+n && posA < 36+n ) { A = 0x49; } 00965 else if(posA > 36+n && posA < 45+n ) { A = 0x79; } 00966 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 00967 00968 if (posB > 0+n && posB < 9+n) { B = 0x46; } 00969 else if(posB > 9+n && posB < 18+n ) { B = 0x49; } 00970 else if(posB > 18+n && posB < 27+n ) { B = 0x49; } 00971 else if(posB > 27+n && posB < 36+n ) { B = 0x49; } 00972 else if(posB > 36+n && posB < 45+n ) { B = 0x79; } 00973 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 00974 00975 if (posC > 0+n && posC < 9+n) { C = 0x46; } 00976 else if(posC > 9+n && posC < 18+n ) { C = 0x49; } 00977 else if(posC > 18+n && posC < 27+n ) { C = 0x49; } 00978 else if(posC > 27+n && posC < 36+n ) { C = 0x49; } 00979 else if(posC > 36+n && posC < 45+n ) { C = 0x79; } 00980 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 00981 } 00982 if (text == '6'){ 00983 if (posA > 0+n && posA < 9+n) { A = 0x26; } 00984 else if(posA > 9+n && posA < 18+n ) { A = 0x49; } 00985 else if(posA > 18+n && posA < 27+n ) { A = 0x49; } 00986 else if(posA > 27+n && posA < 36+n ) { A = 0x49; } 00987 else if(posA > 36+n && posA < 45+n ) { A = 0x3E; } 00988 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 00989 00990 if (posB > 0+n && posB < 9+n) { B = 0x26; } 00991 else if(posB > 9+n && posB < 18+n ) { B = 0x49; } 00992 else if(posB > 18+n && posB < 27+n ) { B = 0x49; } 00993 else if(posB > 27+n && posB < 36+n ) { B = 0x49; } 00994 else if(posB > 36+n && posB < 45+n ) { B = 0x3E; } 00995 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 00996 00997 if (posC > 0+n && posC < 9+n) { C = 0x26; } 00998 else if(posC > 9+n && posC < 18+n ) { C = 0x49; } 00999 else if(posC > 18+n && posC < 27+n ) { C = 0x49; } 01000 else if(posC > 27+n && posC < 36+n ) { C = 0x49; } 01001 else if(posC > 36+n && posC < 45+n ) { C = 0x3E; } 01002 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 01003 } 01004 if (text == '7'){ 01005 if (posA > 0+n && posA < 9+n) { A = 0x60; } 01006 else if(posA > 9+n && posA < 18+n ) { A = 0x50; } 01007 else if(posA > 18+n && posA < 27+n ) { A = 0x47; } 01008 else if(posA > 27+n && posA < 36+n ) { A = 0x40; } 01009 else if(posA > 36+n && posA < 45+n ) { A = 0x40; } 01010 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 01011 01012 if (posB > 0+n && posB < 9+n) { B = 0x60; } 01013 else if(posB > 9+n && posB < 18+n ) { B = 0x50; } 01014 else if(posB > 18+n && posB < 27+n ) { B = 0x47; } 01015 else if(posB > 27+n && posB < 36+n ) { B = 0x40; } 01016 else if(posB > 36+n && posB < 45+n ) { B = 0x40; } 01017 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 01018 01019 if (posC > 0+n && posC < 9+n) { C = 0x60; } 01020 else if(posC > 9+n && posC < 18+n ) { C = 0x50; } 01021 else if(posC > 18+n && posC < 27+n ) { C = 0x47; } 01022 else if(posC > 27+n && posC < 36+n ) { C = 0x40; } 01023 else if(posC > 36+n && posC < 45+n ) { C = 0x40; } 01024 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 01025 } 01026 if (text == '8'){ 01027 if (posA > 0+n && posA < 9+n) { A = 0x36; } 01028 else if(posA > 9+n && posA < 18+n ) { A = 0x49; } 01029 else if(posA > 18+n && posA < 27+n ) { A = 0x49; } 01030 else if(posA > 27+n && posA < 36+n ) { A = 0x49; } 01031 else if(posA > 36+n && posA < 45+n ) { A = 0x36; } 01032 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 01033 01034 if (posB > 0+n && posB < 9+n) { B = 0x36; } 01035 else if(posB > 9+n && posB < 18+n ) { B = 0x49; } 01036 else if(posB > 18+n && posB < 27+n ) { B = 0x49; } 01037 else if(posB > 27+n && posB < 36+n ) { B = 0x49; } 01038 else if(posB > 36+n && posB < 45+n ) { B = 0x36; } 01039 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 01040 01041 if (posC > 0+n && posC < 9+n) { C = 0x36; } 01042 else if(posC > 9+n && posC < 18+n ) { C = 0x49; } 01043 else if(posC > 18+n && posC < 27+n ) { C = 0x49; } 01044 else if(posC > 27+n && posC < 36+n ) { C = 0x49; } 01045 else if(posC > 36+n && posC < 45+n ) { C = 0x36; } 01046 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 01047 } 01048 if (text == '9'){ 01049 if (posA > 0+n && posA < 9+n) { A = 0x3E; } 01050 else if(posA > 9+n && posA < 18+n ) { A = 0x49; } 01051 else if(posA > 18+n && posA < 27+n ) { A = 0x49; } 01052 else if(posA > 27+n && posA < 36+n ) { A = 0x49; } 01053 else if(posA > 36+n && posA < 45+n ) { A = 0x32; } 01054 else if(posA > 45+n && posA < 50+n ) { A = 0x00; } 01055 01056 if (posB > 0+n && posB < 9+n) { B = 0x3E; } 01057 else if(posB > 9+n && posB < 18+n ) { B = 0x49; } 01058 else if(posB > 18+n && posB < 27+n ) { B = 0x49; } 01059 else if(posB > 27+n && posB < 36+n ) { B = 0x49; } 01060 else if(posB > 36+n && posB < 45+n ) { B = 0x32; } 01061 else if(posB > 45+n && posB < 50+n ) { B = 0x00; } 01062 01063 if (posC > 0+n && posC < 9+n) { C = 0x3E; } 01064 else if(posC > 9+n && posC < 18+n ) { C = 0x49; } 01065 else if(posC > 18+n && posC < 27+n ) { C = 0x49; } 01066 else if(posC > 27+n && posC < 36+n ) { C = 0x49; } 01067 else if(posC > 36+n && posC < 45+n ) { C = 0x32; } 01068 else if(posC > 45+n && posC < 50+n ) { C = 0x00; } 01069 } 01070 } 01071 01072 void Velocity(float v ,float posA,float posB,float posC){ 01073 char value[3] ; 01074 float w = v*10; 01075 sprintf(value ,"%f",w); 01076 if (v<10){value[2] = value[1];value[1]=value[0];value[0] = 0;} 01077 if (posA > 180 && posA < 190) { A = 0x0F; } 01078 else if(posA > 190 && posA < 200 ) { A = 0x08; } 01079 else if(posA > 200 && posA < 210 ) { A = 0x7F; } 01080 else if(posA > 210 && posA < 220 ) { A = 0x03; } 01081 else if(posA > 220 && posA < 230 ) { A = 0x0C; } 01082 else if(posA > 230 && posA < 240 ) { A = 0x03; } 01083 else if(posA > 240 && posA < 250 ) { A = 0x0F; } 01084 else if(posA > 250 && posA < 260 ) { A = 0x08; } 01085 else if(posA > 260 && posA < 270 ) { A = 0x04; } 01086 else if(posA > 270 && posA < 280 ) { A = 0x08; } 01087 else if(posA > 280 && posA < 290 ) { A = 0x0F; } 01088 else if(posA > 290 && posA < 300 ) { A = 0x11; } 01089 else if(posA > 300 && posA < 310 ) { A = 0x0A; } 01090 else if(posA > 310 && posA < 320 ) { A = 0x04; } 01091 else if(posA > 320 && posA < 330 ) { A = 0x7F; } 01092 else if(posA > 330 && posA < 340 ) { A = 0x00; } 01093 01094 01095 01096 if (posB > 180 && posB < 190) { B = 0x0F; } 01097 else if(posB > 190 && posB < 200 ) { B = 0x08; } 01098 else if(posB > 200 && posB < 210 ) { B = 0x7F; } 01099 else if(posB > 210 && posB < 220 ) { B = 0x03; } 01100 else if(posB > 220 && posB < 230 ) { B = 0x0C; } 01101 else if(posB > 230 && posB < 240 ) { B = 0x03; } 01102 else if(posB > 240 && posB < 250 ) { B = 0x0F; } 01103 else if(posB > 250 && posB < 260 ) { B = 0x08; } 01104 else if(posB > 260 && posB < 270 ) { B = 0x04; } 01105 else if(posB > 270 && posB < 280 ) { B = 0x08; } 01106 else if(posB > 280 && posB < 290 ) { B = 0x0F; } 01107 else if(posB > 290 && posB < 300 ) { B = 0x11; } 01108 else if(posB > 300 && posB < 310 ) { B = 0x0A; } 01109 else if(posB > 310 && posB < 320 ) { B = 0x04; } 01110 else if(posB > 320 && posB < 330 ) { B = 0x7F; } 01111 else if(posB > 330 && posB < 340 ) { B = 0x7F; } 01112 else if(posB > 340 && posB < 350 ) { B = 0x00; } 01113 01114 if (posC > 180 && posC < 190) { C = 0x0F; } 01115 else if(posC > 190 && posC < 200 ) { C = 0x08; } 01116 else if(posC > 200 && posC < 210 ) { C = 0x7F; } 01117 else if(posC > 210 && posC < 220 ) { C = 0x03; } 01118 else if(posC > 220 && posC < 230 ) { C = 0x0C; } 01119 else if(posC > 230 && posC < 240 ) { C = 0x03; } 01120 else if(posC > 240 && posC < 250 ) { C = 0x0F; } 01121 else if(posC > 250 && posC < 260 ) { C = 0x08; } 01122 else if(posC > 260 && posC < 270 ) { C = 0x04; } 01123 else if(posC > 270 && posC < 280 ) { C = 0x08; } 01124 else if(posC > 280 && posC < 290 ) { C = 0x0F; } 01125 else if(posC > 290 && posC < 300 ) { C = 0x11; } 01126 else if(posC > 300 && posC < 310 ) { C = 0x0A; } 01127 else if(posC > 310 && posC < 320 ) { C = 0x04; } 01128 else if(posC > 320 && posC < 330 ) { C = 0x7F; } 01129 else if(posC > 330 && posC < 340 ) { C = 0x00; } 01130 01131 if (posA > 50 && posA < 60) { A = 0x01; } 01132 else if (posA > 60 && posA < 65) { A = 0x00; } 01133 if (posB > 50 && posB < 60) { B = 0x01; } 01134 else if (posB > 60 && posB < 65) { B = 0x00; } 01135 if (posC > 50 && posC < 60) { C = 0x01; } 01136 else if (posC > 60 && posC < 65) { C = 0x00; } 01137 Number(value[2],posA,posB,posC,0); 01138 Number(value[1],posA,posB,posC,1); 01139 Number(value[0],posA,posB,posC,2); 01140 } 01141 01142 01143 01144 01145 float posA = 0,posB,posC; 01146 01147 01148 01149 01150 01151 01152 01153 01154 01155 int main() 01156 { 01157 A = 0; 01158 B = 0; 01159 C = 0; 01160 mode = 0; 01161 pc.baud(38400); 01162 //Set up I2C 01163 i2c.frequency(400000); // use fast (400 kHz) I2C 01164 01165 bam.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock); 01166 01167 t.start(); 01168 01169 01170 // Read the WHO_AM_I register, this is a good test of communication 01171 uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250 01172 pc.printf("I AM 0x%x\n\r", whoami); bam.printf("I SHOULD BE 0x71\n\r"); 01173 01174 if (whoami == 0x71) // WHO_AM_I should always be 0x68 01175 { 01176 bam.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami); 01177 bam.printf("MPU9250 is online...\n\r"); 01178 sprintf(buffer, "0x%x", whoami); 01179 01180 wait(1); 01181 01182 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration 01183 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values 01184 bam.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]); 01185 bam.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]); 01186 bam.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]); 01187 bam.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]); 01188 bam.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]); 01189 bam.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]); 01190 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 01191 bam.printf("x gyro bias = %f\n\r", gyroBias[0]); 01192 bam.printf("y gyro bias = %f\n\r", gyroBias[1]); 01193 bam.printf("z gyro bias = %f\n\r", gyroBias[2]); 01194 bam.printf("x accel bias = %f\n\r", accelBias[0]); 01195 bam.printf("y accel bias = %f\n\r", accelBias[1]); 01196 bam.printf("z accel bias = %f\n\r", accelBias[2]); 01197 wait(2); 01198 mpu9250.initMPU9250(); 01199 bam.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 01200 mpu9250.initAK8963(magCalibration); 01201 bam.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer 01202 bam.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale)); 01203 bam.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale)); 01204 if(Mscale == 0) bam.printf("Magnetometer resolution = 14 bits\n\r"); 01205 if(Mscale == 1) bam.printf("Magnetometer resolution = 16 bits\n\r"); 01206 if(Mmode == 2) bam.printf("Magnetometer ODR = 8 Hz\n\r"); 01207 if(Mmode == 6) bam.printf("Magnetometer ODR = 100 Hz\n\r"); 01208 wait(1); 01209 } 01210 else 01211 { 01212 bam.printf("Could not connect to MPU9250: \n\r"); 01213 bam.printf("%#x \n", whoami); 01214 01215 sprintf(buffer, "WHO_AM_I 0x%x", whoami); 01216 01217 01218 while(1) ; // Loop forever if communication doesn't happen 01219 } 01220 01221 mpu9250.getAres(); // Get accelerometer sensitivity 01222 mpu9250.getGres(); // Get gyro sensitivity 01223 mpu9250.getMres(); // Get magnetometer sensitivity 01224 pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes); 01225 pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes); 01226 pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes); 01227 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated 01228 magbias[1] = +120.; // User environmental x-axis correction in milliGauss 01229 magbias[2] = +125.; // User environmental x-axis correction in milliGauss 01230 01231 while(1) { 01232 01233 01234 // If intPin goes high, all data registers have new data 01235 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt 01236 01237 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values 01238 // Calculate the gyro value into actual degrees per second 01239 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set 01240 gy = (float)gyroCount[1]*gRes - gyroBias[1]; 01241 gz = (float)gyroCount[2]*gRes - gyroBias[2]; 01242 01243 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); 01244 01245 // Serial print and/or display at 0.5 s rate independent of data rates 01246 delt_t = t.read_us() - count; 01247 if (delt_t > 1) { // update LCD once per half-second independent of read rate 01248 01249 x = 1; 01250 posA += (gz*delt_t*x/1000000); 01251 posA = fmod(posA,360); 01252 posB = fmod(posA + 240,360); 01253 posC = fmod(posA + 120,360); 01254 count = t.read_us(); 01255 01256 if (mode == 1) 01257 { 01258 01259 Velocity(v,posA,posB,posC); 01260 } 01261 else if (mode == 2) 01262 { 01263 Text((char)word[0],posA,posB,posC,5); 01264 Text((char)word[1],posA,posB,posC,4); 01265 Text((char)word[2],posA,posB,posC,3); 01266 Text((char)word[3],posA,posB,posC,2); 01267 Text((char)word[4],posA,posB,posC,1); 01268 Text((char)word[5],posA,posB,posC,0); 01269 } 01270 01271 } 01272 delt_t2 = t.read_us() - count2; 01273 if (delt_t2 >= 1000000) 01274 { 01275 v =(gz*x*PI*0.35*3600)/(180*1000); 01276 pc.printf("%0.1f",v); 01277 count2 = t.read_us(); 01278 } 01279 01280 if (pc.readable()) 01281 01282 { 01283 cmode = pc.getc(); 01284 switch(cmode) 01285 { 01286 case '&': 01287 word.clear(); 01288 break; 01289 01290 case '?': 01291 mode = 1; 01292 break; 01293 01294 case '!': 01295 mode = 2; 01296 break; 01297 01298 default: 01299 word += cmode; 01300 } 01301 // bam.printf("%c %s %d\n",cmode, word, word.length()); 01302 } 01303 } 01304 01305 01306 } 01307 }
Generated on Thu Jul 21 2022 05:18:58 by
1.7.2