From f3bfbd87b1f6faef6bac75c9f94b590bb8b504b6 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Fri, 7 Jun 2013 14:02:18 -0400
Subject: [PATCH] Added sine test.

---
 src/drivers/md25/md25.cpp      | 38 ++++++++++++++++++++++++++++++++++
 src/drivers/md25/md25.hpp      |  3 +++
 src/drivers/md25/md25_main.cpp | 18 ++++++++++++++++
 3 files changed, 59 insertions(+)

diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index 71932ad65d..f9f5d70ab5 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -45,6 +45,7 @@
 #include "md25.hpp"
 #include <poll.h>
 #include <stdio.h>
+#include <math.h>
 
 #include <systemlib/err.h>
 #include <arch/board/board.h>
@@ -550,4 +551,41 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	return 0;
 }
 
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address)
+{
+	printf("md25 sine: starting\n");
+
+	// setup
+	MD25 md25("/dev/md25", bus, address);
+
+	// print status
+	char buf[200];
+	md25.status(buf, sizeof(buf));
+	printf("%s\n", buf);
+
+	// setup for test
+	md25.setSpeedRegulation(true);
+	md25.setTimeout(true);
+	float dt = 0.1;
+	float amplitude = 0.2;
+	float t = 0;
+	float omega = 0.1;
+
+	// sine wave for motor 1
+	md25.resetEncoders();
+	while (true) {
+		float prev_revolution = md25.getRevolutions1();
+		md25.setMotor1Speed(amplitude*sinf(omega*t));
+		usleep(1000000 * dt);
+		t += dt;
+		float speed_rpm = 60*(md25.getRevolutions1() - prev_revolution)/dt;
+		md25.readData();
+		if (t > 2.0f) break;
+	}
+	md25.setMotor1Speed(0);
+
+	printf("md25 sine complete\n");
+	return 0;
+}
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
index e77511b163..cac3ffd293 100644
--- a/src/drivers/md25/md25.hpp
+++ b/src/drivers/md25/md25.hpp
@@ -290,4 +290,7 @@ private:
 // unit testing
 int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
 
+// sine testing
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address);
+
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index e62c46b0d7..701452f2d5 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -136,6 +136,24 @@ int md25_main(int argc, char *argv[])
 		exit(0);
 	}
 
+	if (!strcmp(argv[1], "sine")) {
+
+		if (argc < 4) {
+			printf("usage: md25 sine bus address\n");
+			exit(0);
+		}
+
+		const char *deviceName = "/dev/md25";
+
+		uint8_t bus = strtoul(argv[2], nullptr, 0);
+
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		md25Sine(deviceName, bus, address);
+
+		exit(0);
+	}
+
 	if (!strcmp(argv[1], "probe")) {
 		if (argc < 4) {
 			printf("usage: md25 probe bus address\n");