Diff: main.cpp

Revision:

0:f000d896d188

Child:

1:acaa3d7ede4c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 08 07:53:07 2019 +0000
@@ -0,0 +1,109 @@
+#include "mbed.h"
+#include "KondoServo.h"
+
+KondoServo servo(p9,p10);
+Ticker fliper;
+
+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間隔
+const float offsetDegree = 0.0f;
+const float offsetY = 0.15f;
+
+float TargetTheta1 = 0.00f;
+float 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
+    }
+    else
+    {
+        tarX = 0.5f * LegLength2 * cos(theta);
+        tarY = 0.0f * LegLength2 * sin(theta);
+    }
+}
+
+void PoseToAngles(float targetX, float targetY)
+{
+    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;
+    }
+}
+
+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);
+    MoveLeg(TargetTheta1,TargetTheta2);
+}
+
+float t = 0.0f;
+
+void cb_timer()
+{
+    float tX,tY;
+    if(flagMotor == 0)
+    {
+        servo.set_degree(1, Theta1);
+        F_t(t, tX, tY);
+        InputPose(tX,tY+offsetY);
+        t = t+ 2.0f*tickerTime;
+        flagMotor = 1;
+    }
+    else if(flagMotor == 1)
+    {
+        servo.set_degree(0, Theta2);
+        flagMotor = 0;
+    }
+}
+
+int main() 
+{
+    fliper.attach(&cb_timer, tickerTime);
+}