Diff: servo/servo.cpp

Revision:

21:af9a33d69745

Parent:

20:13934809e117

Child:

23:f45eb02433a5

--- a/servo/servo.cpp	Mon Aug 27 03:47:21 2018 +0000
+++ b/servo/servo.cpp	Tue Aug 28 03:09:31 2018 +0000
@@ -9,7 +9,7 @@
 const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
 const double kOriginTheta[] = {
     180,
-    180,
+    0,
     135,
     205.875,
 };//初期状態の角度
@@ -18,13 +18,13 @@
 const double kCloseVal = 136;//ハンドを閉じたときのサーボへの指示
 const int kTipServoID = 4;
 //////////////////////////////
-KondoServo servo[] = {KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),//サーボID:0
-                      KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),//サーボID:1
-                      KondoServo(pin_serial_servo_tx[2], pin_serial_servo_rx[2]),//サーボID:2,3,4
+KondoServo servo[] = {KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),//サーボID:2,3,4
+                      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,
                      };
 void ServoRad(int id, double rad_relative)
 {
-    double degree = kServoSign[id] * rad_relative *kRadToDegree + kOriginTheta[id];
+    double degree = kServoSign[id] * rad_relative * kRadToDegree + kOriginTheta[id];
     switch(id) {
         case 0:
             servo[1].set_degree(id, degree);
@@ -40,17 +40,26 @@
     }
     SetNowRadRelative(id, rad_relative);
 }
+//一般性は皆無
 void ServoMoveOnArmInTurns()
 {
     static int count = 0;
-    double rad = GetNextRadRelative(count);
-    ServoRad(count, rad);
+    if(count == 0) {
+        for(int i = 0; i < 3; i++) {
+            double rad = GetNextRadRelative(i);
+            ServoRad(i, rad);
+        }
+    } else if(count == 1) {
+        double rad = GetNextRadRelative(3);
+        ServoRad(3, rad);
+    }
     ++count;
-    if(count > SERVONUM - 2) count = 0;
+    if(count == 2) count = 0;
 }
+//一般性は皆無
 void ServoMoveOnArm()
 {
-    for(int i = 0; i < SERVONUM - 1; i++) {
+    for(int i = 0; i < 2; i++) {
         ServoMoveOnArmInTurns();
         wait_ms(kServoSpan_ms);
     }