added manual servo control function

0xjairo · Jan 2, 2012 · 837d38a · 837d38a
1 parent 5f4cc05
commit 837d38a
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 1 deletion.
diff --git 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
@@ -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
@@ -10,7 +10,7 @@
 
 void yaw_servo_init();
 void set_servo_angle(float angle);
-
+void yaw_manual_control();
 
 
 // Servo constants