Diff: main.cpp

Revision:

5:63063a9fa51c

Parent:

4:1cdce6c6c94e

Child:

10:77fcbad99a31

--- a/main.cpp	Fri Oct 12 12:12:55 2018 +0000
+++ b/main.cpp	Mon Dec 09 10:51:46 2019 +0000
@@ -1,322 +1,120 @@
-#include "mbed.h"
-#include <string>
-
-#include "bench.h"
-#include "PinDetect.h"
-#include "SDFileSystem.h"
-#include "MODSERIAL.h"
-
-#define PI 3.14159265
-
-// User io
-
-PinDetect forceNull(PTB16,PullUp);
-PinDetect press0Null(PTA4,PullUp);
-
-PinDetect sw2(SW2,PullUp);
-PinDetect sw3(SW3,PullUp);
-DigitalOut led_g(LED_GREEN);
-
-void ResetForce();
-void ResetPress();
-void TogglePrinting();
-void ToggleLogging();
-void ShowAlive();
+/**
+* Author: Allan Veale
+* Date: 27/11/19
+* Purpose: Datalog from the active wearable test rig fitted with the first
+* realistic (foam tissue) leg
+*/
 
-// Bench
-Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101,SPTE_0,SPTE_1);
-void Update()
-{
-    leg.Update();
-}
-
-// SD Card
-SDFileSystem sd(SD_MOSI, SD_MISO, SD_SCK, SD_CS, "sd");
+//Both the general mbed header and the test rig bench header are needed 
+#include "mbed.h"
+#include "bench.h"
 
-void InitSdCard();
-void StartLogging(const char * fname_append = "data");
-void StopLogging();
-void LogData();
-
-// Serial
-MODSERIAL pc(USBTX,USBRX);
+//Example experiment method 
+void runDemoExperiment0(int cycles, float targetkPa);
 
-void PrintStatus();
-void PrintMenu();
-
-// Timing
-Ticker tick_update, tick_serial, tick_logging;
-Timer timer;
-int firstReadMS = 0; //first timer value read
-bool startedLogging = false; //in the middle of a logging cycle
+// Create bench object - this is used to control the test rig
+Bench leg;
 
 /**
- * Main loop/
+ * 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();
+{   
+    leg.setLoggingFrequency(100); //Set datalogging frequency
+    
+    /* Two extra columns of data will be recorded in this experiment.
+    One for the target pressure, and the other for the number of sit and 
+    stand cycles currently completed in the experiment */ 
+    string colNames[] = {"Target pressure (kPa)","Cycle"}; //add data headings
+    leg.setExtraColumns(colNames,2);
     
-    forceNull.attach_asserted(&ResetForce);
-    press0Null.attach_asserted(&ResetPress);
-    
-    forceNull.setSampleFrequency();
-    press0Null.setSampleFrequency();
+    float targetP = 200; //Target pressure in kPa
+    int expCycles = 2; //Number of sit to stand to sit cycles 
+    float vals[] = {targetP,0}; //set initial values of data that will be logged
+    leg.setExtraData(vals);
     
-    sw2.attach_asserted(&TogglePrinting);
-    sw3.attach_asserted(&ToggleLogging);
-    
-    sw2.setSampleFrequency();
-    sw3.setSampleFrequency();
+    /* Setup all peripherals on rig, display info about SD card and the 
+    user interface menu */ 
+    leg.initialise(); 
 
