Revision 20:891b5270845a, committed 2016-01-26

Comitter:

henryrawas

Date:

Tue Jan 26 17:34:51 2016 +0000

Parent:

19:2f0ec9ac1238

Child:

21:051751f9ca9e

Commit message:

add ms to time. Change taps to repeat

Changed in this revision

--- a/Sequences.cpp	Sat Jan 23 00:20:27 2016 +0000
+++ b/Sequences.cpp	Tue Jan 26 17:34:51 2016 +0000
@@ -30,14 +30,9 @@
     float bigwaveDownPositions[] = { NoMove, 225.0f, NoMove, NoMove, NoMove };
     
     float tapStartPositions[] = { RightPos, 180.0f, 270.0f, 180.0f, 180.0f };
-    float tap1Positions[] = { 60.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap2Positions[] = { 68.7f, NoMove, NoMove, NoMove, NoMove };
-    float tap3Positions[] = { 77.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap4Positions[] = { 85.7f, NoMove, NoMove, NoMove, NoMove };
-    float tap5Positions[] = { 94.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap6Positions[] = { 102.7f, NoMove, NoMove, NoMove, NoMove };
-    float tap7Positions[] = { 111.2f, NoMove, NoMove, NoMove, NoMove };
-    float tap8Positions[] = { 119.7f, NoMove, NoMove, NoMove, NoMove };
+    float tap1Positions[] = { 60.0f, NoMove, NoMove, NoMove, NoMove };
+    float tap3Positions[] = { 78.0f, NoMove, NoMove, NoMove, NoMove };
+    float tap7Positions[] = { 112.0f, NoMove, NoMove, NoMove, NoMove };
     
     // define actions
     ActionSequence* moveStart = new ActionSequence(SA_SetGoal, NUMJOINTS, homePositions, 1500);
@@ -49,17 +44,12 @@
     ActionSequence* bigwaveDown = new ActionSequence(SA_SetGoal, partSize, bigwaveDownPositions, 1000);
     ActionSequence* tapsStart = new ActionSequence(SA_SetGoal, partSize, tapStartPositions, 1000);
     ActionSequence* taps1 = new ActionSequence(SA_SetGoal, partSize, tap1Positions, 300);
-    ActionSequence* taps2 = new ActionSequence(SA_SetGoal, partSize, tap2Positions, 300);
     ActionSequence* taps3 = new ActionSequence(SA_SetGoal, partSize, tap3Positions, 300);
-    ActionSequence* taps4 = new ActionSequence(SA_SetGoal, partSize, tap4Positions, 300);
-    ActionSequence* taps5 = new ActionSequence(SA_SetGoal, partSize, tap5Positions, 300);
-    ActionSequence* taps6 = new ActionSequence(SA_SetGoal, partSize, tap6Positions, 300);
     ActionSequence* taps7 = new ActionSequence(SA_SetGoal, partSize, tap7Positions, 300);
-    ActionSequence* taps8 = new ActionSequence(SA_SetGoal, partSize, tap8Positions, 300);
     ActionSequence* tapDown = new ActionSequence(SA_SetGoal, partSize, waveDownPositions, 500);
     ActionSequence* tapUp = new ActionSequence(SA_SetGoal, partSize, waveMiddlePositions, 500);
     
-    ActionSequence* pause2 = new ActionSequence(SA_Delay, 0 , NULL, 2000);
+    ActionSequence* loop10 = new ActionSequence(SA_LoopBegin, 0 , NULL, 10);  
     ActionSequence* loop20 = new ActionSequence(SA_LoopBegin, 0 , NULL, 20);  
     ActionSequence* loop2 = new ActionSequence(SA_LoopBegin, 0 , NULL, 2);  
     ActionSequence* loopEnd = new ActionSequence(SA_LoopEnd);
@@ -67,6 +57,7 @@
     ActionSequence* pause200ms = new ActionSequence(SA_Delay, 0 , NULL, 200);
     ActionSequence* pause500ms = new ActionSequence(SA_Delay, 0 , NULL, 500);
     ActionSequence* pause1 = new ActionSequence(SA_Delay, 0 , NULL, 1000);
+    ActionSequence* pause2 = new ActionSequence(SA_Delay, 0 , NULL, 2000);
     
     // add actions into StartSeq
     StartSeq.clear();
@@ -114,8 +105,9 @@
     // add actions to tap sequence
     TapsSeq.clear();
     TapsSeq.push_back(tapsStart);
+    TapsSeq.push_back(loop10);
     TapsSeq.push_back(pause1);
-    TapsSeq.push_back(taps3);
+    TapsSeq.push_back(taps1);
     TapsSeq.push_back(tapDown);
     TapsSeq.push_back(tapUp);
     TapsSeq.push_back(pause100ms);
@@ -123,25 +115,10 @@
     TapsSeq.push_back(tapDown);
     TapsSeq.push_back(tapUp);
     TapsSeq.push_back(pause200ms);
-    TapsSeq.push_back(taps1);
-    TapsSeq.push_back(tapDown);
-    TapsSeq.push_back(tapUp);
-    TapsSeq.push_back(pause100ms);
-    TapsSeq.push_back(taps5);
-    TapsSeq.push_back(tapDown);
-    TapsSeq.push_back(tapUp);
-    TapsSeq.push_back(pause100ms);
     TapsSeq.push_back(taps7);
     TapsSeq.push_back(tapDown);
     TapsSeq.push_back(tapUp);
-    TapsSeq.push_back(pause200ms);
-    TapsSeq.push_back(taps6);
-    TapsSeq.push_back(tapDown);
-    TapsSeq.push_back(tapUp);
-    TapsSeq.push_back(pause100ms);
-    TapsSeq.push_back(taps7);
-    TapsSeq.push_back(tapDown);
-    TapsSeq.push_back(tapUp);
+    TapsSeq.push_back(loopEnd);
     TapsSeq.push_back(moveUp);
 
     // add actions into BigWaveSeq

--- a/Utils/Alert.cpp	Sat Jan 23 00:20:27 2016 +0000
+++ b/Utils/Alert.cpp	Tue Jan 26 17:34:51 2016 +0000
@@ -10,9 +10,10 @@
 
 SafeCircBuf<Alert, AlertBufSize, uint32_t> AlertBuf;
 
-void Alert::SetAlert(time_t created, char* msg, char* atype)
+void Alert::SetAlert(time_t created, int createdMs, char* msg, char* atype)
 {
     Created = created;
+    CreatedMs = createdMs;
     
     if (strlen(atype) >= AlertTypeMaxLen)
     {
@@ -35,12 +36,13 @@
 }
 
 
-void Alert::SetPositionAlert(time_t created, int partIx, float diff)
+void Alert::SetPositionAlert(time_t created, int createdMs, int partIx, float diff)
 {
     char* msg = "Arm joint failed to move to desired position. Joint %d is off by %f";
     int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, diff);
 
     Created = created;
+    CreatedMs = createdMs;
     strcpy(MeasureName, "rot");
     Index = partIx;
     Value = diff;
@@ -48,12 +50,13 @@
     strcpy(AlertType, "Position");
 }
 
-void Alert::SetLoadAlert(time_t created, int partIx, float val)
+void Alert::SetLoadAlert(time_t created, int createdMs, int partIx, float val)
 {
     char* msg = "Arm joint reported a high load. Joint %d, load %f";
     int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, val);
 
     Created = created;
+    CreatedMs = createdMs;
     strcpy(MeasureName, "load");
     Index = partIx;
     Value = val;
@@ -61,12 +64,13 @@
     strcpy(AlertType, "Load");
 }
 
-void Alert::SetHardwareAlert(time_t created, int partIx, int code)
+void Alert::SetHardwareAlert(time_t created, int createdMs, int partIx, int code)
 {
     char* msg = "Arm joint reported an error. Joint %d error code %d";
     int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, code);
 
     Created = created;
+    CreatedMs = createdMs;
     strcpy(MeasureName, "error");
     Index = partIx;
     Value = (float)code;
@@ -74,12 +78,13 @@
     strcpy(AlertType, "Hardware");
 }
 
-void Alert::SetTemperatureAlert(time_t created, int partIx, float temp)
+void Alert::SetTemperatureAlert(time_t created, int createdMs, int partIx, float temp)
 {
     char* msg = "Arm joint reported a high temperature. Joint %d temperature %f";
     int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, temp);
 
     Created = created;
+    CreatedMs = createdMs;
     strcpy(MeasureName, "temp");
     Index = partIx;
     Value = temp;
@@ -87,12 +92,13 @@
     strcpy(AlertType, "Temperature");
 }
 
