Diff: main.cpp

Revision:

1:acaa3d7ede4c

Parent:

0:f000d896d188

Child:

2:a92568bdeb5c

diff -r f000d896d188 -r acaa3d7ede4c main.cpp
--- a/main.cpp	Fri Feb 08 07:53:07 2019 +0000
+++ b/main.cpp	Fri Feb 08 10:01:44 2019 +0000
@@ -1,10 +1,15 @@
 #include "mbed.h"
 #include "KondoServo.h"
 
+
 KondoServo servo(p9,p10);
 Ticker fliper;
 
-const float LegLength1 = 0.1f;
+const float tickerTime = 0.006f;//Ticker間隔
+
+class Leg
+{
+    const float LegLength1 = 0.1f;
 const float LegLength2 = 0.2f;
 const float dist = 0.03f;
 const float omega = 3.1415f * 2.5f;//角速度
@@ -12,8 +17,16 @@
 const float offsetDegree = 0.0f;
 const float offsetY = 0.15f;
 
-float TargetTheta1 = 0.00f;
-float TargetTheta2 = 3.14f;
+float TargetTheta1 ;
+float TargetTheta2 ;
+
+    public:
+    Leg()
+    {
+        TargetTheta1 = 0.00f;
+        TargetTheta2 = 3.14f;
+    }
+    
 
 void F_t(float t, float &tarX, float &tarY)
 {
@@ -30,38 +43,14 @@
     }
 }
 
-void PoseToAngles(float targetX, float targetY)
+void PoseToAngles(float targetX, float targetY, float &targetTheta1, float &targetTheta2)
 {
-    float alpha = atan2(targetY , targetX - dist );
-    float beta = atan2(targetY , targetX + dist );
-    //printf("alpha %.3f  beta %.3f\r\n",alpha,beta);
     float r1 = sqrt((targetX - dist) * (targetX - dist) + targetY * targetY);
     float r2 = sqrt((targetX + dist) * (targetX + dist) + targetY * targetY);
-    //printf("r1 %.3f  r2 %.3f\r\n",r1,r2);
-    float X1 = (LegLength1 * LegLength1 - LegLength2 * LegLength2 + r1 * r1) / (2.0f * r1 * LegLength1);
-    float X2 = (LegLength1 * LegLength1 - LegLength2 * LegLength2 + r2 * r2) / (2.0f * r2 * LegLength1);
-    //printf("X1 %.3f  X2 %.3f\r\n",X1,X2);
-    float theta1pre1 = atan2(sqrt(1.0f - X1 * X1), X1);
-    float theta1pre2 = atan2(-sqrt(1.0f - X1 * X1), X1);
-    float theta2pre1 = atan2(sqrt(1.0f - X2 * X2), X2);
-    float theta2pre2 = atan2(-sqrt(1.0f - X2 * X2), X2);
-
-    if (theta1pre1 >= 0 && theta1pre2 <= 0)
-    {
-        TargetTheta1 = alpha - theta1pre1;
-    }
-    else if(theta1pre1 <=0 && theta1pre2 >= 0)
-    {
-        TargetTheta1 = alpha - theta1pre2;
-    }
-    if (theta2pre1 >= 0 && theta2pre2 <= 0)
-    {
-        TargetTheta2 = beta - theta2pre2;
-    }
-    else if (theta2pre1 <= 0 && theta2pre2 >= 0)
-    {
-        TargetTheta2 = beta - theta2pre1;
-    }
+    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;
@@ -79,15 +68,16 @@
 
 void InputPose(float X, float Y)
 {
-    PoseToAngles(X,Y);
+    PoseToAngles(X,Y, TargetTheta1,TargetTheta2);
     MoveLeg(TargetTheta1,TargetTheta2);
 }
 
+};
+
 float t = 0.0f;
-
 void cb_timer()
 {
-    float tX,tY;
+    /*float tX,tY;
     if(flagMotor == 0)
     {
         servo.set_degree(1, Theta1);
@@ -100,7 +90,7 @@
     {
         servo.set_degree(0, Theta2);
         flagMotor = 0;
-    }
+    }*/
 }
 
 int main()