Diff: main.cpp

Revision:

3:bcae0bb64b81

Parent:

2:a92568bdeb5c

Child:

4:fffdb273836e

--- a/main.cpp	Fri Feb 08 12:14:19 2019 +0000
+++ b/main.cpp	Fri Feb 08 16:59:48 2019 +0000
@@ -1,70 +1,80 @@
+//mbed間の同期を、master-slaveで行いたい。
+
 #include "mbed.h"
 #include "KondoServo.h"
+#define Mastar;
+//#define Slave;
 ////////////////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
-{
-    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)
+namespace Quadruped
 {
-    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;
+    class Leg
+    {
+        float rad_[2];
+        int id_[2];
+        //パラメータ。実際の機体に合わせていじる
+        static const float dist_between_servo_half_m_ = 0.06f * 0.5;
+        static const float LegLength1 = 0.1f;
+        static const float LegLength2 = 0.2f;
+        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);
+    };
 
-    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(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);
+//////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
+using namespace Quadruped;
+Leg leg1(p9, p10);
+Leg leg2(p28,p27);
+CAN can1(p30,p29);
 Ticker fliper;
 
 const float tickerTime = 0.006f;    //Ticker間隔
@@ -77,13 +87,13 @@
     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.1f * cos(theta);  //0.1
+        tarY = 0.03f * sin(theta); //0.03
     }
     else
     {
-        tarX = 0.5f * LegLength2 * cos(theta);
-        tarY = 0.0f * LegLength2 * sin(theta);
+        tarX = 0.1f * cos(theta);
+        tarY = 0.0f * sin(theta);
     }
 }
 void cb_timer()
@@ -93,22 +103,25 @@
     float target_x_m = 0;
     float target_y_m = 0;
     //出力。片サーボずつ
-    leg.MoveServo(servo_num);
+    leg1.MoveServo(servo_num);
+    leg2.MoveServo(servo_num);
     ++servo_num;
     if (servo_num > 1) //両サーボ出力したら
     {
         //目標角の更新
         F_t(time_s, target_x_m, target_y_m);
-        leg.CalServoRad(target_x_m, target_y_m + offsetY);
+        leg1.CalServoRad(target_x_m, target_y_m + offsetY);
+        leg2.CalServoRad(target_x_m, target_y_m + offsetY);
         servo_num = 0;
     }
     time_s += tickerTime;
 }
 int main()
 {
-       float target_x_m = 0;
+    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);
+    leg1.CalServoRad(target_x_m, target_y_m + offsetY);
+    leg2.CalServoRad(target_x_m, target_y_m + offsetY);
     fliper.attach(&cb_timer, tickerTime);
 }