-void Alert::SetVoltageAlert(time_t created, int partIx, float val)
+void Alert::SetVoltageAlert(time_t created, int createdMs, int partIx, float val)
 {
     char* msg = "Arm joint reported an unexpected voltge. Joint %d volt %f";
     int slen = sprintf_s(Msg, AlertMsgMaxLen, msg, partIx, val);
 
     Created = created;
+    CreatedMs = createdMs;
     strcpy(MeasureName, "volt");
     Index = partIx;
     Value = val;

--- a/Utils/Alert.h	Sat Jan 23 00:20:27 2016 +0000
+++ b/Utils/Alert.h	Tue Jan 26 17:34:51 2016 +0000
@@ -17,17 +17,17 @@
 public:
     Alert() {};
     
-    void SetAlert(time_t created, char* msg, char* atype);
+    void SetAlert(time_t created, int createdMs, char* msg, char* atype);
     
-    void SetPositionAlert(time_t created, int partIx, float diff);
+    void SetPositionAlert(time_t created, int createdMs, int partIx, float diff);
     
-    void SetLoadAlert(time_t created, int partIx, float val);
+    void SetLoadAlert(time_t created, int createdMs, int partIx, float val);
     
-    void SetHardwareAlert(time_t created, int partIx, int code);
+    void SetHardwareAlert(time_t created, int createdMs, int partIx, int code);
     
-    void SetTemperatureAlert(time_t created, int partIx, float temp);
+    void SetTemperatureAlert(time_t created, int createdMs, int partIx, float temp);
     
-    void SetVoltageAlert(time_t created, int partIx, float val);
+    void SetVoltageAlert(time_t created, int createdMs, int partIx, float val);
     
     char AlertType[AlertTypeMaxLen];
     
@@ -40,6 +40,8 @@
     float Value;
     
     time_t Created;
+    
+    int CreatedMs;
 };
 
 extern SafeCircBuf<Alert, AlertBufSize, uint32_t> AlertBuf;

