test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp
- Committer:
- yuto17320508
- Date:
- 2019-02-08
- Revision:
- 1:acaa3d7ede4c
- Parent:
- 0:f000d896d188
- Child:
- 2:a92568bdeb5c
File content as of revision 1:acaa3d7ede4c:
#include "mbed.h" #include "KondoServo.h" KondoServo servo(p9,p10); Ticker fliper; 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;//角速度 const float tickerTime = 0.006f;//Ticker間隔 const float offsetDegree = 0.0f; const float offsetY = 0.15f; float TargetTheta1 ; float TargetTheta2 ; public: Leg() { TargetTheta1 = 0.00f; 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 &targetTheta1, float &targetTheta2) { float r1 = sqrt((targetX - dist) * (targetX - dist) + targetY * targetY); float r2 = sqrt((targetX + dist) * (targetX + dist) + targetY * targetY); 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; 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, TargetTheta1,TargetTheta2); 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); }