Kobayashi Akihiro / ActiveCaster

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ActiveCaster_ ActiveCaster_2

SDclass.cpp@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
0:5e4f1e288e2a 
DDSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:5e4f1e288e2a 1 #include "SDclass.h"
e5119053f6 0:5e4f1e288e2a 2
e5119053f6 0:5e4f1e288e2a 3 char REDpath[20] = "/fs/PATH_RED.txt";
e5119053f6 0:5e4f1e288e2a 4 char REDvel[20] = "/fs/VEL_RED.txt";
e5119053f6 0:5e4f1e288e2a 5 char BLUpath[20] = "/fs/PATH_BLU.txt";
e5119053f6 0:5e4f1e288e2a 6 char BLUvel[20] = "/fs/VEL_BLU.txt";
e5119053f6 0:5e4f1e288e2a 7
e5119053f6 0:5e4f1e288e2a 8 FATFileSystem fs("fs");
e5119053f6 0:5e4f1e288e2a 9 SDBlockDevice sd(P8_5, P8_6, P8_3, P8_4);
e5119053f6 0:5e4f1e288e2a 10
e5119053f6 0:5e4f1e288e2a 11 mySDclass::mySDclass() {}
e5119053f6 0:5e4f1e288e2a 12
e5119053f6 0:5e4f1e288e2a 13 /* SDカードの初期化 */
e5119053f6 0:5e4f1e288e2a 14 int mySDclass::init() {
e5119053f6 0:5e4f1e288e2a 15 printf("mySDclass");
e5119053f6 0:5e4f1e288e2a 16
e5119053f6 0:5e4f1e288e2a 17 if (fs.mount(&sd) == 0) {
e5119053f6 0:5e4f1e288e2a 18 sd.frequency(2000000);
e5119053f6 0:5e4f1e288e2a 19 SD_enable = true;
e5119053f6 0:5e4f1e288e2a 20 printf(" init done");
e5119053f6 0:5e4f1e288e2a 21 return 0;
e5119053f6 0:5e4f1e288e2a 22 }
e5119053f6 0:5e4f1e288e2a 23
e5119053f6 0:5e4f1e288e2a 24 printf(" init failure");
e5119053f6 0:5e4f1e288e2a 25 return -1;
e5119053f6 0:5e4f1e288e2a 26 }
e5119053f6 0:5e4f1e288e2a 27
e5119053f6 0:5e4f1e288e2a 28 /* ログデータ書き出し用ファイル名の設定 */
e5119053f6 0:5e4f1e288e2a 29 int mySDclass::make_logfile() {
e5119053f6 0:5e4f1e288e2a 30 // bool nameOK = false;
e5119053f6 0:5e4f1e288e2a 31 int file_num = 0;
e5119053f6 0:5e4f1e288e2a 32
e5119053f6 0:5e4f1e288e2a 33 DIR *d;
e5119053f6 0:5e4f1e288e2a 34 d = opendir("/fs");
e5119053f6 0:5e4f1e288e2a 35 if (d != NULL) {
e5119053f6 0:5e4f1e288e2a 36 while (readdir(d) != NULL) {
e5119053f6 0:5e4f1e288e2a 37 file_num++;
e5119053f6 0:5e4f1e288e2a 38 }
e5119053f6 0:5e4f1e288e2a 39 }
e5119053f6 0:5e4f1e288e2a 40
e5119053f6 0:5e4f1e288e2a 41 int keta = 0;
e5119053f6 0:5e4f1e288e2a 42 for(int tmp = file_num; (tmp /= 10) != 0; keta++);
e5119053f6 0:5e4f1e288e2a 43 logFileName = "/fs/LOG_";
e5119053f6 0:5e4f1e288e2a 44 for(int i = 0; i < (2 - keta); i++){
e5119053f6 0:5e4f1e288e2a 45 logFileName += "0";
e5119053f6 0:5e4f1e288e2a 46 }
e5119053f6 0:5e4f1e288e2a 47 logFileName += to_string(file_num);
e5119053f6 0:5e4f1e288e2a 48 logFileName += ".txt";
e5119053f6 0:5e4f1e288e2a 49 //c_logFileName = logFileName.c_str();
e5119053f6 0:5e4f1e288e2a 50
e5119053f6 0:5e4f1e288e2a 51 return file_num;
e5119053f6 0:5e4f1e288e2a 52 }
e5119053f6 0:5e4f1e288e2a 53
e5119053f6 0:5e4f1e288e2a 54 /* ログデータ書き出し用の関数 */
e5119053f6 0:5e4f1e288e2a 55 int mySDclass::write_logdata(string dataString) {
e5119053f6 0:5e4f1e288e2a 56 FILE *dataFile = fopen(logFileName.c_str(), "w");
e5119053f6 0:5e4f1e288e2a 57
e5119053f6 0:5e4f1e288e2a 58 if (dataFile) {
e5119053f6 0:5e4f1e288e2a 59 fprintf(dataFile, dataString.c_str());
e5119053f6 0:5e4f1e288e2a 60 fclose(dataFile);
e5119053f6 0:5e4f1e288e2a 61 // Serial.println(dataString);
e5119053f6 0:5e4f1e288e2a 62 return 0;
e5119053f6 0:5e4f1e288e2a 63 } else {
e5119053f6 0:5e4f1e288e2a 64 // Serial.println("error opening ");
e5119053f6 0:5e4f1e288e2a 65 return -1;
e5119053f6 0:5e4f1e288e2a 66 }
e5119053f6 0:5e4f1e288e2a 67 }
e5119053f6 0:5e4f1e288e2a 68
e5119053f6 0:5e4f1e288e2a 69 int mySDclass::path_read(int field, double Px[], double Py[], double vel[],
e5119053f6 0:5e4f1e288e2a 70 double angle[], int mode[], int count[],
e5119053f6 0:5e4f1e288e2a 71 double tbe[]) {
e5119053f6 0:5e4f1e288e2a 72 FILE *myFile;
e5119053f6 0:5e4f1e288e2a 73 char *pathFile;
e5119053f6 0:5e4f1e288e2a 74 char *velFile;
e5119053f6 0:5e4f1e288e2a 75 char tmpchar;
e5119053f6 0:5e4f1e288e2a 76 char tmpA[10], tmpB[10], tmpC[10], tmpD[10], tmpE[10];
e5119053f6 0:5e4f1e288e2a 77 bool file_end = false;
e5119053f6 0:5e4f1e288e2a 78 int numa = 0, numb = 0, numc = 0, numd = 0, nume = 0;
e5119053f6 0:5e4f1e288e2a 79 int path_num = 0, point_num = 0;
e5119053f6 0:5e4f1e288e2a 80
e5119053f6 0:5e4f1e288e2a 81 // 赤か青かで読み込むファイルを変更する
e5119053f6 0:5e4f1e288e2a 82 if (field == RED) {
e5119053f6 0:5e4f1e288e2a 83 pathFile = REDpath; // 赤ゾーンのパス設定ファイル
e5119053f6 0:5e4f1e288e2a 84 velFile = REDvel; // 赤ゾーンの速度と角度の設定ファイル
e5119053f6 0:5e4f1e288e2a 85 } else if (field == BLUE) {
e5119053f6 0:5e4f1e288e2a 86 pathFile = BLUpath; // 青ゾーンのパス設定ファイル
e5119053f6 0:5e4f1e288e2a 87 velFile = BLUvel; // 青ゾーンの速度と角度の設定ファイル
e5119053f6 0:5e4f1e288e2a 88 } else {
e5119053f6 0:5e4f1e288e2a 89 return -1;
e5119053f6 0:5e4f1e288e2a 90 }
e5119053f6 0:5e4f1e288e2a 91
e5119053f6 0:5e4f1e288e2a 92 // パス設定用ファイルからデータを読み込む
e5119053f6 0:5e4f1e288e2a 93 printf("openning... %s\n", pathFile);
e5119053f6 0:5e4f1e288e2a 94 myFile = fopen(pathFile, "r");
e5119053f6 0:5e4f1e288e2a 95
e5119053f6 0:5e4f1e288e2a 96 if (myFile) {
e5119053f6 0:5e4f1e288e2a 97 // read from the file until there's nothing else in it:
e5119053f6 0:5e4f1e288e2a 98 while (!file_end && !feof(myFile)) {
e5119053f6 0:5e4f1e288e2a 99 while ((tmpchar = fgetc(myFile)) != ',') { // カンマが来るまで繰り返し
e5119053f6 0:5e4f1e288e2a 100 tmpA[numa] = tmpchar; // 文字列に1文字ずつ格納していく
e5119053f6 0:5e4f1e288e2a 101 numa++;
e5119053f6 0:5e4f1e288e2a 102 }
e5119053f6 0:5e4f1e288e2a 103 *Px = str2double(tmpA, numa); //関数でdoubleに変換
e5119053f6 0:5e4f1e288e2a 104 // Serial.print(tmpA);
e5119053f6 0:5e4f1e288e2a 105 printf("%lf, ", *Px);
e5119053f6 0:5e4f1e288e2a 106 for (int i = 0; i < 10; i++)
e5119053f6 0:5e4f1e288e2a 107 tmpA[i] = 0; // 文字列を初期化
e5119053f6 0:5e4f1e288e2a 108 numa = 0;
e5119053f6 0:5e4f1e288e2a 109 Px++;
e5119053f6 0:5e4f1e288e2a 110 while (((tmpchar = fgetc(myFile)) != '\r' && tmpchar != ';') && tmpchar != '/') { // 改行コードかセミコロン，スラッシュが来るまで繰り返し
e5119053f6 0:5e4f1e288e2a 111 tmpB[numb] = tmpchar;
e5119053f6 0:5e4f1e288e2a 112 numb++;
e5119053f6 0:5e4f1e288e2a 113 }
e5119053f6 0:5e4f1e288e2a 114 if (tmpchar == ';') {
e5119053f6 0:5e4f1e288e2a 115 file_end = true;
e5119053f6 0:5e4f1e288e2a 116 } else if (tmpchar == '/') { // コメントアウト対応
e5119053f6 0:5e4f1e288e2a 117 while ((tmpchar = fgetc(myFile)) != '\n');
e5119053f6 0:5e4f1e288e2a 118 } else {
e5119053f6 0:5e4f1e288e2a 119 fgetc(myFile); // "\n"を捨てるため
e5119053f6 0:5e4f1e288e2a 120 }
e5119053f6 0:5e4f1e288e2a 121 *Py = str2double(tmpB, numb); //関数でdoubleに変換
e5119053f6 0:5e4f1e288e2a 122 point_num++;
e5119053f6 0:5e4f1e288e2a 123
e5119053f6 0:5e4f1e288e2a 124 printf("%lf\n", *Py);
e5119053f6 0:5e4f1e288e2a 125 // Serial.println(tmpB);
e5119053f6 0:5e4f1e288e2a 126 for (int i = 0; i < 10; i++)
e5119053f6 0:5e4f1e288e2a 127 tmpB[i] = 0;
e5119053f6 0:5e4f1e288e2a 128 numb = 0;
e5119053f6 0:5e4f1e288e2a 129 Py++;
e5119053f6 0:5e4f1e288e2a 130 }
e5119053f6 0:5e4f1e288e2a 131 // Serial.print("path done! ");
e5119053f6 0:5e4f1e288e2a 132 file_end = false;
e5119053f6 0:5e4f1e288e2a 133 // the file:
e5119053f6 0:5e4f1e288e2a 134 fclose(myFile);
e5119053f6 0:5e4f1e288e2a 135 } else {
e5119053f6 0:5e4f1e288e2a 136 // if the file didn't open, print an error:
e5119053f6 0:5e4f1e288e2a 137 // Serial.println("error opening test.txt");
e5119053f6 0:5e4f1e288e2a 138 return -2;
e5119053f6 0:5e4f1e288e2a 139 }
e5119053f6 0:5e4f1e288e2a 140 printf("point num: %d\n", point_num);
e5119053f6 0:5e4f1e288e2a 141
e5119053f6 0:5e4f1e288e2a 142 // 速度設定用ファイルからデータを読み込む
e5119053f6 0:5e4f1e288e2a 143 printf("openning... %s\n", velFile);
e5119053f6 0:5e4f1e288e2a 144 myFile = fopen(velFile, "r");
e5119053f6 0:5e4f1e288e2a 145 if (myFile) {
e5119053f6 0:5e4f1e288e2a 146 while (!file_end && !feof(myFile)) {
e5119053f6 0:5e4f1e288e2a 147 while ((tmpchar = fgetc(myFile)) != ',') {
e5119053f6 0:5e4f1e288e2a 148 tmpA[numa] = tmpchar;
e5119053f6 0:5e4f1e288e2a 149 numa++;
e5119053f6 0:5e4f1e288e2a 150 }
e5119053f6 0:5e4f1e288e2a 151 *vel = str2double(tmpA, numa); //関数でdoubleに変換
e5119053f6 0:5e4f1e288e2a 152 // Serial.print(tmpA);
e5119053f6 0:5e4f1e288e2a 153 printf("%lf, ", *vel);
e5119053f6 0:5e4f1e288e2a 154 for (int i = 0; i < 10; i++)
e5119053f6 0:5e4f1e288e2a 155 tmpA[i] = 0;
e5119053f6 0:5e4f1e288e2a 156 numa = 0;
e5119053f6 0:5e4f1e288e2a 157 vel++;
e5119053f6 0:5e4f1e288e2a 158 //////////////////////////////////
e5119053f6 0:5e4f1e288e2a 159 while ((tmpchar = fgetc(myFile)) != ',') {
e5119053f6 0:5e4f1e288e2a 160 tmpB[numb] = tmpchar;
e5119053f6 0:5e4f1e288e2a 161 numb++;
e5119053f6 0:5e4f1e288e2a 162 }
e5119053f6 0:5e4f1e288e2a 163 *angle = str2double(tmpB, numb); //関数でdoubleに変換
e5119053f6 0:5e4f1e288e2a 164 // Serial.print(tmpA);
e5119053f6 0:5e4f1e288e2a 165 printf("%lf, ", *angle);
e5119053f6 0:5e4f1e288e2a 166 for (int i = 0; i < 10; i++)
e5119053f6 0:5e4f1e288e2a 167 tmpB[i] = 0;
e5119053f6 0:5e4f1e288e2a 168 numb = 0;
e5119053f6 0:5e4f1e288e2a 169 angle++;
e5119053f6 0:5e4f1e288e2a 170 //////////////////////////////////
e5119053f6 0:5e4f1e288e2a 171 while ((tmpchar = fgetc(myFile)) != ',') {
e5119053f6 0:5e4f1e288e2a 172 tmpC[numc] = tmpchar;
e5119053f6 0:5e4f1e288e2a 173 numc++;
e5119053f6 0:5e4f1e288e2a 174 }
e5119053f6 0:5e4f1e288e2a 175 *mode = str2uint(tmpC, numc); //関数でdoubleに変換
e5119053f6 0:5e4f1e288e2a 176 // Serial.print(tmpA);
e5119053f6 0:5e4f1e288e2a 177 printf("%d, ", *mode);
e5119053f6 0:5e4f1e288e2a 178 for (int i = 0; i < 10; i++)
e5119053f6 0:5e4f1e288e2a 179 tmpC[i] = 0;
e5119053f6 0:5e4f1e288e2a 180 numc = 0;
e5119053f6 0:5e4f1e288e2a 181 mode++;
e5119053f6 0:5e4f1e288e2a 182 //////////////////////////////////
e5119053f6 0:5e4f1e288e2a 183 while ((tmpchar = fgetc(myFile)) != ',') {
e5119053f6 0:5e4f1e288e2a 184 tmpD[numd] = tmpchar;
e5119053f6 0:5e4f1e288e2a 185 numd++;
e5119053f6 0:5e4f1e288e2a 186 }
e5119053f6 0:5e4f1e288e2a 187 *count = str2uint(tmpD, numd); //関数でdoubleに変換
e5119053f6 0:5e4f1e288e2a 188 // Serial.print(tmpA);
e5119053f6 0:5e4f1e288e2a 189 printf("%d, ", *count);
e5119053f6 0:5e4f1e288e2a 190 for (int i = 0; i < 10; i++)
e5119053f6 0:5e4f1e288e2a 191 tmpD[i] = 0;
e5119053f6 0:5e4f1e288e2a 192 numd = 0;
e5119053f6 0:5e4f1e288e2a 193 count++;
e5119053f6 0:5e4f1e288e2a 194 //////////////////////////////////
e5119053f6 0:5e4f1e288e2a 195 while (((tmpchar = fgetc(myFile)) != '\r' && tmpchar != ';') &&
e5119053f6 0:5e4f1e288e2a 196 tmpchar != '/') {
e5119053f6 0:5e4f1e288e2a 197 tmpE[nume] = tmpchar;
e5119053f6 0:5e4f1e288e2a 198 nume++;
e5119053f6 0:5e4f1e288e2a 199 }
e5119053f6 0:5e4f1e288e2a 200 if (tmpchar == ';') {
e5119053f6 0:5e4f1e288e2a 201 file_end = true;
e5119053f6 0:5e4f1e288e2a 202 } else if (tmpchar == '/') { // コメントアウト対応
e5119053f6 0:5e4f1e288e2a 203 while ((tmpchar = fgetc(myFile)) != '\n');
e5119053f6 0:5e4f1e288e2a 204 } else {
e5119053f6 0:5e4f1e288e2a 205 fgetc(myFile); // "\n"を捨てるため
e5119053f6 0:5e4f1e288e2a 206 }
e5119053f6 0:5e4f1e288e2a 207 *tbe = str2double(tmpE, nume); //関数でdoubleに変換
e5119053f6 0:5e4f1e288e2a 208 path_num++;
e5119053f6 0:5e4f1e288e2a 209
e5119053f6 0:5e4f1e288e2a 210 // Serial.print(tmpB);
e5119053f6 0:5e4f1e288e2a 211 printf("%lf\n", *tbe);
e5119053f6 0:5e4f1e288e2a 212 for (int i = 0; i < 10; i++)
e5119053f6 0:5e4f1e288e2a 213 tmpE[i] = 0;
e5119053f6 0:5e4f1e288e2a 214 nume = 0;
e5119053f6 0:5e4f1e288e2a 215 tbe++;
e5119053f6 0:5e4f1e288e2a 216 }
e5119053f6 0:5e4f1e288e2a 217 // Serial.println("vel/angle done!");
e5119053f6 0:5e4f1e288e2a 218 // close the file:
e5119053f6 0:5e4f1e288e2a 219 fclose(myFile);
e5119053f6 0:5e4f1e288e2a 220 } else {
e5119053f6 0:5e4f1e288e2a 221 // if the file didn't open, print an error:
e5119053f6 0:5e4f1e288e2a 222 // Serial.println("error opening test.txt");
e5119053f6 0:5e4f1e288e2a 223 return -3;
e5119053f6 0:5e4f1e288e2a 224 }
e5119053f6 0:5e4f1e288e2a 225
e5119053f6 0:5e4f1e288e2a 226 if ((int)((point_num - 2) / 3) >= (path_num - 1)) {
e5119053f6 0:5e4f1e288e2a 227 return path_num - 1;
e5119053f6 0:5e4f1e288e2a 228 }
e5119053f6 0:5e4f1e288e2a 229 return -4;
e5119053f6 0:5e4f1e288e2a 230 }
e5119053f6 0:5e4f1e288e2a 231
e5119053f6 0:5e4f1e288e2a 232 double mySDclass::str2double(char *str, int num) {
e5119053f6 0:5e4f1e288e2a 233 double ret = 0.0;
e5119053f6 0:5e4f1e288e2a 234 bool minus = false;
e5119053f6 0:5e4f1e288e2a 235 int m = 0, keta;
e5119053f6 0:5e4f1e288e2a 236
e5119053f6 0:5e4f1e288e2a 237 // マイナス符号が付いているかチェック
e5119053f6 0:5e4f1e288e2a 238 if (str[0] == '-') {
e5119053f6 0:5e4f1e288e2a 239 minus = true;
e5119053f6 0:5e4f1e288e2a 240 m++;
e5119053f6 0:5e4f1e288e2a 241 }
e5119053f6 0:5e4f1e288e2a 242
e5119053f6 0:5e4f1e288e2a 243 // 何桁あるかを確認
e5119053f6 0:5e4f1e288e2a 244 keta = m;
e5119053f6 0:5e4f1e288e2a 245 while ((str[keta] != '.') && (keta < num)) {
e5119053f6 0:5e4f1e288e2a 246 keta++;
e5119053f6 0:5e4f1e288e2a 247 }
e5119053f6 0:5e4f1e288e2a 248 keta = keta - (m + 1);
e5119053f6 0:5e4f1e288e2a 249
e5119053f6 0:5e4f1e288e2a 250 // 整数部を変換
e5119053f6 0:5e4f1e288e2a 251 for (int i = m; i <= (keta + m); i++) {
e5119053f6 0:5e4f1e288e2a 252 if (str[i] >= 48 && str[i] <= 57) {
e5119053f6 0:5e4f1e288e2a 253 ret += (double)(str[i] - 48) * pow(10.0, keta - (i - m));
e5119053f6 0:5e4f1e288e2a 254 }
e5119053f6 0:5e4f1e288e2a 255 }
e5119053f6 0:5e4f1e288e2a 256
e5119053f6 0:5e4f1e288e2a 257 // 小数部を変換
e5119053f6 0:5e4f1e288e2a 258 int n = -1;
e5119053f6 0:5e4f1e288e2a 259 for (int i = keta + m + 2; i < num; i++) {
e5119053f6 0:5e4f1e288e2a 260 if (str[i] >= 48 && str[i] <= 57) {
e5119053f6 0:5e4f1e288e2a 261 ret += (double)(str[i] - 48) * pow(10.0, n);
e5119053f6 0:5e4f1e288e2a 262 n--;
e5119053f6 0:5e4f1e288e2a 263 }
e5119053f6 0:5e4f1e288e2a 264 }
e5119053f6 0:5e4f1e288e2a 265 if (minus)
e5119053f6 0:5e4f1e288e2a 266 return -1.0 * ret;
e5119053f6 0:5e4f1e288e2a 267 return ret;
e5119053f6 0:5e4f1e288e2a 268 }
e5119053f6 0:5e4f1e288e2a 269
e5119053f6 0:5e4f1e288e2a 270 int mySDclass::str2uint(char *str, int num) {
e5119053f6 0:5e4f1e288e2a 271 num--;
e5119053f6 0:5e4f1e288e2a 272 int ret = 0;
e5119053f6 0:5e4f1e288e2a 273 // bool minus = false;
e5119053f6 0:5e4f1e288e2a 274
e5119053f6 0:5e4f1e288e2a 275 // 整数部を変換
e5119053f6 0:5e4f1e288e2a 276 for (int i = 0; i <= num; i++) {
e5119053f6 0:5e4f1e288e2a 277 if (str[i] >= 48 && str[i] <= 57) {
e5119053f6 0:5e4f1e288e2a 278 ret += (double)(str[i] - 48) * pow(10.0, num - i);
e5119053f6 0:5e4f1e288e2a 279 }
e5119053f6 0:5e4f1e288e2a 280 }
e5119053f6 0:5e4f1e288e2a 281
e5119053f6 0:5e4f1e288e2a 282 return ret;
e5119053f6 0:5e4f1e288e2a 283 }