/firmware/tests/T4-Commands/src/T4-Commands-exerciser.cc

  1/*
  2 * T1-UART-exerciser.cc
  3 *
  4 *  Created on: Dec 10, 2009
  5 *      Author: phooky
  6 */
  7
  8/// Based on the hoary old serial-programming HOWTO
  9
 10#include <termios.h>
 11#include <stdio.h>
 12#include <unistd.h>
 13#include <fcntl.h>
 14#include <sys/signal.h>
 15#include <sys/types.h>
 16#include <stdlib.h>
 17#include <getopt.h>
 18#include <sys/time.h>
 19#include <gtest/gtest.h>
 20#include <math.h>
 21
 22#include "Packet.hh"
 23#include "Commands.hh"
 24
 25#define COMMAND_BUFFER_SIZE 256
 26
 27const char* default_port = "/dev/ttyUSB0";
 28#define BAUDRATE B38400
 29#define _POSIX_SOURCE 1 /* POSIX compliant source */
 30
 31class SerialTest: public ::testing::Test {
 32protected:
 33	const char* port_name_;
 34	int serial_fd_;
 35	struct termios oldtio_;
 36	InPacket in_;
 37	OutPacket out_;
 38	uint16_t sequence_number_;
 39public:
 40	SerialTest() :
 41		sequence_number_(0) {
 42	}
 43	void SetUp() {
 44		port_name_ = getenv("PORT");
 45		if (port_name_ == NULL) {
 46			port_name_ = default_port;
 47		}
 48		struct termios newtio;
 49		char buf[255];
 50
 51		/* open the device to be non-blocking (read will return immediately) */
 52		serial_fd_ = open(port_name_, O_RDWR | O_NOCTTY | O_NONBLOCK);
 53		if (serial_fd_ < 0) {
 54			perror(port_name_);
 55			return;
 56		}
 57
 58		tcgetattr(serial_fd_, &oldtio_); /* save current port settings */
 59		/* set new port settings for canonical input processing */
 60		newtio.c_cflag = BAUDRATE | CS8 | CLOCAL | CREAD;
 61
 62		newtio.c_cflag &= ~PARENB;
 63		newtio.c_cflag &= ~CSTOPB;
 64		newtio.c_cflag &= ~CSIZE;
 65		newtio.c_cflag |= CS8;
 66
 67		newtio.c_iflag = IGNPAR;
 68		newtio.c_oflag = 0;
 69		newtio.c_lflag = 0;
 70		newtio.c_cc[VMIN] = 1;
 71		newtio.c_cc[VTIME] = 0;
 72		tcflush(serial_fd_, TCIFLUSH);
 73		tcsetattr(serial_fd_, TCSANOW, &newtio);
 74	}
 75	void TearDown() {
 76		/* restore old port settings */
 77		tcsetattr(serial_fd_, TCSANOW, &oldtio_);
 78		close(serial_fd_);
 79	}
 80
 81	void writePacket();
 82	void readPacketWithTimeout(uint16_t timeout);
 83
 84	void runPacket();
 85	void reset();
 86
 87	void resetPosition();
 88	void moveTo(int x, int y, int z, int dda);
 89
 90	void delay(float seconds);
 91
 92	void waitUntilDone();
 93};
 94
 95void SerialTest::moveTo(int x, int y, int z, int dda) {
 96	bool done = false;
 97	while (!done) {
 98		reset();
 99		out_.append8(HOST_CMD_QUEUE_POINT_ABS);
100		out_.append32(x);
101		out_.append32(y);
102		out_.append32(z);
103		out_.append32(dda);
104		writePacket();
105		readPacketWithTimeout(50);
106		ASSERT_TRUE(in_.isFinished());
107		ASSERT_FALSE(in_.hasError());
108		if (in_.read8(0)== RC_OK) {
109			done=true;
110		} else {
111			ASSERT_EQ(in_.read8(0),RC_BUFFER_OVERFLOW);
112			// delay for 1ms
113			struct timespec t;
114			t.tv_sec = 0;
115			t.tv_nsec = 10 * 1000 * 1000;
116			nanosleep(&t,NULL);
117		}
118	}
119}
120
121void SerialTest::delay(float seconds) {
122	bool done = false;
123	int32_t milliseconds = (int32_t) (seconds * 1000.0);
124	while (!done) {
125		reset();
126		out_.append8(HOST_CMD_DELAY);
127		out_.append32(milliseconds);
128		writePacket();
129		readPacketWithTimeout(50);
130		ASSERT_TRUE(in_.isFinished());
131		ASSERT_FALSE(in_.hasError());
132		if (in_.read8(0)== RC_OK) {
133			done=true;
134		} else {
135			ASSERT_EQ(in_.read8(0),RC_BUFFER_OVERFLOW);
136			// delay for 1ms
137			struct timespec t;
138			t.tv_sec = 0;
139			t.tv_nsec = 10 * 1000 * 1000;
140			nanosleep(&t,NULL);
141		}
142	}
143}
144
145void SerialTest::resetPosition() {
146	reset();
147	out_.append8(HOST_CMD_SET_POSITION);
148	out_.append32(0);
149	out_.append32(0);
150	out_.append32(0);
151	runPacket();
152}
153
154const char* port_string = default_port;
155
156class Timer {
157private:
158	struct timeval start_;
159public:
160	Timer() {
161		gettimeofday(&start_, NULL);
162	}
163	long millisSince() {
164		struct timeval now;
165		gettimeofday(&now, NULL);
166		return ((now.tv_sec - start_.tv_sec) * 1000L) + (now.tv_usec
167				- start_.tv_usec) / 1000L;
168	}
169};
170
171void SerialTest::writePacket() {
172	while (!out_.isFinished()) {
173		uint8_t out_byte = out_.getNextByteToSend();
174		write(serial_fd_, &out_byte, 1);
175	}
176}
177
178void SerialTest::readPacketWithTimeout(uint16_t timeout) {
179	Timer mark;
180	while (!in_.isFinished()) {
181		if (mark.millisSince() > timeout) {
182			in_.timeout();
183			printf("state is %d\n",in_.debugGetState());
184			break;
185		}
186		uint8_t in_byte;
187		int count = read(serial_fd_, &in_byte, 1);
188		if (count == 1) {
189			in_.processByte(in_byte);
190		}
191	}
192}
193
194void SerialTest::runPacket() {
195	writePacket();
196	readPacketWithTimeout(50);
197	ASSERT_TRUE(in_.isFinished());
198	ASSERT_FALSE(in_.hasError());
199	ASSERT_EQ(in_.read8(0), RC_OK);
200}
201
202void SerialTest::reset() {
203	in_.reset();
204	out_.reset();
205}
206
207TEST_F(SerialTest,CheckVersion)
208{
209	reset();
210	out_.append8(HOST_CMD_VERSION);
211	runPacket();
212	/// Version should be >=200, <300
213	ASSERT_GE(in_.read8(1), 200);
214	ASSERT_LT(in_.read8(1), 300);
215}
216/*
217TEST_F(SerialTest,Init)
218{
219	reset();
220	out_.append8(HOST_CMD_INIT);
221	runPacket();
222	/// No response
223}
224
225TEST_F(SerialTest,IsFinished)
226{
227	SCOPED_TRACE("Initial finished");
228	reset();
229	out_.append8(HOST_CMD_IS_FINISHED);
230	runPacket();
231	/// Should respond with a 1 (yes, we're done)
232	ASSERT_EQ(in_.read8(1), 1);
233	/// Start a move
234	SCOPED_TRACE("Adding moves");
235	resetPosition();
236	moveTo(500,500,0,500);
237	moveTo(0,0,0,500);
238	/// Should be busy
239	reset();
240	out_.append8(HOST_CMD_IS_FINISHED);
241	runPacket();
242	/// Should respond with a 0 (no, not done)
243	ASSERT_EQ(in_.read8(1), 0);
244	/// Wait until done
245	bool done = false;
246	while (!done) {
247		reset();
248		out_.append8(HOST_CMD_IS_FINISHED);
249		runPacket();
250		done = in_.read8(1) == 1;
251	}
252}
253*/
254/// Wait until done
255void SerialTest::waitUntilDone() {
256	bool done = false;
257	while (!done) {
258		reset();
259		out_.append8(HOST_CMD_IS_FINISHED);
260		runPacket();
261		done = in_.read8(1) == 1;
262	}
263}
264
265TEST_F(SerialTest,Abort)
266{
267	reset();
268	out_.append8(HOST_CMD_IS_FINISHED);
269	runPacket();
270	/// Should respond with a 1 (yes, we're done)
271	ASSERT_EQ(in_.read8(1), 1);
272	/// Start a move
273	resetPosition();
274	moveTo(500,500,0,2000);
275	moveTo(500,0,0,2000);
276	/// Should be busy
277	reset();
278	out_.append8(HOST_CMD_IS_FINISHED);
279	runPacket();
280	/// Should respond with a 0 (no, not done)
281	ASSERT_EQ(in_.read8(1), 0);
282
283	/// Abort moves
284	//sleep(1);
285	reset();
286	out_.append8(HOST_CMD_ABORT);
287	runPacket();
288
289	reset();
290	out_.append8(HOST_CMD_IS_FINISHED);
291	runPacket();
292	/// Should respond with a 1 (yes, we're done)
293	ASSERT_EQ(in_.read8(1), 1);
294
295	/// Return to zero point
296	moveTo(0,0,0,2000);
297	waitUntilDone();
298
299}
300
301TEST_F(SerialTest,NoSuchCommand)
302{
303	reset();
304	out_.append8(0x75);
305	writePacket();
306	readPacketWithTimeout(50);
307	ASSERT_TRUE(in_.isFinished());
308	ASSERT_FALSE(in_.hasError());
309	ASSERT_EQ(in_.read8(0), RC_CMD_UNSUPPORTED);
310}
311
312TEST_F(SerialTest,PauseUnpause)
313{
314	reset();
315	waitUntilDone();
316	reset();
317	/// Start a move
318	resetPosition();
319	moveTo(0, 750, 0, 1000);
320	moveTo(0, 0, 0, 1000);
321	/// Pause after 1s
322	sleep(1);
323	reset();
324	out_.append8(HOST_CMD_PAUSE);
325	runPacket();
326	/// Wait 1s, unpause
327	sleep(1);
328	reset();
329	out_.append8(HOST_CMD_PAUSE);
330	runPacket();
331
332	waitUntilDone();
333}
334
335TEST_F(SerialTest,Delay)
336{
337	reset();
338	waitUntilDone();
339	reset();
340	/// Start a move
341	resetPosition();
342	moveTo(500, 0, 0, 1000);
343	delay(1.5); // delay for 1.5 seconds
344	moveTo(0, 0, 0, 1000);
345	waitUntilDone();
346}
347
348TEST_F(SerialTest,Circle)
349{
350	reset();
351	waitUntilDone();
352	reset();
353	/// Start a move
354	resetPosition();
355	const float radius = 400.0;
356	const int speed = 700;
357	moveTo(0, radius, 0, speed);
358	for (int i = 0; i < 400; i++) {
359		double theta = i * M_PI / 50.0;
360		moveTo(radius*sin(theta), radius*cos(theta),0,speed);
361	}
362	moveTo(0,0,0,speed);
363	waitUntilDone();
364}
365
366TEST_F(SerialTest,Eeprom)
367{
368	// write 0 1 2 3 4 5 6
369	const int pattern_len = 7;
370	reset();
371	out_.append8(HOST_CMD_WRITE_EEPROM);
372	out_.append16(10);
373	out_.append8(pattern_len);
374	for (int i = 0; i < pattern_len; i++) {
375		out_.append8(i);
376	}
377	runPacket();
378
379	// read back and check
380	reset();
381	out_.append8(HOST_CMD_READ_EEPROM);
382	out_.append16(10);
383	out_.append8(pattern_len);
384	runPacket();
385	for (int i = 0; i < pattern_len; i++) {
386		ASSERT_EQ(in_.read8(i+1), i);
387	}
388
389	// replace 2nd value with 0xff
390	reset();
391	out_.append8(HOST_CMD_WRITE_EEPROM);
392	out_.append16(12);
393	out_.append8(1);
394	out_.append8(0xff);
395	runPacket();
396
397	// read back and check
398	reset();
399	out_.append8(HOST_CMD_READ_EEPROM);
400	out_.append16(10);
401	out_.append8(pattern_len);
402	runPacket();
403	for (int i = 0; i < pattern_len; i++) {
404		if ( i != 2 ) {
405			ASSERT_EQ(in_.read8(i+1), i);
406		} else {
407			ASSERT_EQ(in_.read8(i+1), 255);
408		}
409	}
410}
411
412TEST_F(SerialTest,Reset)
413{
414  // Reset device
415  reset();
416  out_.append8(HOST_CMD_RESET);
417  runPacket();
418}