From 837d38ad985b991e2400279ddc892cbc7e01721a Mon Sep 17 00:00:00 2001
From: Jairo <jairoscz@gmail.com>
Date: Sun, 1 Jan 2012 19:59:10 -0600
Subject: [PATCH] added manual servo control function

---
 main.cpp      |  4 ++++
 yaw-servo.cpp | 43 +++++++++++++++++++++++++++++++++++++++++++
 yaw-servo.h   |  2 +-
 3 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/main.cpp b/main.cpp
index 58b7d17..4773bf5 100644
--- a/main.cpp
+++ b/main.cpp
@@ -100,6 +100,10 @@ int main(void) {
                 esc_manual_control();
                 break;
 
+            case 'y':
+            	yaw_manual_control();
+            	break;
+
             case 'b':
                 cmd_board_info();
                 break;
diff --git a/yaw-servo.cpp b/yaw-servo.cpp
index a6ea9ef..1f911f0 100644
--- a/yaw-servo.cpp
+++ b/yaw-servo.cpp
@@ -24,3 +24,46 @@ void set_servo_angle(float angle)
     if(duty > SERVO_MAX) duty = SERVO_MAX;
     pwmWrite(YAW_SERVO_PIN, duty);
 }
+
+void yaw_manual_control()
+{
+	SerialUSB.println("Press \'j\' to lower speed.");
+	SerialUSB.println("Press \'k\' to increase speed.");
+	SerialUSB.println("Press \'z\' to zero command.");
+	SerialUSB.println("Any other key zeroes command and exits");
+	SerialUSB.println();
+
+	uint8 input;
+	float angle = 0;
+	while (1) {
+
+		input = SerialUSB.read();
+		if (input == 'j')
+		{
+			angle -= 1;
+		}
+		else if(input == 'k')
+		{
+			angle += 1;
+		}
+		else if(input == 'z')
+		{
+			angle = 0;
+
+		}else{
+			break;
+		}
+
+
+		if (angle > 90)  angle = 90;
+
+		SerialUSB.println(angle);
+
+		set_servo_angle(angle);
+		delay(20);
+	}
+
+	set_servo_angle(0.0);
+
+
+}
diff --git a/yaw-servo.h b/yaw-servo.h
index f05dd5c..d2c4d7b 100644
--- a/yaw-servo.h
+++ b/yaw-servo.h
@@ -10,7 +10,7 @@
 
 void yaw_servo_init();
 void set_servo_angle(float angle);
-
+void yaw_manual_control();
 
 
 // Servo constants