wg0x_test.cpp
Go to the documentation of this file.
00001 #include "ethercat_hardware/wg0x.h"
00002 #include <gtest/gtest.h>
00003 #include <iostream>
00004 #include <math.h>
00005 
00010 TEST(WG0X, timestampDiff)
00011 {
00012   int32_t diff=1000;
00013   uint32_t a,b;
00014 
00015   a = 0x1;
00016   b = a + diff;
00017   EXPECT_EQ(WG0X::timestampDiff(b, a), diff);
00018   EXPECT_EQ(WG0X::timestampDiff(a, b), -diff);
00019 
00020   a = 0xFFFFFF00;
00021   b = a + diff;
00022   EXPECT_EQ(WG0X::timestampDiff(b, a), diff);
00023   EXPECT_EQ(WG0X::timestampDiff(a, b), -diff);
00024 }
00025 
00026 
00031 TEST(WG0X, positionDiff)
00032 {
00033   int32_t diff=1000;
00034   int32_t a,b;
00035 
00036   a = 0x1;
00037   b = a + diff;
00038   EXPECT_EQ(WG0X::positionDiff(b, a), diff);
00039   EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
00040 
00041   a = 0x7FFFFFF0;
00042   b = a + diff;
00043   EXPECT_EQ(WG0X::positionDiff(b, a), diff);
00044   EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
00045 
00046   a = 0x800000F;
00047   b = a + diff;
00048   EXPECT_EQ(WG0X::positionDiff(b, a), diff);
00049   EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
00050 
00051   a = 0x800000F;
00052   b = a + diff;
00053   EXPECT_EQ(WG0X::positionDiff(b, a), diff);
00054   EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
00055 }
00056 
00057 
00061 TEST(WG0X, timediffToDuration)
00062 {
00063   // 1 microsecond ==>  (0 second + 1000 nanoseconds)
00064   EXPECT_EQ(WG0X::timediffToDuration(1), ros::Duration(0, 1000));
00065 
00066   // -1 microsecond ==> - ( 0 second + 1000 nanoseconds )
00067   EXPECT_EQ(WG0X::timediffToDuration(-1), ros::Duration(0, -1000));
00068 
00069   // 1,000,000 microseconds ==> ( 1 second + 0 nanoseconds )
00070   EXPECT_EQ(WG0X::timediffToDuration(1000000), ros::Duration(1, 0));
00071 
00072   // - 1,000,000 microseconds ==> - ( 1 second + 0 nanoseconds )
00073   EXPECT_EQ(WG0X::timediffToDuration(-1000000), ros::Duration(-1, 0));
00074 
00075   // 1,000,001 microseconds ==> ( 1 second + 1 nanoseconds )
00076   EXPECT_EQ(WG0X::timediffToDuration(1000001), ros::Duration(1, 1000));
00077 
00078   // - 1,000,001 microseconds ==> - ( 1 second + 1 nanoseconds )
00079   EXPECT_EQ(WG0X::timediffToDuration(-1000001), ros::Duration(-1, -1000));
00080 }
00081 
00082 
00086 TEST(WG0X, calcEncoderVelocity)
00087 {
00088   int timediff; 
00089   int positiondiff;
00090   uint32_t t1,t2;
00091   int32_t p1,p2;
00092 
00093   // A position change of zero should always provide a velocity of zero (except with timechange is zero)
00094   t1=0; t2=1;
00095   p1=0; p2=0;
00096   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  0.0);
00097   
00098   // A position change of 1 tick over 1 second
00099   t1=0; t2=1000000;
00100   p1=0; p2=1;
00101   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  1.0);
00102   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1),  -1.0);
00103   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2),  -1.0);
00104   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2),  1.0);
00105 
00106   // A position change of 1 tick over 1 second with different starting times
00107   // Velocity should equal 1 ticks/second
00108   p1=0; p2=1;
00109   timediff = 1000000;
00110 
00111   t1=0; 
00112   t2=t1+timediff;
00113   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  1.0);
00114   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1),  -1.0);
00115   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2),  -1.0);
00116   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2),  1.0);
00117 
00118   t1=0xFFFFfff0; 
00119   t2=t1+timediff;
00120   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  1.0);
00121   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1),  -1.0);
00122   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2),  -1.0);
00123   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2),  1.0);
00124 
00125   t1=0x7FFFfff0; 
00126   t2=t1+timediff;
00127   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  1.0);
00128   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1),  -1.0);
00129   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2),  -1.0);
00130   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2),  1.0);
00131 
00132 
00133   // A position change of 1000 ticks over 1 second with different starting positions
00134   // Velocity should equal 1000 ticks/second
00135   positiondiff = 1000;
00136   t1=0; t2=1000000;
00137 
00138   p1=0; 
00139   p2=p1+positiondiff;
00140   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  1000.0);
00141   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1),  -1000.0);
00142   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2),  -1000.0);
00143   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2),  1000.0);
00144 
00145   p1=0x7FFFFFF0; 
00146   p2=p1+positiondiff;
00147   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  1000.0);
00148   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1),  -1000.0);
00149   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2),  -1000.0);
00150   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2),  1000.0);
00151 
00152   p1=0x8000000F;
00153   p2=p1+positiondiff;
00154   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1),  1000.0);
00155   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1),  -1000.0);
00156   EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2),  -1000.0);
00157   EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2),  1000.0);
00158 }
00159 
00160 
00161 
00162 // Run all the tests that were declared with TEST()
00163 int main(int argc, char **argv)
00164 {
00165   testing::InitGoogleTest(&argc, argv);
00166   return RUN_ALL_TESTS();
00167 }


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Jun 6 2019 19:46:32