test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 1:acaa3d7ede4c
- Parent:
- 0:f000d896d188
- Child:
- 2:a92568bdeb5c
--- 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()