Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
Diff: syncTime.cpp
- Revision:
- 5:01e1e68309ae
diff -r 778bc352c47f -r 01e1e68309ae syncTime.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/syncTime.cpp Mon Oct 15 18:30:20 2018 +0000 @@ -0,0 +1,64 @@ +#include "syncTime.h" +extern BufferedSerial buffered_pc; +SyncTime::SyncTime( int seconds, int nSeconds ){ + refTime.seconds = seconds; + refTime.nSeconds = nSeconds; + sinceRefTimer.reset(); + sinceRefTimer.start(); + +} + const int million = 1000000; + const int billion = 1000000000; + +rosTime SyncTime::getTime(){ + rosTime ret; + uint64_t uSecs = sinceRefTimer.read_high_resolution_us(); + ret.seconds = (int)(uSecs/million); + ret.nSeconds = (int)((uSecs% million)*1000); + ret.seconds += refTime.seconds; + ret.nSeconds += refTime.nSeconds; + if(ret.nSeconds >= (int)1e9){ + ret.seconds ++; + ret.nSeconds -= (int)1e9; + } + if(ret.nSeconds < 0){ + ret.seconds --; + ret.nSeconds += (int)1e9; + } + return ret; +} + +void SyncTime::resetOffsetCounter(){ + rosTime update = getTime(); + refTime = update; + sinceRefTimer.reset(); + //buffered_pc.printf("resetting counter %dus\r\n", sinceRefTimer.read_high_resolution_us()); + +} + +void SyncTime::hardReset(int seconds, int nSeconds){ + refTime.seconds = seconds; + refTime.nSeconds = nSeconds; + sinceRefTimer.reset(); + sinceRefTimer.start(); + } + + +void SyncTime::updateTime(float correction){ + int seconds = (int)correction; + int nSeconds = (correction - (float)seconds) * 1e9; +// buffered_pc.printf("time before %ds %dns \n\r", refTime.seconds, refTime.nSeconds); +// buffered_pc.printf("updating by %ds %dns \n\r", seconds, nSeconds); + refTime.seconds += seconds; + refTime.nSeconds += nSeconds; + if(refTime.nSeconds >= billion){ + refTime.seconds ++; + refTime.nSeconds -= billion; + } + if(refTime.nSeconds < 0){ + refTime.seconds --; + refTime.nSeconds += billion; + } + //buffered_pc.printf("time after %ds %dns \n\r", refTime.seconds, refTime.nSeconds); + resetOffsetCounter(); +} \ No newline at end of file