Diff: servo/servo.cpp

Revision:

4:187c62291654

Parent:

3:05b1dcb3634e

Child:

5:af5ccfce1b90

--- a/servo/servo.cpp	Mon Aug 06 04:30:05 2018 +0000
+++ b/servo/servo.cpp	Thu Aug 09 05:37:04 2018 +0000
@@ -2,37 +2,27 @@
 #include <mbed.h>
 #include "debug.h"//DEBUG("",変数);でデバッグ。
 #include "pinnames.h"
+#include "coordinate.h"
 #include "KondoServo.h"
-double L = 500; //関節の長さ
-double l = 50;  //ハンドまでの長さ
 
 KondoServo servo(pin_serial_servo_tx, pin_serial_servo_rx);//サーボID:0,1,2,3,4
-const double kRadToDegree = 180/3.14159265359;
-const double origin[3] = {180,180,180}; //初期状態の角度
+const double kOriginTheta[] = {180,180,180,180}; //初期状態の角度
+const int kServoSign[]  {-1, -1, 1, 1};//サーボの正負と座標系の正負の補正
 const int kServoSpan_ms = 5; //指示の前後に必要なwait
 
 void Open();
 void Move(double x, double y, double theta);
 
-void ServoMoveOnArm(int id, double theta)
-{
-    servo.set_degree(id, (-1)*theta+origin[i]);
-}
-
-void Move(double x, double y, double theta)
+void ServoMoveOnArm()
 {
-    x = x - kArmLength[2] * cos(theta);
-    y = y - kArmLength[2] * sin(theta);
-    double theta_2 = acos((pow(x,2)+pow(y,2)-2*pow(L,2))/(2*pow(L,2)));
-    double theta_1 = -acos((L+L*cos(theta_2))/sqrt(pow(x,2)+pow(y,2)))+atan(y/x);
-    double theta_3 = theta * kRadToDegree+180 - (theta_1 + theta_2);
-    double angle[3] = {theta_1,theta_2,theta_3};
-    for(int i = 0; i < 3; i++){
-        ServoMoveOnArm(i, angle[i]);
+    for(int i = 0; i < kServoNum; i++) {
+        servo.set_degree(i, kServoSign[i]*GetNextRadRelative(i)+kOriginTheta[i]);
         wait_ms(kServoSpan_ms);
     }
 }
 
+
+
 int HandSet(WorkState &params)
 {
     switch(params.areaname) {
@@ -41,7 +31,7 @@
             Open();
             break;
         case BOX:
-            Open();
+            Close();
             break;
     }
     return 1;
@@ -66,7 +56,7 @@
 {
     servo.set_degree(3,262.125);
 }
-
+/*
 void ArmVerticalForward()
 {
     ArmVertical();
@@ -77,3 +67,4 @@
     ArmVertical();
     SetArmState(VERTICAL_BACK);
 }
+*/
\ No newline at end of file