Diff: servo/servo.cpp

Revision:

13:126c3f7f9b89

Parent:

12:174f0090aa79

Child:

16:b2358fd35999

diff -r 174f0090aa79 -r 126c3f7f9b89 servo/servo.cpp
--- a/servo/servo.cpp	Mon Aug 13 02:22:58 2018 +0000
+++ b/servo/servo.cpp	Mon Aug 13 09:03:30 2018 +0000
@@ -15,16 +15,18 @@
 
 
 const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
-const int kServoSpan_ms = 5; //指示の前後に必要なwait
+const int kServoSpan_ms = 2; //指示の前後に必要なwait
 
 void Open();
 void Move(double x, double y, double theta);
-
+void ServoRad(int i, double rad){
+    servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]);
+}
 void ServoMoveOnArm()
 {
     for(int i = 0; i < kServoNum; i++) {
         double rad = GetNextRadRelative(i);
-        servo.set_degree(i, kServoSign[i] * rad *kRadToDegree + kOriginTheta[i]);
+        ServoRad(i, rad);
         SetNowRadRelative(i,rad);
         wait_ms(kServoSpan_ms);
     }