岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@1:acaa3d7ede4c, 2019-02-08 (annotated)

Committer:
yuto17320508
Date:
Fri Feb 08 10:01:44 2019 +0000
Revision:
1:acaa3d7ede4c
Parent:
0:f000d896d188
Child:
2:a92568bdeb5c
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuto17320508 0:f000d896d188 1 #include "mbed.h"
yuto17320508 0:f000d896d188 2 #include "KondoServo.h"
yuto17320508 0:f000d896d188 3
yuto17320508 1:acaa3d7ede4c 4
yuto17320508 0:f000d896d188 5 KondoServo servo(p9,p10);
yuto17320508 0:f000d896d188 6 Ticker fliper;
yuto17320508 0:f000d896d188 7
yuto17320508 1:acaa3d7ede4c 8 const float tickerTime = 0.006f;//Ticker間隔
yuto17320508 1:acaa3d7ede4c 9
yuto17320508 1:acaa3d7ede4c 10 class Leg
yuto17320508 1:acaa3d7ede4c 11 {
yuto17320508 1:acaa3d7ede4c 12 const float LegLength1 = 0.1f;
yuto17320508 0:f000d896d188 13 const float LegLength2 = 0.2f;
yuto17320508 0:f000d896d188 14 const float dist = 0.03f;
yuto17320508 0:f000d896d188 15 const float omega = 3.1415f * 2.5f;//角速度
yuto17320508 0:f000d896d188 16 const float tickerTime = 0.006f;//Ticker間隔
yuto17320508 0:f000d896d188 17 const float offsetDegree = 0.0f;
yuto17320508 0:f000d896d188 18 const float offsetY = 0.15f;
yuto17320508 0:f000d896d188 19
yuto17320508 1:acaa3d7ede4c 20 float TargetTheta1 ;
yuto17320508 1:acaa3d7ede4c 21 float TargetTheta2 ;
yuto17320508 1:acaa3d7ede4c 22
yuto17320508 1:acaa3d7ede4c 23 public:
yuto17320508 1:acaa3d7ede4c 24 Leg()
yuto17320508 1:acaa3d7ede4c 25 {
yuto17320508 1:acaa3d7ede4c 26 TargetTheta1 = 0.00f;
yuto17320508 1:acaa3d7ede4c 27 TargetTheta2 = 3.14f;
yuto17320508 1:acaa3d7ede4c 28 }
yuto17320508 1:acaa3d7ede4c 29
yuto17320508 0:f000d896d188 30
yuto17320508 0:f000d896d188 31 void F_t(float t, float &tarX, float &tarY)
yuto17320508 0:f000d896d188 32 {
yuto17320508 0:f000d896d188 33 float theta = -omega * t + offsetDegree;
yuto17320508 0:f000d896d188 34 if (sin(theta) <= 0.0f)
yuto17320508 0:f000d896d188 35 {
yuto17320508 0:f000d896d188 36 tarX = 0.5f * LegLength2 * cos(theta);//0.5
yuto17320508 0:f000d896d188 37 tarY = 0.15f * LegLength2 * sin(theta);//0.15
yuto17320508 0:f000d896d188 38 }
yuto17320508 0:f000d896d188 39 else
yuto17320508 0:f000d896d188 40 {
yuto17320508 0:f000d896d188 41 tarX = 0.5f * LegLength2 * cos(theta);
yuto17320508 0:f000d896d188 42 tarY = 0.0f * LegLength2 * sin(theta);
yuto17320508 0:f000d896d188 43 }
yuto17320508 0:f000d896d188 44 }
yuto17320508 0:f000d896d188 45
yuto17320508 1:acaa3d7ede4c 46 void PoseToAngles(float targetX, float targetY, float &targetTheta1, float &targetTheta2)
yuto17320508 0:f000d896d188 47 {
yuto17320508 0:f000d896d188 48 float r1 = sqrt((targetX - dist) * (targetX - dist) + targetY * targetY);
yuto17320508 0:f000d896d188 49 float r2 = sqrt((targetX + dist) * (targetX + dist) + targetY * targetY);
yuto17320508 1:acaa3d7ede4c 50 targetTheta1 = atan2(targetY , targetX - dist) - acos((LegLength1 * LegLength1 - LegLength2 * LegLength2 + r1 * r1) / (2.0f * r1 * LegLength1));
yuto17320508 1:acaa3d7ede4c 51 targetTheta2 = atan2(targetY , targetX + dist) + acos((LegLength1 * LegLength1 - LegLength2 * LegLength2 + r2 * r2) / (2.0f * r2 * LegLength1));
yuto17320508 1:acaa3d7ede4c 52
yuto17320508 1:acaa3d7ede4c 53
yuto17320508 0:f000d896d188 54 }
yuto17320508 0:f000d896d188 55
yuto17320508 0:f000d896d188 56 double Theta1;
yuto17320508 0:f000d896d188 57 double Theta2;
yuto17320508 0:f000d896d188 58 int flagMotor;
yuto17320508 0:f000d896d188 59
yuto17320508 0:f000d896d188 60 void MoveLeg(float theta1, float theta2)
yuto17320508 0:f000d896d188 61 {
yuto17320508 0:f000d896d188 62 Theta1 = ((double)theta1*180.0/3.14) + 90.0;
yuto17320508 0:f000d896d188 63 Theta2 = 270.0 - (double)theta2*180.0/3.14;
yuto17320508 0:f000d896d188 64
yuto17320508 0:f000d896d188 65 /*printf("target theta1:%.3f ",Theta1);
yuto17320508 0:f000d896d188 66 printf("target theta2:%.3f \r\n",Theta2);*/
yuto17320508 0:f000d896d188 67 }
yuto17320508 0:f000d896d188 68
yuto17320508 0:f000d896d188 69 void InputPose(float X, float Y)
yuto17320508 0:f000d896d188 70 {
yuto17320508 1:acaa3d7ede4c 71 PoseToAngles(X,Y, TargetTheta1,TargetTheta2);
yuto17320508 0:f000d896d188 72 MoveLeg(TargetTheta1,TargetTheta2);
yuto17320508 0:f000d896d188 73 }
yuto17320508 0:f000d896d188 74
yuto17320508 1:acaa3d7ede4c 75 };
yuto17320508 1:acaa3d7ede4c 76
yuto17320508 0:f000d896d188 77 float t = 0.0f;
yuto17320508 0:f000d896d188 78 void cb_timer()
yuto17320508 0:f000d896d188 79 {
yuto17320508 1:acaa3d7ede4c 80 /*float tX,tY;
yuto17320508 0:f000d896d188 81 if(flagMotor == 0)
yuto17320508 0:f000d896d188 82 {
yuto17320508 0:f000d896d188 83 servo.set_degree(1, Theta1);
yuto17320508 0:f000d896d188 84 F_t(t, tX, tY);
yuto17320508 0:f000d896d188 85 InputPose(tX,tY+offsetY);
yuto17320508 0:f000d896d188 86 t = t+ 2.0f*tickerTime;
yuto17320508 0:f000d896d188 87 flagMotor = 1;
yuto17320508 0:f000d896d188 88 }
yuto17320508 0:f000d896d188 89 else if(flagMotor == 1)
yuto17320508 0:f000d896d188 90 {
yuto17320508 0:f000d896d188 91 servo.set_degree(0, Theta2);
yuto17320508 0:f000d896d188 92 flagMotor = 0;
yuto17320508 1:acaa3d7ede4c 93 }*/
yuto17320508 0:f000d896d188 94 }
yuto17320508 0:f000d896d188 95
yuto17320508 0:f000d896d188 96 int main()
yuto17320508 0:f000d896d188 97 {
yuto17320508 0:f000d896d188 98 fliper.attach(&cb_timer, tickerTime);
yuto17320508 0:f000d896d188 99 }