Diff: bench.cpp

Revision:

5:63063a9fa51c

Parent:

4:1cdce6c6c94e

Child:

6:02507d7a6f51

diff -r 1cdce6c6c94e -r 63063a9fa51c bench.cpp
--- a/bench.cpp	Fri Oct 12 12:12:55 2018 +0000
+++ b/bench.cpp	Mon Dec 09 10:51:46 2019 +0000
@@ -7,31 +7,154 @@
  * @param miso: miso pin for sensor chain
  * @param sck: clock pin for sensor chain
  * @param cs: chip select pin for sensor chain
+ * @param use5kNLoadCell: if 5kN load cell is used (if false, 1kN sensor is used)
  * @param p_lcm101: analog input pin for load cell
  * @param p_spte0: analog input pin for pressure sensor 0
  * @param p_spte1: analog input pin for pressure sensor 1
+ * @param p_valve: digital output for valve (on/off) that inflates leg actuator
+ * @param mosi: mosi pin for sd card
+ * @param miso: miso pin for sd card
+ * @param sck: clock pin for sd card
+ * @param cs: chip select pin for sd card
+ * @param tx: serial transmission line
+ * @param rx: serial receive line
+ * @param but0: first input button 
+ * @param but1: second input button
  */
 Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
-      PinName p_lcm101, PinName p_spte0, PinName p_spte1) :
+             bool use5kNLoadCell,PinName p_lcm101, PinName p_spte0, PinName p_spte1, PinName p_valve,
+             PinName sd_mosi, PinName sd_miso, PinName sd_sck, PinName sd_cs,
+             PinName tx, PinName rx,
+             PinName but0, PinName but1) :
+    pc(tx,rx),
+    lowerBut(but0,PullUp),
+    upperBut(but1,PullUp),
     as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
-    lcm101_(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor),
+    loadCell5kN(p_lcm101, sensors::kGen5kNOffset, sensors::kGen5kNFactor),
+    loadCell1kN(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor),
+    use5kN(use5kNLoadCell),
     spte0(p_spte0, sensors::kSPTE0Offset, sensors::kSPTE0Factor),
