Diff: main.cpp

Revision:

0:3855d4588f76

Child:

1:417a5b28ac84

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 01 11:14:38 2017 +0000
@@ -0,0 +1,237 @@
+#include "mbed.h"
+#include "string.h"
+
+#include "bench.h"
+#include "PinDetect.h"
+#include "SDFileSystem.h"
+#include "MODSERIAL.h"
+
+
+// User io
+PinDetect sw2(SW2,PullUp);
+PinDetect sw3(SW3,PullUp);
+DigitalOut led_g(LED_GREEN);
+
+void TogglePrinting();
+void ToggleLogging();
+void ShowAlive();
+
+// Bench
+Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101);
+void Update()
+{
+    leg.Update();
+}
+
+// SD Card
+SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd");
+
+void InitSdCard();
+void StartLogging(const char * fname_append = "data");
+void StopLogging();
+void LogData();
+
+// Serial
+MODSERIAL pc(USBTX,USBRX);
+
+void PrintStatus();
+void PrintMenu();
+
+// Timing
+Ticker tick_update, tick_serial, tick_logging;
+Timer timer;
+
+/**
+ * Main loop/
+ */
+int main()
+{
+    pc.baud(timing::kSerialBaudrate);
+    pc.printf("**Hello!**\r\n");
+
+    InitSdCard();
+
+    tick_update.attach_us(&Update,timing::kTimeControlUs);
+    tick_serial.attach_us(&PrintStatus,timing::kTimeSerialPrintUs);
+
+    PrintMenu();
+    
+    sw2.attach_asserted(&TogglePrinting);
+    sw3.attach_asserted(&ToggleLogging);
+    
+    sw2.setSampleFrequency();
+    sw3.setSampleFrequency();
+
+    while (true);
+}
+
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// IMPLEMENTATION USER IO
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+bool is_printing = false;
+void TogglePrinting()
+{
+    if (not is_printing) {
+        is_printing = true;
+    } else {
+        is_printing = false;
+        PrintMenu();
+    }
+}
+
+bool is_logging = false;
+void ToggleLogging()
+{
+    if (not is_logging) {
+        StartLogging();
+    } else {
+        is_logging = false;
+        StopLogging();
+    }
+    PrintMenu();
+}
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// IMPLEMENTATION SERIAL COM
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+const int kNumJoints = 4;
+const char *kJointNames[kNumJoints] = {"Toes","Ankle","knee","Hip"};
+
+void PrintStatus()
+{
+    led_g = !led_g;
+    if (is_printing) {
+        pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*");
+        for (int i=0; i<kNumJoints; ++i)
+            pc.printf("\t%5s %7.2f\r\n",kJointNames[i], leg.getDegrees(i));
+        pc.printf("\t%5s %7.2f\r\n","Force",  leg.getForce());
+    }
+}
+
+void PrintMenu()
+{
+    pc.printf("\r\nMENU\r\n");
+    pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
+    pc.printf("\t> Press SW3 to toggle data logging\r\n");
+}
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// IMPLEMENTATION DATA LOGGING
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+FILE * fp_data;
+
+bool sd_card_present = false;
+int fname_prepend = 0;
+
+/**
+ * Check contents of SD card and count files in order
+ * to ensure unique file name for logging data
+ */
+void InitSdCard()
+{
+    pc.printf("INITIALIZING SD CARD\r\n");
+
+    int num_files = 0;
+
+    // scan dir
+    DIR *d;
+    struct dirent *p;
+
+    d = opendir("/sd");
+    if (d != NULL) {
+        sd_card_present = true;
+
+        pc.printf("\t> Contents of SD Card:");
+        while ((p = readdir(d)) != NULL) {
+            if (p->d_name[0] != '.') {
+                // skip files starting with '.'
+                pc.printf("\t  %s",p->d_name);
+                ++num_files;
+            }
+        }
+        pc.printf("\t> Counted %d visible files.\r\n",num_files);
+
+        closedir(d);
+    } else {
+        sd_card_present = false;
+
+        pc.printf("\t> No SD Card present. Data cannot be logged.\r\n");
+    }
+
+    // id to be appended to logged data files
+    fname_prepend = num_files;
+}
+
+/**
+ * Start logging data
+ */
+void StartLogging(const char * fname_append)
+{
+    
+    pc.printf("DATA LOGGING");
+    if (sd_card_present) {
+
+        // create unique file name
+        ++fname_prepend;
+        char fname[50];
+        sprintf(fname, "/sd/%d_%s.csv",fname_prepend,fname_append);
+
+        pc.printf("\t> Opening data log file '%s'...\r\n",fname);
+
+        // open file for writing and start logging after success
+        fp_data = fopen(fname,"w");
+        if (fp_data==NULL) {
+            pc.printf("\t> ERROR: failed to open log file (t=%d ms)\r\n",
+                      timer.read_ms());
+        } else {
+            fprintf(fp_data, "time_ms, theta_toe, theta_ankle, theta_knee, theta_hip, force");
+            tick_logging.attach_us(&LogData,timing::kTimeLogDataUs);
+            pc.printf("\t> Logging started.\r\n");
+            
+            is_logging = true;
+        }
+
+    } else {
+        pc.printf("\t> No SD Card; no data will be logged.\r\n");
+    }
+}
+
+
+/**
+ * Stop logging data
+ */
+void StopLogging()
+{
+    pc.printf("DATA LOGGING:");
+    if (sd_card_present) {
+        // close data file, stop logging
+        fclose(fp_data);
+        tick_logging.detach();
+        pc.printf("\r> Stopped.");
+    } else {
+        pc.printf("\t> No data was logged.");
+    }
+    
+    is_logging = false;
+}
+
+/**
+ * Log data
+ */
+void LogData()
+{
+    // time
+    fprintf(fp_data,"\n%d", timer.read_ms());
+
+    // bench: joint angles and force sensor
+    fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f",
+            leg.getDegrees(0),
+            leg.getDegrees(1),
+            leg.getDegrees(2),
+            leg.getDegrees(3),
+            leg.getForce()
+           );
+}
\ No newline at end of file