-    while (true);
-}
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-// IMPLEMENTATION torque calculations
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-float getTorque(float theta_hip, float theta_knee, float M0)
-{
-    float theta_shin = theta_knee - theta_hip;
-    //Values for torque calculation
-    const float Mt = 7.24;//mass of thigh in kg
-    const float Ms = 3.06;//mass of shank in kg
-    const float Marm = 2.0;//mass of load carrying arm in kg
-    const float g = 9.81;//gravity coefficient in m/s2
-    const float dt = 0.45;//length of thigh in m
-    const float dtm = 0.25755;//distance to mass middle point thigh in m
-    const float ds = 0.45;//length of shank in m
-    const float dsm = 0.197134;//distance to mass middle point shank in m
-    const float shank_angle_correction = 6.04;//corrections for the difference in moment arm angle
-    const float thigh_angle_correction = 3.59;//corrections for the difference in moment arm angle
-    float Mw=M0/g;//M0 is measured in N, but it is calculated as kg in the equation. So instead of changing the whole equation, this is a quick fix.
-    //Calculation of knee torque
-    float Tk = -1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))-((-1*(Ms*g)*(dsm*sin((theta_shin+shank_angle_correction)*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180))-(Mt*g)*(dtm*sin((theta_hip-thigh_angle_correction)*PI/180))-(Mw+Marm)*g*(dt*sin(theta_hip*PI/180)))/(ds*cos(theta_shin*PI/180)+dt*cos(theta_hip*PI/180)))*(ds*cos(theta_shin*PI/180))+(Mw+Marm+Mt+Ms)*g*(ds*sin(theta_shin*PI/180));
-    
-    return Tk;
-}
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-// IMPLEMENTATION USER IO
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-
-void ResetForce()
-{
-    leg.nullForce();
-}
-
-void ResetPress()
-{
-    leg.nullPressure0();
-    leg.nullPressure1();
-}
-
-bool is_printing = false;
-void TogglePrinting()
-{
-    if (not is_printing) {
-        is_printing = true;
-    } else {
-        is_printing = false;
-        PrintMenu();
+      
+    /*Run an experiment when the button is pressed to start datalogging and 
+    stop it if the button is pressed again to stop datalogging 
+    (or when experiment stops - then datalogging stops by itself) */
+    while (true) {
+        if (leg.isLogging()) {
+            runDemoExperiment0(expCycles,targetP);            
+        }
     }
 }
 
-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"};
-    
-bool sd_card_present = false;
-int fname_prepend = 0;    
-    
-void PrintStatus()
+/**
+ * Shows how a demo experiment works. This experiment pressurises the leg to 
+ * pressure targetkPa, depressurises it, and then repeats the process cycles 
+ * number of times
+ * @param cycles: the number of cycles the leg goes up and down
+ * @param targetkPa: the pressure at which the valve is opened to let the leg go down
+ */
+void runDemoExperiment0(int cycles, float targetkPa) 
 {
-    led_g = !led_g;
-    if (is_printing) {
-        pc.printf("\r\nLEG STATUS (%s)\r\n",led_g?"+":"*");
-        pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");
-        if(sd_card_present)
-        {
-            pc.printf("\tFile number %15i\r\n", fname_prepend);
-            pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
-        }
-        pc.printf("\n\t%15s %7.2f\r\n","Pressure0 (kPa)",  leg.getPressure0()*100);
-        pc.printf("\t%15s %7.2f\r\n","Pressure1 (kPa)",  leg.getPressure1()*100);
-        
-        for (int i=0; i<kNumJoints; ++i)
-        {
-            string jointName = kJointNames[i];
-            jointName = jointName + " (deg)";
-            pc.printf("\t%15s %7.2f\r\n",jointName, leg.getDegrees(i));
-        }
-        pc.printf("\t%15s %7.2f\r\n","Force (N)",  leg.getForce());
-        pc.printf("\t%15s %7.2f\r\n","Torque (Nm)",  getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()));        
-    }
-}
+    //The experiment starts when logging does
+    bool experimentRunning = leg.isLogging(); 
+    
+    //Stop the Bench class from printing, so this method can print
+    leg.pausePrint();
+    
+    if (experimentRunning) {
+        // Pressurise and depressurise the leg cycles number of times
+        for (int i=0; i<cycles; i++) {
+            leg.pc.printf("\r\nCycle: \t%i out of \t%i",i+1,cycles);
+            
+            //Update cycles logged
+            float data[] = {targetkPa,i+1};
+            leg.setExtraData(data);
+
+            //Pressurise
+            leg.setValve(true);
 
-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");
-    
-    pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");    
-    if(sd_card_present)
-    {
-        pc.printf("\tFile number %15i\r\n", fname_prepend);
-        pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
-    }
-}
-
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-// IMPLEMENTATION DATA LOGGING
-// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
-
-FILE * fp_data;
-
-
+            //Wait until measured pressure reaches target pressure
+            float measureP = 0;
+            while(measureP < targetkPa) {
+                //Keep checking logging is going
+                experimentRunning = leg.isLogging(); 
+                
+                if (experimentRunning) {
+                    measureP = leg.getPressure0()*100;//Conversion of bar to kPa
+                    wait(0.2);//Wait a bit
+                } else { //Logging stopped
+                    leg.stopLogging(); //Stop logging data
+                    leg.setValve(false); //Depressurise
+                    leg.resumePrint(); //Let the Bench class print
+                    return;
+                }
+            }
 
-/**
- * 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;
+            //Depressurise
+            leg.setValve(false);
 
-    // 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;
+            /*Wait until depressurised (completely depressurised is 
+            around 10-12 kPa due to current sensor calibration)*/
+            while(measureP > 15) {
+                //Keep checking logging is going
+                experimentRunning = leg.isLogging();
+                
+                if (experimentRunning) {
+                    measureP = leg.getPressure0()*100;//Conversion of bar to kpa
+                    wait(0.2);//Wait a bit
+                } else { //Logging stopped
+                    leg.stopLogging(); //Stop logging data
+                    leg.resumePrint(); //Let the Bench class print
+                    return;
+                }
             }
         }
-        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 (s), theta_toe (deg), theta_ankle (deg), theta_knee (deg), theta_hip (deg), force (N), torque (Nm), pressure0 (kPa), pressure1 (kPa)");
-            tick_logging.attach_us(&LogData,timing::kTimeLogDataUs);
-            timer.start();
-            startedLogging = true;
-            
-            pc.printf("\t> Logging started.\r\n");
-            
-            is_logging = true;
-        }
-
-    } else {
-        pc.printf("\t> No SD Card; no data will be logged.\r\n");
-        startedLogging = false;
+        // Logging stopped as experiment is fully completed
+        leg.stopLogging(); //Stop logging data
+        leg.resumePrint(); //Let the Bench class print
     }
 }
 
 
-/**
- * Stop logging data
- */
-void StopLogging()
-{
-    pc.printf("DATA LOGGING:");
-    if (sd_card_present) {
-        // close data file, stop logging
-        fclose(fp_data);
-        tick_logging.detach();
-        timer.stop();
-        timer.reset();
-        pc.printf("\r> Stopped.");
-    } else {
-        pc.printf("\t> No data was logged.");
-    }
-    
-    is_logging = false;
-}
-/**
- * Log data
- */
-void LogData()
-{
-    int currTime = timer.read_ms();
-    if(startedLogging) {
-        firstReadMS = currTime;
-        startedLogging = false;
-    }
-    double currTimeS = ((double)currTime - firstReadMS)/1000;
-    
-    // time
-    fprintf(fp_data,"\n%+f",currTimeS);
-
-    // bench: joint angles and force sensor
-    fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f, %+f, %+f, %+f",
-            leg.getDegrees(0),
-            leg.getDegrees(1),
-            leg.getDegrees(2),
-            leg.getDegrees(3),
-            leg.getForce(),
-            getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()),
-            leg.getPressure0()*100,
-            leg.getPressure1()*100
-           );
-}
\ No newline at end of file