--- 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;
 

--- a/Utils/IothubSerial.cpp	Sat Jan 23 00:20:27 2016 +0000
+++ b/Utils/IothubSerial.cpp	Tue Jan 26 17:34:51 2016 +0000
@@ -60,18 +60,15 @@
     return startlen - bufsize;
 }
 
-int AddTime(time_t seconds, char* buf, int bufsize)
+int AddTime(time_t seconds, int ms, char* buf, int bufsize)
 {
-    if (bufsize > 32)
+    char tbuf[32];
+    strftime(tbuf, 32, "%FT%T", localtime(&seconds));
+    int slen = sprintf_s(buf, bufsize, "\"time\": \"%s.%03d\",", tbuf, ms);
+    if (slen > 0)
     {
-        int slen = strftime(buf, 32, "\"time\": \"%FT%T\",", localtime(&seconds));
-        if (slen > 0)
-        {
-            return slen;
-        }
-        else
-            return -1;
-    }        
+        return slen;
+    }
     else
         return -1;
 }
@@ -95,7 +92,7 @@
     else
         return -1;
 
-    slen = AddTime(msnap.Created, buf, bufsize);
+    slen = AddTime(msnap.Created, msnap.CreatedMs, buf, bufsize);
     if (slen > 0)
     {
         bufsize -= slen;

--- a/Utils/MeasureBuf.h	Sat Jan 23 00:20:27 2016 +0000
+++ b/Utils/MeasureBuf.h	Tue Jan 26 17:34:51 2016 +0000
@@ -40,6 +40,8 @@
     MeasureGroup Positions;
     
     time_t Created;
+    
+    int CreatedMs;
 };
 
 extern SafeCircBuf<MeasureSnapshot, MeasureBufSize, uint32_t> MeasureBuf;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Utils/Timestamp.h	Tue Jan 26 17:34:51 2016 +0000
@@ -0,0 +1,73 @@
+// Copyright (c) Microsoft. All rights reserved.
+// Licensed under the MIT license. See LICENSE file in the project root for full license information.
+
+#ifndef __TIMESTAMP_H__
+#define __TIMESTAMP_H__
+
+// use to timestamp alerts and measurements
+// with seconds and milliseconds
+// MS values are not real, but are intended to space out times properly
+class Timestamp
+{
+public:
+    Timestamp()
+    {
+        _timestamp.start();
+        _stampSecs = time(NULL);
+        int now = _timestamp.read_ms();
+        _stampMs = ((unsigned int)now) % 1000;
+        _zeroMs = _stampMs;
+    };
+    
+    // Take a timestamp and save secs and ms
+    // Note: We try to set ms to 0 for first measure, and then set ms 
+    // with the difference for subsequent values with same secs
+    void GetTimestamp()
+    {
+        time_t nowsecs = time(NULL);
+        int now = _timestamp.read_ms();
+
+        double diffsecs = difftime(nowsecs, _stampSecs);
+        
+        if (diffsecs > 0)
+        {
+            {
+                _zeroMs = now;
+            }
+        }
+        else
+        {
+            if (now - _zeroMs > 999)
+            {
+                _zeroMs = now - 999;
+            }
+            if (now - _zeroMs < _stampMs)
+            {
+                _zeroMs = now - _stampMs;
+            }
+        }
+        // keep the ms difference for timestamp
+        _stampMs = ((unsigned int)(now - _zeroMs)) % 1000;
+        _stampSecs = nowsecs;
+    };
+
+    time_t GetSecs()
+    {
+        return _stampSecs;
+    };
+    
+    int GetMs()
+    {
+        return _stampMs;
+    };
+    
+private:
+
+    Timer _timestamp;
+
+    time_t _stampSecs;
+    unsigned int _stampMs;
+    int _zeroMs;
+};
+
+#endif