Diff: servo/servo.cpp

Revision:

23:f45eb02433a5

Parent:

21:af9a33d69745

Child:

24:b4a89db4ed72

--- a/servo/servo.cpp	Tue Aug 28 03:09:39 2018 +0000
+++ b/servo/servo.cpp	Sun Sep 02 07:53:06 2018 +0000
@@ -4,13 +4,13 @@
 #include "pinnames.h"
 #include "coordinate.h"
 #include "KondoServo.h"
-
+const double kServoValToDegree =  270.0 / (11500 - 3500);
 //////////////////////////////変更できるパラメータ
 const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
-const double kOriginTheta[] = {
-    180,
-    0,
-    135,
+const double kOriginDegree[] = {
+    (8850 - 3500)  * kServoValToDegree,
+    (11400 - 3500)  * kServoValToDegree,
+    (7500 - 3500) * kServoValToDegree,
     205.875,
 };//初期状態の角度
 const int kServoSpan_ms = 3; //指示の前後に必要なwait
@@ -22,9 +22,11 @@
                       KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),//サーボID:0,
                       KondoServo(pin_serial_servo_tx[2], pin_serial_servo_rx[2]),//サーボID:1,
                      };
+double old_rad[SERVONUM] = {};
+
 void ServoRad(int id, double rad_relative)
 {
-    double degree = kServoSign[id] * rad_relative * kRadToDegree + kOriginTheta[id];
+    double degree = kServoSign[id] * rad_relative * kRadToDegree + kOriginDegree[id];
     switch(id) {
         case 0:
             servo[1].set_degree(id, degree);
@@ -38,7 +40,8 @@
             servo[0].set_degree(id, degree);
             break;
     }
-    SetNowRadRelative(id, rad_relative);
+    SetNowRadRelative(id, old_rad[id]);//現在地変更はワンテンポ遅らせる(サーボの応答が遅いため)
+    old_rad[id] = rad_relative;
 }
 //一般性は皆無
 void ServoMoveOnArmInTurns()
@@ -49,12 +52,12 @@
             double rad = GetNextRadRelative(i);
             ServoRad(i, rad);
         }
-    } else if(count == 1) {
+    } else if(count == 2) {
         double rad = GetNextRadRelative(3);
         ServoRad(3, rad);
     }
     ++count;
-    if(count == 2) count = 0;
+    if(count == 3) count = 0;
 }
 //一般性は皆無
 void ServoMoveOnArm()