test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
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); +}