Diff: main.cpp

Revision:

4:1cdce6c6c94e

Parent:

2:84d479fe9b5e

Child:

5:63063a9fa51c

--- a/main.cpp	Tue Jun 05 08:51:45 2018 +0000
+++ b/main.cpp	Fri Oct 12 12:12:55 2018 +0000
@@ -1,5 +1,5 @@
 #include "mbed.h"
-#include "string.h"
+#include <string>
 
 #include "bench.h"
 #include "PinDetect.h"
@@ -9,16 +9,22 @@
 #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();
 
 // Bench
-Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101);
+Bench leg(AS5048_MOSI, AS5048_MISO, AS5048_SCLK, AS5048_CS, LCM101,SPTE_0,SPTE_1);
 void Update()
 {
     leg.Update();
@@ -41,6 +47,8 @@
 // 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
 
 /**
  * Main loop/
@@ -57,6 +65,12 @@
 
     PrintMenu();
     
+    forceNull.attach_asserted(&ResetForce);
+    press0Null.attach_asserted(&ResetPress);
+    
+    forceNull.setSampleFrequency();
+    press0Null.setSampleFrequency();
+    
     sw2.attach_asserted(&TogglePrinting);
     sw3.attach_asserted(&ToggleLogging);
     
@@ -92,6 +106,17 @@
 // IMPLEMENTATION USER IO
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
 
+void ResetForce()
+{
+    leg.nullForce();
+}
+
+void ResetPress()
+{
+    leg.nullPressure0();
+    leg.nullPressure1();
+}
+
 bool is_printing = false;
 void TogglePrinting()
 {
@@ -119,17 +144,33 @@
 // IMPLEMENTATION SERIAL COM
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
 const int kNumJoints = 4;
-const char *kJointNames[kNumJoints] = {"Toes","Ankle","knee","Hip"};
+const char *kJointNames[kNumJoints] = {"Toes","Ankle","Knee","Hip"};
+    
+bool sd_card_present = false;
+int fname_prepend = 0;    
     
 void PrintStatus()
 {
     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)
-            pc.printf("\t%5s %7.2f\r\n",kJointNames[i], leg.getDegrees(i));
-        pc.printf("\t%5s %7.2f\r\n","Force",  leg.getForce());
-        pc.printf("\t%5s %7.2f\r\n","Torque",  getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()));
+        {
+            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()));        
     }
 }
 
@@ -138,6 +179,13 @@
     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");
+    }
 }
 
 // = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = = =
@@ -146,8 +194,7 @@
 
 FILE * fp_data;
 
-bool sd_card_present = false;
-int fname_prepend = 0;
+
 
 /**
  * Check contents of SD card and count files in order
@@ -210,9 +257,11 @@
             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, torque");
+            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;
@@ -220,6 +269,7 @@
 
     } else {
         pc.printf("\t> No SD Card; no data will be logged.\r\n");
+        startedLogging = false;
     }
 }
 
@@ -243,22 +293,30 @@
     
     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%d", timer.read_ms());
+    fprintf(fp_data,"\n%+f",currTimeS);
 
     // bench: joint angles and force sensor
-    fprintf(fp_data,", %+f, %+f, %+f, %+f, %+f, %+f",
+    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())
+            getTorque(leg.getDegrees(3),leg.getDegrees(2),leg.getForce()),
+            leg.getPressure0()*100,
+            leg.getPressure1()*100
            );
 }
\ No newline at end of file