wg0x_test.cpp
Go to the documentation of this file.
2 #include <gtest/gtest.h>
3 #include <iostream>
4 #include <math.h>
5 
10 TEST(WG0X, timestampDiff)
11 {
12  int32_t diff=1000;
13  uint32_t a,b;
14 
15  a = 0x1;
16  b = a + diff;
17  EXPECT_EQ(WG0X::timestampDiff(b, a), diff);
18  EXPECT_EQ(WG0X::timestampDiff(a, b), -diff);
19 
20  a = 0xFFFFFF00;
21  b = a + diff;
22  EXPECT_EQ(WG0X::timestampDiff(b, a), diff);
23  EXPECT_EQ(WG0X::timestampDiff(a, b), -diff);
24 }
25 
26 
31 TEST(WG0X, positionDiff)
32 {
33  int32_t diff=1000;
34  int32_t a,b;
35 
36  a = 0x1;
37  b = a + diff;
38  EXPECT_EQ(WG0X::positionDiff(b, a), diff);
39  EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
40 
41  a = 0x7FFFFFF0;
42  b = a + diff;
43  EXPECT_EQ(WG0X::positionDiff(b, a), diff);
44  EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
45 
46  a = 0x800000F;
47  b = a + diff;
48  EXPECT_EQ(WG0X::positionDiff(b, a), diff);
49  EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
50 
51  a = 0x800000F;
52  b = a + diff;
53  EXPECT_EQ(WG0X::positionDiff(b, a), diff);
54  EXPECT_EQ(WG0X::positionDiff(a, b), -diff);
55 }
56 
57 
61 TEST(WG0X, timediffToDuration)
62 {
63  // 1 microsecond ==> (0 second + 1000 nanoseconds)
64  EXPECT_EQ(WG0X::timediffToDuration(1), ros::Duration(0, 1000));
65 
66  // -1 microsecond ==> - ( 0 second + 1000 nanoseconds )
67  EXPECT_EQ(WG0X::timediffToDuration(-1), ros::Duration(0, -1000));
68 
69  // 1,000,000 microseconds ==> ( 1 second + 0 nanoseconds )
70  EXPECT_EQ(WG0X::timediffToDuration(1000000), ros::Duration(1, 0));
71 
72  // - 1,000,000 microseconds ==> - ( 1 second + 0 nanoseconds )
73  EXPECT_EQ(WG0X::timediffToDuration(-1000000), ros::Duration(-1, 0));
74 
75  // 1,000,001 microseconds ==> ( 1 second + 1 nanoseconds )
76  EXPECT_EQ(WG0X::timediffToDuration(1000001), ros::Duration(1, 1000));
77 
78  // - 1,000,001 microseconds ==> - ( 1 second + 1 nanoseconds )
79  EXPECT_EQ(WG0X::timediffToDuration(-1000001), ros::Duration(-1, -1000));
80 }
81 
82 
86 TEST(WG0X, calcEncoderVelocity)
87 {
88  int timediff;
89  int positiondiff;
90  uint32_t t1,t2;
91  int32_t p1,p2;
92 
93  // A position change of zero should always provide a velocity of zero (except with timechange is zero)
94  t1=0; t2=1;
95  p1=0; p2=0;
96  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 0.0);
97 
98  // A position change of 1 tick over 1 second
99  t1=0; t2=1000000;
100  p1=0; p2=1;
101  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 1.0);
102  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1), -1.0);
103  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2), -1.0);
104  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2), 1.0);
105 
106  // A position change of 1 tick over 1 second with different starting times
107  // Velocity should equal 1 ticks/second
108  p1=0; p2=1;
109  timediff = 1000000;
110 
111  t1=0;
112  t2=t1+timediff;
113  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 1.0);
114  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1), -1.0);
115  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2), -1.0);
116  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2), 1.0);
117 
118  t1=0xFFFFfff0;
119  t2=t1+timediff;
120  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 1.0);
121  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1), -1.0);
122  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2), -1.0);
123  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2), 1.0);
124 
125  t1=0x7FFFfff0;
126  t2=t1+timediff;
127  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 1.0);
128  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1), -1.0);
129  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2), -1.0);
130  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2), 1.0);
131 
132 
133  // A position change of 1000 ticks over 1 second with different starting positions
134  // Velocity should equal 1000 ticks/second
135  positiondiff = 1000;
136  t1=0; t2=1000000;
137 
138  p1=0;
139  p2=p1+positiondiff;
140  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 1000.0);
141  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1), -1000.0);
142  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2), -1000.0);
143  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2), 1000.0);
144 
145  p1=0x7FFFFFF0;
146  p2=p1+positiondiff;
147  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 1000.0);
148  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1), -1000.0);
149  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2), -1000.0);
150  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2), 1000.0);
151 
152  p1=0x8000000F;
153  p2=p1+positiondiff;
154  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t2, p1, t1), 1000.0);
155  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t2, p2, t1), -1000.0);
156  EXPECT_EQ( WG0X::calcEncoderVelocity(p2, t1, p1, t2), -1000.0);
157  EXPECT_EQ( WG0X::calcEncoderVelocity(p1, t1, p2, t2), 1000.0);
158 }
159 
160 
161 
162 // Run all the tests that were declared with TEST()
163 int main(int argc, char **argv)
164 {
165  testing::InitGoogleTest(&argc, argv);
166  return RUN_ALL_TESTS();
167 }
int main(int argc, char **argv)
Definition: wg0x_test.cpp:163
static ros::Duration timediffToDuration(int32_t timediff_usec)
Definition: wg0x.cpp:696
static double calcEncoderVelocity(int32_t new_position, uint32_t new_timestamp, int32_t old_position, uint32_t old_timestamp)
Definition: wg0x.cpp:721
static int32_t positionDiff(int32_t new_position, int32_t old_position)
Definition: wg0x.cpp:710
static int32_t timestampDiff(uint32_t new_timestamp, uint32_t old_timestamp)
Definition: wg0x.cpp:685
Definition: wg0x.h:234
TEST(WG0X, timestampDiff)
Definition: wg0x_test.cpp:10


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Mar 4 2021 03:10:21