Diff: servo/servo.cpp

Revision:

9:a553ace4d9d1

Parent:

5:af5ccfce1b90

--- a/servo/servo.cpp	Fri Aug 10 07:47:22 2018 +0000
+++ b/servo/servo.cpp	Sat Aug 11 06:09:04 2018 +0000
@@ -6,8 +6,8 @@
 #include "KondoServo.h"
 
 KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
-const double kOriginTheta[] = {180,180,180,180}; //初期状態の角度
-const int kServoSign[] = {-1, -1, 1, 1};//サーボの正負と座標系の正負の補正
+const double kOriginTheta[] = {164,236.8669,151.18,180}; //初期状態の角度
+const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
 const int kServoSpan_ms = 5; //指示の前後に必要なwait
 
 void Open();
@@ -17,7 +17,7 @@
 {
     for(int i = 0; i < kServoNum; i++) {
         double rad = GetNextRadRelative(i);
-        servo.set_degree(i, kServoSign[i] * rad + kOriginTheta[i]);
+        servo.set_degree(i, kServoSign[i] * rad*kRadToDegree + kOriginTheta[i]);
         SetNowRadRelative(i,rad);
         wait_ms(kServoSpan_ms);
     }