-    spte1(p_spte1, sensors::kSPTE1Offset, sensors::kSPTE1Factor)
-{
+    spte1(p_spte1, sensors::kSPTE1Offset, sensors::kSPTE1Factor),
+    valveFesto(p_valve),
+    sd(sd_mosi, sd_miso, sd_sck, sd_cs, "sd")
+{    
+    //init serial things
+    pc.baud(timing::kSerialBaudrate);//set the serial rate
+    
+    sd_card_present = false;
+    fname_prepend = 0;
+    is_logging = false;
+    is_printing = false;
+    was_printing = false;
+        
+    usedExtraCols = 0;
+    
+    firstReadMS = 0; //first timer value read
+    startedLogging = false; //in the middle of a logging cycle
+    
+    //set data logging frequency
+    loggingUS = timing::kTimeLogDataUs;    
+    loggingHz = (int)(1000000/timing::kTimeLogDataUs); 
+    
     for (int i=0; i<sensors::kNumJoints; ++i) {
         as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
         as5048_.setDirection(i,sensors::kDirections[i]);
+    }   
+}
+
+/**
+ * Initialises: timers, SD card
+ */
+void Bench::initialise()
+{
+    //setup the timing
+    tick_update.attach_us(this,&Bench::update,timing::kTimeControlUs);
+    
+        
+    // set rate at which data is printed
+    tick_serial.attach_us(this,&Bench::PrintStatus,timing::kTimeSerialPrintUs);
+     
+    //setup the buttons with debouncing    
+    lowerBut.attach_asserted(this,&Bench::TogglePrinting);
+    upperBut.attach_asserted(this,&Bench::ToggleLogging);
+    
+    lowerBut.setSampleFrequency();
+    upperBut.setSampleFrequency();
+    
+    //display welcome message
+    pc.printf("\r\n**Hello!**\r\n");
+    pc.printf("Version: 3/12/2019 - 00:00\r\n\n");
+    
+    Bench::pc.printf("5kN load cell? %s\r\n\n",sensors::use5kN?"Yes":"No");
+    
+    pc.printf("Bench update rate (Hz): %i\r\n",timing::TimeControlHertz);
+    pc.printf("Logging rate (Hz): %i\r\n\n",(int)loggingHz);
+    
+    //startup the SD card
+    InitSdCard();
+    
+    //Display the menu
+    PrintMenu();    
+}
+
+/**
+ * Sets the rate at which data is logged in an experiment
+ * @param hertz: logging frequency in Hz
+ */
+void Bench::setLoggingFrequency(float hertz)
+{
+    //set data logging frequency
+    if (hertz > 0) {
+        loggingUS = 1000000/hertz;
+        loggingHz = hertz;
+    } else {
+        loggingUS = timing::kTimeLogDataUs;
+        loggingHz = (int)(1000000/timing::kTimeLogDataUs);
     }
 }
 
 /**
+ * Sets names of extra columns of data to be recorded
+ * @param extraColumnNames: string array of the column names
+ * @param numCols: number of elements in the string array
+ */
+void Bench::setExtraColumns(string extraColumnNames[], int numCols)
+{
+    usedExtraCols = numCols;
+    //pc.printf("Length: %i\r\n",usedExtraCols);
+    // save the names   
+    for(int i=0; i<maxCols; i++) {
+        if (i<numCols) {
+            extraColNames[i] = extraColumnNames[i];
+            //pc.printf("\r\nS%d: %s\r\n",i,extraColNames[i]);
+        } else {
+            extraColNames[i] = ""; //less columns than the max possible are filled with a null space
+        }
+    }
+    
+}
+
+/**
+ * Sets values of extra columns of data to be recorded
+ * @param data: data to be logged with the extra columns
+ */
+void Bench::setExtraData(float data[])
+{
+    for (int i=0; i<usedExtraCols; i++) {
+        extraColValues[i] = data[i];
+        //pc.printf("\r\nD%d: %f\r\n",i,extraColValues[i]);
+    }
+    
+}
+
+/**
  * Update routine for the testbench.
  * - Updates the angle buffer of the sensor array
  * - ... that's it for now (add filtering?)
  * Note that angles lag one Update() behind, due to the way the SPI
  * protocol works.
  */
-void Bench::Update() 
+void Bench::update() 
 {
     as5048_.UpdateAngleBuffer();
 }
@@ -45,6 +168,253 @@
     return ang;
 }
 
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// IMPLEMENTATION DATA LOGGING
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+/**
+ * Check contents of SD card and count files in order
+ * to ensure unique file name for logging data
+ */
+void Bench::InitSdCard()
+{
+    pc.printf("\r\nINITIALISING 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:\r\n");
+        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("\r\n\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 Bench::StartLogging(const char * fname_append)
+{    
+    pc.printf("\r\nDATA 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 {
+            string fHeader = "time (s),theta_knee (deg),force (N),pressure (kPa)";
+            
+            for (int i=0; i<usedExtraCols; i++) {
+                if (extraColNames[i] != "") {
+                    fHeader = fHeader + "," + extraColNames[i];
+                }
+            }
+            //pc.printf("%s", fHeader.c_str());
+            fprintf(fp_data, "%s", fHeader.c_str());
+            tick_logging.attach_us(this,&Bench::LogData,loggingUS);
+            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;
+    }
+}
+
+
+/**
+ * Stop logging data
+ */
+void Bench::StopLogging()
+{
+    Bench::pc.printf("\r\nDATA LOGGING:");
+    if (sd_card_present) {
+        // close data file, stop logging
+        fclose(fp_data);
+        tick_logging.detach();
+        timer.stop();
+        timer.reset();
+        Bench::pc.printf("\r> Stopped.");
+    } else {
+        Bench::pc.printf("\t> No data was logged.");
+    }
+    
+    is_logging = false;
+}
+
+/**
+ * Stop logging data and print the menu
+ */
+void Bench::stopLogging()
+{
+    if(is_logging) {
+        is_logging = false;
+        StopLogging();
+        PrintMenu();
+    }    
+}
+
+/**
+ * Log data
+ */
+void Bench::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 and pressure sensor values
+    fprintf(fp_data,",%f,%f,%f",
+            getDegrees(0),
+            getForce(),            
+            getPressure0()*100
+           );
+    for (int i=0; i<usedExtraCols; i++) {
+        fprintf(fp_data,",%f",extraColValues[i]);
+    }            
+}
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// IMPLEMENTATION SERIAL COM
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =      
+/**
+ * Prints the values of the key measurements in the setup
+ */
+void Bench::PrintStatus()
+{    
+    if (is_printing) {
+        if(sd_card_present)
+        {
+            Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
+            Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
+        }
+        
+        for (int i=0; i<sensors::kNumJoints; ++i)
+        {
+            string jointName = sensors::kJointNames[i];
+            jointName = jointName + " (deg)";
+            Bench::pc.printf("\t%15s %7.2f\r\n",jointName, getDegrees(i));
+        }
+        Bench::pc.printf("\t%15s %7.2f\r\n","Force (N)",  getForce()); 
+        
+        Bench::pc.printf("\t%15s %7.2f\r\n","Pressure0 (kPa)",  getPressure0()*100);   
+    }
+}
+
+/**
+ * Prints the user choices to be made
+ */
+void Bench::PrintMenu()
+{
+    Bench::pc.printf("\r\nMENU\r\n");
+    Bench::pc.printf("\t> Press SW2 to toggle printing leg status\r\n");
+    Bench::pc.printf("\t> Press SW3 to toggle data logging\r\n");
+    
+    Bench::pc.printf("\tSD card detected? %9s\r\n",sd_card_present?"Yes":"No");    
+    if(sd_card_present)
+    {
+        Bench::pc.printf("\tFile number %15i\r\n", fname_prepend);
+        Bench::pc.printf("\tLogging? %18s\r\n",is_logging?"Yes":"No");
+    }
+}
+
+/**
+ * Stops the Bench class from printing data if it is
+ */
+void Bench::pausePrint()
+{
+    was_printing = is_printing;
+    is_printing = false;
+}
+
+/**
+ * Resumes printing in the Bench class
+ */
+void Bench::resumePrint()
+{
+    is_printing = was_printing;
+}
+
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+// IMPLEMENTATION USER IO
+// = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
+
+/**
+ * button lower toggles printing of data
+ */
+void Bench::TogglePrinting()
+{
+    if (not is_printing) {
+        is_printing = true;
+    } else {
+        is_printing = false;
+        PrintMenu();
+    }
+    was_printing = is_printing;
+}
+
+/*
+ * button lower toggles printing of data
+ */
+void Bench::ToggleLogging()
+{
+    if (not is_logging) {
+        StartLogging();
+    } else {
+        is_logging = false;
+        StopLogging();
+    }        
+    PrintMenu();
+}
+
+/*
+ * Indicates if we are now data logging
+ */
+bool Bench::isLogging()
+{
+    return is_logging;
+}
+    
 /**
  * Obtain the joint angle in degrees.
  * These are the angles at the time of two Update() calls back
@@ -94,12 +464,21 @@
 
 float Bench::getForce() 
 {
-    return lcm101_.getForce();
+    if (Bench::use5kN) {
+        return loadCell5kN.getForce();
+    } else {
+        return loadCell1kN.getForce();
+    }
+    
 }
 
 void Bench::nullForce()
 {
-    return lcm101_.nullForce();
+    if (use5kN) {
+        return loadCell5kN.nullForce();
+    } else {
+        return loadCell1kN.nullForce();
+    }
 }
 
 float Bench::getPressure0() 
@@ -122,4 +501,11 @@
     return spte1.nullPressure();
 }
 
+void Bench::setValve(bool set)
+{
+    valveFesto.setValve(set);
+}
 
+bool Bench::getValve() {
+    return valveFesto.getValve();   
+}
\ No newline at end of file