main.cpp

00001 #include "mbed.h"
00002 #include "string.h"
00003 
00004 #include "bench.h"
00005 #include "PinDetect.h"
00006 #include "SDFileSystem.h"
00007 #include "MODSERIAL.h"
00008 
00009 
00010 // User io
00011 PinDetect sw2(SW2,PullUp);
00012 PinDetect sw3(SW3,PullUp);
00013 DigitalOut led_g(LED_GREEN);
00014 
00015 void TogglePrinting();
00016 void ToggleLogging();
00017 void ShowAlive();
00018 
00019 // Bench
00020 Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101);
00021 void Update()
00022 {
00023     leg.Update();
00024 }
00025 
00026 // SD Card
00027 SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd");
00028 
00029 void InitSdCard();
00030 void StartLogging(const char * fname_append = "data");
00031 void StopLogging();
00032 void LogData();
00033 
00034 // Serial
00035 MODSERIAL pc(USBTX,USBRX);
00036 
00037 void PrintStatus();
00038 void PrintMenu();
00039 
00040 // Timing
00041 Ticker tick_update, tick_serial, tick_logging;
00042 Timer timer;
00043 
00044 /**
00045  * Main loop/
00046  */
00047 int main()
00048 {
00049     pc.baud(timing::kSerialBaudrate);
00050     pc.printf("**Hello!**\r\n");
00051 
00052     InitSdCard();
00053 
00054     tick_update.attach_us(&Update,timing::kTimeControlUs);
00055     tick_serial.attach_us(&PrintStatus,timing::kTimeSerialPrintUs);
00056 
00057     PrintMenu();
00058     
00059     sw2.attach_asserted(&TogglePrinting);
00060     sw3.attach_asserted(&ToggleLogging);
00061     
00062     sw2.setSampleFrequency();
00063     sw3.setSampleFrequency();
00064 
00065     while (true);
00066 }
00067 
00068 
00069 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
00070 // IMPLEMENTATION USER IO
00071 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
00072 
00073 bool is_printing = false;
00074 void TogglePrinting()
00075 {
00076     if (not is_printing) {
00077         is_printing = true;
00078     } else {
00079         is_printing = false;
00080         PrintMenu();
00081     }
00082 }
00083 
00084 bool is_logging = false;
00085 void ToggleLogging()
00086 {
00087     if (not is_logging) {
00088         StartLogging();
00089     } else {
00090         is_logging = false;
00091         StopLogging();
00092     }
00093     PrintMenu();
00094 }
00095 
00096 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
00097 // IMPLEMENTATION SERIAL COM
00098 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
00099 const int kNumJoints = 4;
00100 const char *kJointNames[kNumJoints] = {"Toes","Ankle","knee","Hip"};
00101 
00102 void PrintStatus()
00103 {
00104     led_g = !led_g;
00105     if (is_printing) {
00106         pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*");
00107         for (int i=0; i<kNumJoints; ++i)
00108             pc.printf("\t%5s %7.2f\r\n",kJointNames[i], leg.getDegrees(i));
00109         pc.printf("\t%5s %7.2f\r\n","Force",  leg.getForce());
00110     }
00111 }
00112 
00113 void PrintMenu()
00114 {
00115     pc.printf("\r\nMENU\r\n");
00116     pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
00117     pc.printf("\t> Press SW3 to toggle data logging\r\n");
00118 }
00119 
00120 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
00121 // IMPLEMENTATION DATA LOGGING
00122 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
00123 
00124 FILE * fp_data;
00125 
00126 bool sd_card_present = false;
00127 int fname_prepend = 0;
00128 
00129 /**
00130  * Check contents of SD card and count files in order
00131  * to ensure unique file name for logging data
00132  */
00133 void InitSdCard()
00134 {
00135     pc.printf("INITIALIZING SD CARD\r\n");
00136 
00137     int num_files = 0;
00138 
00139     // scan dir
00140     DIR *d;
00141     struct dirent *p;
00142 
00143     d = opendir("/sd");
00144     if (d != NULL) {
00145         sd_card_present = true;
00146 
00147         pc.printf("\t> Contents of SD Card:");
00148         while ((p = readdir(d)) != NULL) {
00149             if (p->d_name[0] != '.') {
00150                 // skip files starting with '.'
00151                 pc.printf("\t  %s",p->d_name);
00152                 ++num_files;
00153             }
00154         }
00155         pc.printf("\t> Counted %d visible files.\r\n",num_files);
00156 
00157         closedir(d);
00158     } else {
00159         sd_card_present = false;
00160 
00161         pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
00162     }
00163 
00164     // id to be appended to logged data files
00165     fname_prepend = num_files;
00166 }
00167 
00168 /**
00169  * Start logging data
00170  */
00171 void StartLogging(const char * fname_append)
00172 {
00173     
00174     pc.printf("DATA LOGGING");
00175     if (sd_card_present) {
00176 
00177         // create unique file name
00178         ++fname_prepend;
00179         char fname[50];
00180         sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
00181 
00182         pc.printf("\t> Opening data log file '%s'...\r\n",fname);
00183 
00184         // open file for writing and start logging after success
00185         fp_data = fopen(fname,"w");
00186         if (fp_data==NULL) {
00187             pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
00188                       timer.read_ms());
00189         } else {
00190             fprintf(fp_data, "time_ms, theta_toe, theta_ankle, theta_knee, theta_hip, force");
00191             tick_logging.attach_us(&LogData,timing::kTimeLogDataUs);
00192             timer.start();
00193             pc.printf("\t> Logging started.\r\n");
00194             
00195             is_logging = true;
00196         }
00197 
00198     } else {
00199         pc.printf("\t> No SD Card; no data will be logged.\r\n");
00200     }
00201 }
00202 
00203 
00204 /**
00205  * Stop logging data
00206  */
00207 void StopLogging()
00208 {
00209     pc.printf("DATA LOGGING:");
00210     if (sd_card_present) {
00211         // close data file, stop logging
00212         fclose(fp_data);
00213         tick_logging.detach();
00214         timer.stop();
00215         timer.reset();
00216         pc.printf("\r> Stopped.");
00217     } else {
00218         pc.printf("\t> No data was logged.");
00219     }
00220     
00221     is_logging = false;
00222 }
00223 
00224 /**
00225  * Log data
00226  */
00227 void LogData()
00228 {
00229     // time
00230     fprintf(fp_data,"\n%d", timer.read_ms());
00231 
00232     // bench: joint angles and force sensor
00233     fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f",
00234             leg.getDegrees(0),
00235             leg.getDegrees(1),
00236             leg.getDegrees(2),
00237             leg.getDegrees(3),
00238             leg.getForce()
00239            );
00240 }