SDclass.cpp

00001 #include "SDclass.h"
00002 
00003 char REDpath[20] = "/fs/PATH_RED.txt";
00004 char REDvel[20] = "/fs/VEL_RED.txt";
00005 char BLUpath[20] = "/fs/PATH_BLU.txt";
00006 char BLUvel[20] = "/fs/VEL_BLU.txt";
00007 
00008 FATFileSystem fs("fs");
00009 SDBlockDevice sd(P8_5, P8_6, P8_3, P8_4);
00010 
00011 mySDclass::mySDclass() {}
00012 
00013 /* SDカードの初期化 */
00014 int mySDclass::init() {
00015   printf("mySDclass");
00016 
00017   if (fs.mount(&sd) == 0) {
00018     sd.frequency(2000000);
00019     SD_enable = true;
00020     printf(" init done");
00021     return 0;
00022   }
00023 
00024   printf(" init failure");
00025   return -1;
00026 }
00027 
00028 /* ログデータ書き出し用ファイル名の設定 */
00029 int mySDclass::make_logfile() {
00030   // bool nameOK = false;
00031   int file_num = 0;
00032 
00033   DIR *d;
00034   d = opendir("/fs");
00035   if (d != NULL) {
00036     while (readdir(d) != NULL) {
00037       file_num++;
00038     }
00039   }
00040 
00041   int keta = 0;
00042   for(int tmp = file_num; (tmp /= 10) != 0; keta++);
00043   logFileName = "/fs/LOG_";
00044   for(int i = 0; i < (2 - keta); i++){
00045       logFileName += "0";
00046   }
00047   logFileName += to_string(file_num);
00048   logFileName += ".txt";
00049   //c_logFileName = logFileName.c_str();
00050   
00051   return file_num;
00052 }
00053 
00054 /* ログデータ書き出し用の関数 */
00055 int mySDclass::write_logdata(string dataString) {
00056   FILE *dataFile = fopen(logFileName.c_str(), "w");
00057 
00058   if (dataFile) {
00059     fprintf(dataFile, dataString.c_str());
00060     fclose(dataFile);
00061     // Serial.println(dataString);
00062     return 0;
00063   } else {
00064     // Serial.println("error opening ");
00065     return -1;
00066   }
00067 }
00068 
00069 int mySDclass::path_read(int field, double Px[], double Py[], double vel[],
00070                          double angle[], int mode[], int count[],
00071                          double tbe[]) {
00072   FILE *myFile;
00073   char *pathFile;
00074   char *velFile;
00075   char tmpchar;
00076   char tmpA[10], tmpB[10], tmpC[10], tmpD[10], tmpE[10];
00077   bool file_end = false;
00078   int numa = 0, numb = 0, numc = 0, numd = 0, nume = 0;
00079   int path_num = 0, point_num = 0;
00080 
00081   // 赤か青かで読み込むファイルを変更する
00082   if (field == RED) {
00083     pathFile = REDpath; // 赤ゾーンのパス設定ファイル
00084     velFile = REDvel;   // 赤ゾーンの速度と角度の設定ファイル
00085   } else if (field == BLUE) {
00086     pathFile = BLUpath; // 青ゾーンのパス設定ファイル
00087     velFile = BLUvel;   // 青ゾーンの速度と角度の設定ファイル
00088   } else {
00089     return -1;
00090   }
00091 
00092   // パス設定用ファイルからデータを読み込む
00093   printf("openning... %s\n", pathFile);
00094   myFile = fopen(pathFile, "r");
00095 
00096   if (myFile) {
00097     // read from the file until there's nothing else in it:
00098     while (!file_end && !feof(myFile)) {
00099       while ((tmpchar = fgetc(myFile)) != ',') { // カンマが来るまで繰り返し
00100         tmpA[numa] = tmpchar; // 文字列に1文字ずつ格納していく
00101         numa++;
00102       }
00103       *Px = str2double(tmpA, numa); //関数でdoubleに変換
00104       // Serial.print(tmpA);
00105       printf("%lf, ", *Px);
00106       for (int i = 0; i < 10; i++)
00107         tmpA[i] = 0; // 文字列を初期化
00108       numa = 0;
00109       Px++;
00110       while (((tmpchar = fgetc(myFile)) != '\r' && tmpchar != ';') && tmpchar != '/') { // 改行コードかセミコロン，スラッシュが来るまで繰り返し
00111         tmpB[numb] = tmpchar;
00112         numb++;
00113       }
00114       if (tmpchar == ';') {
00115         file_end = true;
00116       } else if (tmpchar == '/') { // コメントアウト対応
00117         while ((tmpchar = fgetc(myFile)) != '\n');
00118       } else {
00119         fgetc(myFile); // "\n"を捨てるため
00120       }
00121       *Py = str2double(tmpB, numb); //関数でdoubleに変換
00122       point_num++;
00123 
00124       printf("%lf\n", *Py);
00125       // Serial.println(tmpB);
00126       for (int i = 0; i < 10; i++)
00127         tmpB[i] = 0;
00128       numb = 0;
00129       Py++;
00130     }
00131     // Serial.print("path done! ");
00132     file_end = false;
00133     //   the file:
00134     fclose(myFile);
00135   } else {
00136     // if the file didn't open, print an error:
00137     // Serial.println("error opening test.txt");
00138     return -2;
00139   }
00140   printf("point num: %d\n", point_num);
00141 
00142   // 速度設定用ファイルからデータを読み込む
00143   printf("openning... %s\n", velFile);
00144   myFile = fopen(velFile, "r");
00145   if (myFile) {
00146     while (!file_end && !feof(myFile)) {
00147       while ((tmpchar = fgetc(myFile)) != ',') {
00148         tmpA[numa] = tmpchar;
00149         numa++;
00150       }
00151       *vel = str2double(tmpA, numa); //関数でdoubleに変換
00152       // Serial.print(tmpA);
00153       printf("%lf, ", *vel);
00154       for (int i = 0; i < 10; i++)
00155         tmpA[i] = 0;
00156       numa = 0;
00157       vel++;
00158       //////////////////////////////////
00159       while ((tmpchar = fgetc(myFile)) != ',') {
00160         tmpB[numb] = tmpchar;
00161         numb++;
00162       }
00163       *angle = str2double(tmpB, numb); //関数でdoubleに変換
00164       // Serial.print(tmpA);
00165       printf("%lf, ", *angle);
00166       for (int i = 0; i < 10; i++)
00167         tmpB[i] = 0;
00168       numb = 0;
00169       angle++;
00170       //////////////////////////////////
00171       while ((tmpchar = fgetc(myFile)) != ',') {
00172         tmpC[numc] = tmpchar;
00173         numc++;
00174       }
00175       *mode = str2uint(tmpC, numc); //関数でdoubleに変換
00176       // Serial.print(tmpA);
00177       printf("%d, ", *mode);
00178       for (int i = 0; i < 10; i++)
00179         tmpC[i] = 0;
00180       numc = 0;
00181       mode++;
00182       //////////////////////////////////
00183       while ((tmpchar = fgetc(myFile)) != ',') {
00184         tmpD[numd] = tmpchar;
00185         numd++;
00186       }
00187       *count = str2uint(tmpD, numd); //関数でdoubleに変換
00188       // Serial.print(tmpA);
00189       printf("%d, ", *count);
00190       for (int i = 0; i < 10; i++)
00191         tmpD[i] = 0;
00192       numd = 0;
00193       count++;
00194       //////////////////////////////////
00195       while (((tmpchar = fgetc(myFile)) != '\r' && tmpchar != ';') &&
00196              tmpchar != '/') {
00197         tmpE[nume] = tmpchar;
00198         nume++;
00199       }
00200       if (tmpchar == ';') {
00201         file_end = true;
00202       } else if (tmpchar == '/') { // コメントアウト対応
00203         while ((tmpchar = fgetc(myFile)) != '\n');
00204       } else {
00205         fgetc(myFile); // "\n"を捨てるため
00206       }
00207       *tbe = str2double(tmpE, nume); //関数でdoubleに変換
00208       path_num++;
00209 
00210       // Serial.print(tmpB);
00211       printf("%lf\n", *tbe);
00212       for (int i = 0; i < 10; i++)
00213         tmpE[i] = 0;
00214       nume = 0;
00215       tbe++;
00216     }
00217     // Serial.println("vel/angle done!");
00218     // close the file:
00219     fclose(myFile);
00220   } else {
00221     // if the file didn't open, print an error:
00222     // Serial.println("error opening test.txt");
00223     return -3;
00224   }
00225 
00226   if ((int)((point_num - 2) / 3) >= (path_num - 1)) {
00227     return path_num - 1;
00228   }
00229   return -4;
00230 }
00231 
00232 double mySDclass::str2double(char *str, int num) {
00233   double ret = 0.0;
00234   bool minus = false;
00235   int m = 0, keta;
00236 
00237   // マイナス符号が付いているかチェック
00238   if (str[0] == '-') {
00239     minus = true;
00240     m++;
00241   }
00242 
00243   // 何桁あるかを確認
00244   keta = m;
00245   while ((str[keta] != '.') && (keta < num)) {
00246     keta++;
00247   }
00248   keta = keta - (m + 1);
00249 
00250   // 整数部を変換
00251   for (int i = m; i <= (keta + m); i++) {
00252     if (str[i] >= 48 && str[i] <= 57) {
00253       ret += (double)(str[i] - 48) * pow(10.0, keta - (i - m));
00254     }
00255   }
00256 
00257   // 小数部を変換
00258   int n = -1;
00259   for (int i = keta + m + 2; i < num; i++) {
00260     if (str[i] >= 48 && str[i] <= 57) {
00261       ret += (double)(str[i] - 48) * pow(10.0, n);
00262       n--;
00263     }
00264   }
00265   if (minus)
00266     return -1.0 * ret;
00267   return ret;
00268 }
00269 
00270 int mySDclass::str2uint(char *str, int num) {
00271   num--;
00272   int ret = 0;
00273   // bool minus = false;
00274 
00275   // 整数部を変換
00276   for (int i = 0; i <= num; i++) {
00277     if (str[i] >= 48 && str[i] <= 57) {
00278       ret += (double)(str[i] - 48) * pow(10.0, num - i);
00279     }
00280   }
00281 
00282   return ret;
00283 }