Diff: servo/servo.cpp

Revision:

37:d4b711619962

Parent:

36:b8171eeefcd0

Child:

38:9bed85d32c97

--- a/servo/servo.cpp	Wed Sep 05 03:58:53 2018 +0000
+++ b/servo/servo.cpp	Wed Sep 05 09:37:41 2018 +0000
@@ -4,18 +4,20 @@
 #include "pinnames.h"
 #include "coordinate.h"
 #include "KondoServo.h"
+#include "servo.h"
 const double kServoValToDegree =  270.0 / (11500 - 3500);
 //////////////////////////////変更できるパラメータ
 const int kServoSign[] = {-1, -1, -1, -1};//サーボの正負と座標系の正負の補正
-const double kOriginDegree[] = {
+const double kOriginDegree[SERVONUM - 1] = {
     (8850 - 3500)  * kServoValToDegree ,
     (8635 - 3500)  * kServoValToDegree,
     (7533 - 3500) * kServoValToDegree,
     (8000 - 3500) * kServoValToDegree,
 };//初期状態の角度
-const int kServoSpan_ms = 3; //指示の前後に必要なwait
-const double kOpenVal = (7400 - 3500) * kServoValToDegree;//ハンドを開いたときのサーボへの指示
-const double kCloseVal = (5200 - 3500) * kServoValToDegree - 5;//ハンドを閉じたときのサーボへの指示
+
+const int kServoSpan_ms = 5; //指示の前後に必要なwait
+const double kOpenVal = (9000 - 3500) * kServoValToDegree;//ハンドを開いたときのサーボへの指示
+const double kCloseVal = (7000 - 3500) * kServoValToDegree;//ハンドを閉じたときのサーボへの指示
 const int kTipServoID = 4;
 //////////////////////////////
 KondoServo servo[] = {KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),//サーボID:2,3,4
@@ -23,10 +25,10 @@
                       KondoServo(pin_serial_servo_tx[2], pin_serial_servo_rx[2]),//サーボID:1,
                      };
 double old_rad[SERVONUM] = {};
-
+double OriginDegree[SERVONUM - 1] = {kOriginDegree[0], kOriginDegree[1], kOriginDegree[2], kOriginDegree[3]};
 void ServoRad(int id, double rad_relative)
 {
-    double degree = kServoSign[id] * rad_relative * kRadToDegree + kOriginDegree[id];
+    double degree = kServoSign[id] * rad_relative * kRadToDegree + OriginDegree[id];
     switch(id) {
         case 0:
             servo[1].set_degree(id, degree);