Diff: Utils/ControllerUtil.cpp

Revision:

20:891b5270845a

Parent:

19:2f0ec9ac1238

--- a/Utils/ControllerUtil.cpp	Sat Jan 23 00:20:27 2016 +0000
+++ b/Utils/ControllerUtil.cpp	Tue Jan 26 17:34:51 2016 +0000
@@ -4,14 +4,19 @@
 #include "mbed.h"
 #include "rtos.h"
 
-#include <RobotArm.h>
-#include <MeasureBuf.h>
-#include <ControllerIo.h>
+#include "RobotArm.h"
+#include "MeasureBuf.h"
 #include "Alert.h"
+#include "ControllerIo.h"
+#include "Timestamp.h"
+
 
 // use controller timer
 extern Timer IdleTimer;
 
+// use timestamp to get secs and ms
+Timestamp MessageTimer;
+
 
 // utility method to show state in console
 void DispMeasure(char* label, int partSize, float vals[])
@@ -19,7 +24,7 @@
 //    printf("%s: ", label);
 //    for (int ix = 0; ix < partSize; ix++)
 //    {
-//        printf("%d:%f ", ix, vals[ix]); 
+//        printf("%d:%f ", ix, vals[ix]);
 //    }
 //    printf("\r\n");
 }
@@ -37,10 +42,10 @@
     printf("Position error detected joint %d, value diff %f\r\n", ix, diff);
 
     Alert alert;
-    time_t seconds = time(NULL);
+    MessageTimer.GetTimestamp();
     
     ShowLedRed();
-    alert.SetPositionAlert(seconds, ix, diff);
+    alert.SetPositionAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, diff);
     AlertBuf.push(alert);
     
     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
@@ -60,10 +65,10 @@
     printf("Load error detected joint %d, value %f\r\n", ix, lastVals[ix]);
 
     Alert alert;
-    time_t seconds = time(NULL);
+    MessageTimer.GetTimestamp();
     
     ShowLedRed();
-    alert.SetLoadAlert(seconds, ix, lastVals[ix]);
+    alert.SetLoadAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
     AlertBuf.push(alert);
     
     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
@@ -83,10 +88,10 @@
     printf("Temperature error detected joint %d, value %f\r\n", ix, lastVals[ix]);
 
     Alert alert;
-    time_t seconds = time(NULL);
+    MessageTimer.GetTimestamp();
     
     ShowLedRed();
-    alert.SetTemperatureAlert(seconds, ix, lastVals[ix]);
+    alert.SetTemperatureAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
     AlertBuf.push(alert);
     
     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
@@ -106,10 +111,10 @@
     printf("Voltage error detected joint %d, value %f\r\n", ix, lastVals[ix]);
 
     Alert alert;
-    time_t seconds = time(NULL);
+    MessageTimer.GetTimestamp();
     
     ShowLedRed();
-    alert.SetVoltageAlert(seconds, ix, lastVals[ix]);
+    alert.SetVoltageAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), ix, lastVals[ix]);
     AlertBuf.push(alert);
     
     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
@@ -120,10 +125,10 @@
 void PushHardwareAlert(int partIx, int code)
 {
     Alert alert;
-    time_t seconds = time(NULL);
+    MessageTimer.GetTimestamp();
     
     ShowLedRed();
-    alert.SetHardwareAlert(seconds, partIx, code);
+    alert.SetHardwareAlert(MessageTimer.GetSecs(), MessageTimer.GetMs(), partIx, code);
     AlertBuf.push(alert);
        
     BuzzerStartMs((int)IdleTimer.read_ms(), 500);
@@ -135,7 +140,9 @@
     MeasureSnapshot measureSnap;
     
     float lastVals[NUMJOINTS];
-    measureSnap.Created = time(NULL);
+    MessageTimer.GetTimestamp();
+    measureSnap.Created = MessageTimer.GetSecs();
+    measureSnap.CreatedMs = MessageTimer.GetMs();
    
     bool ok = true;