Diff: servo/servo.cpp

Revision:

19:48c3af917932

Parent:

18:05f5a3323bda

Child:

20:13934809e117

--- a/servo/servo.cpp	Fri Aug 17 00:10:25 2018 +0000
+++ b/servo/servo.cpp	Fri Aug 17 08:47:23 2018 +0000
@@ -5,32 +5,45 @@
 #include "coordinate.h"
 #include "KondoServo.h"
 
-KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
+//////////////////////////////変更できるパラメータ
+const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
 const double kOriginTheta[] = {
     163.9575,
     236.79,
     151.1325,
     205.875,
 };//初期状態の角度
-
-
-const int kServoSign[] = {-1, -1, -1, 1};//サーボの正負と座標系の正負の補正
 const int kServoSpan_ms = 3; //指示の前後に必要なwait
+//////////////////////////////
+const int kServoOutPutOrder[] = {1,0,2,3};
+KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
 
 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 SetServoSpeed(int id, unsigned char speed);
+
+void ServoRad(int id, double rad)
+{
+    servo.set_degree(id, kServoSign[id] * rad *kRadToDegree + kOriginTheta[id]);
+    wait_ms(kServoSpan_ms);
 }
+
+void ServoMoveOnArmInTurns()
+{
+    static int i = 0;
+    double rad = GetNextRadRelative(i);
+    ServoRad(i, rad);
+    SetNowRadRelative(i,rad);
+    ++i;
+    if(i > SERVONUM - 2) i = 0;
+}
+
 void ServoMoveOnArm()
 {
-    for(int i = 0; i < kServoNum; i++) {
-        double rad = GetNextRadRelative(i);
-        ServoRad(i, rad);
-        SetNowRadRelative(i,rad);
+    for(int i = 0; i < SERVONUM - 1; i++) {
+        ServoMoveOnArmInTurns();
         wait_ms(kServoSpan_ms);
     }
-
 }
 
 
@@ -38,12 +51,24 @@
 void Open()
 {
     servo.set_degree(4,203.61375);
+    wait_ms(kServoSpan_ms);
     DEBUG("open real\r\n");
     //wait(1);
 }
 void Close()
 {
     servo.set_degree(4,136);//124.63875
+    wait_ms(kServoSpan_ms);
     DEBUG("close real\r\n");
     //wait(1);
+}
+void SetServoSpeed(int id, unsigned char speed)
+{
+    servo.setSpeed(id, speed);
+    wait_ms(kServoSpan_ms);
+    DEBUG("set servo %d speed %d\r\n", id, (int)speed);
+}
+void SetAllServoSpeed(unsigned char speed)
+{
+    for(int i = 0; i < 5; i++) SetServoSpeed(i, speed);
 }
\ No newline at end of file