Diff: servo.cpp

Revision:

1:c8ecfd809e22

Parent:

0:655e1fcb96be

Child:

2:442fae3b1c0e

Child:

3:c112df463a8d

--- a/servo.cpp	Wed Sep 07 07:35:46 2016 +0000
+++ b/servo.cpp	Wed Sep 07 09:40:58 2016 +0000
@@ -2,9 +2,43 @@
 
 PwmOut pwmarm(PC_6);
 PwmOut pwmhand(PC_8);
-
+PwmOut pwmbelt(PC_9);
 float PERIOD=20000;
 
+void armdegree(int degree) {
+    int i;
+
+   pwmarm.period_ms(20);        //20ms
+   
+       degree=10;
+
+       i=500+degree*1900/180;
+       pwmarm.write(i/PERIOD); 
+}
+
+void handdegree(int degree) {
+    int i;
+
+   pwmarm.period_ms(20);        //20ms
+   
+       degree=10;
+
+       i=500+degree*1900/180;
+       pwmhand.write(i/PERIOD); 
+       
+}
+
+void beltdegree(int degree) {
+    int i;
+
+   pwmarm.period_ms(20);        //20ms
+   
+       degree=10;
+
+       i=500+degree*1900/180;
+       pwmbelt.write(i/PERIOD); 
+       
+}
 void close_hand(void) {
      int i,degree;