Diff: main.cpp

Revision:

2:a92568bdeb5c

Parent:

1:acaa3d7ede4c

Child:

3:bcae0bb64b81

--- a/main.cpp	Fri Feb 08 10:01:44 2019 +0000
+++ b/main.cpp	Fri Feb 08 12:14:19 2019 +0000
@@ -1,40 +1,84 @@
 #include "mbed.h"
 #include "KondoServo.h"
-
-
-KondoServo servo(p9,p10);
-Ticker fliper;
-
-const float tickerTime = 0.006f;//Ticker間隔
+////////////////Legクラス。下にmain文がある。
+const float Pi = 3.141592;
+const float kRadToDegree = 180.0 / Pi;
+//const float kDegreeToRad = Pi / 180.0;
+const float LegLength1 = 0.1f;
+const float LegLength2 = 0.2f;
 
 class Leg
 {
-    const float LegLength1 = 0.1f;
-const float LegLength2 = 0.2f;
-const float dist = 0.03f;
-const float omega = 3.1415f * 2.5f;//角速度
-const float tickerTime = 0.006f;//Ticker間隔
+    float rad_[2];
+    int id_[2];
+    //パラメータ。実際の機体に合わせていじる
+    static const float dist_between_servo_half_m_ = 0.06f * 0.5;
+    KondoServo servo_;
+
+  public:
+    Leg(PinName pin_serial_tx, PinName pin_serial_rx);
+    void MoveServo(int servo_num);
+    void CalServoRad(float x_m, float y_m);
+    void SetRad(float rad, int servo_num);
+    float GetRad(int servo_num);
+};
+
+Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
+{
+    rad_[0] = 0;
+    rad_[1] = Pi;
+};
+void Leg::MoveServo(int servo_num)
+{
+    float degree = GetRad(servo_num) * kRadToDegree;
+    //servo1は反転させる
+    if (servo_num == 0)
+        degree += 90;
+    else
+        degree = 270 - degree;
+    servo_.set_degree(servo_num, degree);
+}
+void Leg::CalServoRad(float x_m, float y_m)
+{
+    //処理を軽くするために共通部分は先に計算
+    float temp_x[] = {x_m + dist_between_servo_half_m_,
+                      x_m - dist_between_servo_half_m_};
+    float temp_y2 = y_m * y_m;
+    float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2;
+
+    float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
+    float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
+    float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)),
+                           atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))};
+    for (size_t i = 0; i < 2; i++)
+        SetRad(targetTheta[i], i);
+}
+void Leg::SetRad(float rad, int servo_num)
+{
+    rad_[servo_num] = rad;
+}
+float Leg::GetRad(int servo_num)
+{
+    return rad_[servo_num];
+}
+
+//////////////////////////////////////////////////////////////Legクラスの定義終了
+
+Leg leg(p9, p10);
+Ticker fliper;
+
+const float tickerTime = 0.006f;    //Ticker間隔
+const float omega = 3.1415f * 2.5f; //角速度
 const float offsetDegree = 0.0f;
 const float offsetY = 0.15f;
 
-float TargetTheta1 ;
-float TargetTheta2 ;
-
-    public:
-    Leg()
-    {
-        TargetTheta1 = 0.00f;
-        TargetTheta2 = 3.14f;
-    }
-    
-
 void F_t(float t, float &tarX, float &tarY)
 {
     float theta = -omega * t + offsetDegree;
     if (sin(theta) <= 0.0f)
     {
-        tarX = 0.5f * LegLength2 * cos(theta);//0.5
-        tarY = 0.15f * LegLength2 * sin(theta);//0.15
+        tarX = 0.5f * LegLength2 * cos(theta);  //0.5
+        tarY = 0.15f * LegLength2 * sin(theta); //0.15
     }
     else
     {
@@ -42,58 +86,29 @@
         tarY = 0.0f * LegLength2 * sin(theta);
     }
 }
-
-void PoseToAngles(float targetX, float targetY, float &targetTheta1, float &targetTheta2)
-{
-    float r1 = sqrt((targetX - dist) * (targetX - dist) + targetY * targetY);
-    float r2 = sqrt((targetX + dist) * (targetX + dist) + targetY * targetY);
-    targetTheta1 = atan2(targetY , targetX - dist) - acos((LegLength1 * LegLength1 - LegLength2 * LegLength2 + r1 * r1) / (2.0f * r1 * LegLength1));
-    targetTheta2 = atan2(targetY , targetX + dist) + acos((LegLength1 * LegLength1 - LegLength2 * LegLength2 + r2 * r2) / (2.0f * r2 * LegLength1));
-    
-    
-}
-
-double Theta1;
-double Theta2;
-int flagMotor;
-
-void MoveLeg(float theta1, float theta2)
-{
-     Theta1 = ((double)theta1*180.0/3.14) + 90.0;
-     Theta2 = 270.0 - (double)theta2*180.0/3.14;
-     
-     /*printf("target theta1:%.3f  ",Theta1);
-     printf("target theta2:%.3f  \r\n",Theta2);*/
-}
-
-void InputPose(float X, float Y)
-{
-    PoseToAngles(X,Y, TargetTheta1,TargetTheta2);
-    MoveLeg(TargetTheta1,TargetTheta2);
-}
-
-};
-
-float t = 0.0f;
 void cb_timer()
 {
-    /*float tX,tY;
-    if(flagMotor == 0)
+    static int servo_num = 0;
+    static float time_s = 0.0f;
+    float target_x_m = 0;
+    float target_y_m = 0;
+    //出力。片サーボずつ
+    leg.MoveServo(servo_num);
+    ++servo_num;
+    if (servo_num > 1) //両サーボ出力したら
     {
-        servo.set_degree(1, Theta1);
-        F_t(t, tX, tY);
-        InputPose(tX,tY+offsetY);
-        t = t+ 2.0f*tickerTime;
-        flagMotor = 1;
+        //目標角の更新
+        F_t(time_s, target_x_m, target_y_m);
+        leg.CalServoRad(target_x_m, target_y_m + offsetY);
+        servo_num = 0;
     }
-    else if(flagMotor == 1)
-    {
-        servo.set_degree(0, Theta2);
-        flagMotor = 0;
-    }*/
+    time_s += tickerTime;
 }
-
-int main() 
+int main()
 {
+       float target_x_m = 0;
+    float target_y_m = 0;
+    F_t(0, target_x_m, target_y_m);
+    leg.CalServoRad(target_x_m, target_y_m + offsetY);
     fliper.attach(&cb_timer, tickerTime);
 }