Diff: servo/servo.cpp

Revision:

3:05b1dcb3634e

Parent:

2:4515e8cc6b60

Child:

4:187c62291654

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo/servo.cpp	Mon Aug 06 04:30:05 2018 +0000
@@ -0,0 +1,79 @@
+#include "servo.h"
+#include <mbed.h>
+#include "debug.h"//DEBUG("",変数);でデバッグ。
+#include "pinnames.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 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)
+{
+    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]);
+        wait_ms(kServoSpan_ms);
+    }
+}
+
+int HandSet(WorkState &params)
+{
+    switch(params.areaname) {
+        case COMMONAREA:
+        case WORKAREA:
+            Open();
+            break;
+        case BOX:
+            Open();
+            break;
+    }
+    return 1;
+}
+
+void Open()
+{
+    servo.set_degree(4,182.7225);
+}
+
+void Close()
+{
+    servo.set_degree(4,135.43875);
+}
+
+void ArmHorizon()
+{
+    servo.set_degree(3,172.125);
+    SetArmState(HORIZON);
+}
+void ArmVertical()
+{
+    servo.set_degree(3,262.125);
+}
+
+void ArmVerticalForward()
+{
+    ArmVertical();
+    SetArmState(VERTICAL_FORWARD);
+}
+void ArmVerticalBack()
+{
+    ArmVertical();
+    SetArmState(VERTICAL_BACK);
+}