test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
main.cpp@0:f000d896d188, 2019-02-08 (annotated)
- Committer:
- yuto17320508
- Date:
- Fri Feb 08 07:53:07 2019 +0000
- Revision:
- 0:f000d896d188
- Child:
- 1:acaa3d7ede4c
serbo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuto17320508 | 0:f000d896d188 | 1 | #include "mbed.h" |
yuto17320508 | 0:f000d896d188 | 2 | #include "KondoServo.h" |
yuto17320508 | 0:f000d896d188 | 3 | |
yuto17320508 | 0:f000d896d188 | 4 | KondoServo servo(p9,p10); |
yuto17320508 | 0:f000d896d188 | 5 | Ticker fliper; |
yuto17320508 | 0:f000d896d188 | 6 | |
yuto17320508 | 0:f000d896d188 | 7 | const float LegLength1 = 0.1f; |
yuto17320508 | 0:f000d896d188 | 8 | const float LegLength2 = 0.2f; |
yuto17320508 | 0:f000d896d188 | 9 | const float dist = 0.03f; |
yuto17320508 | 0:f000d896d188 | 10 | const float omega = 3.1415f * 2.5f;//角速度 |
yuto17320508 | 0:f000d896d188 | 11 | const float tickerTime = 0.006f;//Ticker間隔 |
yuto17320508 | 0:f000d896d188 | 12 | const float offsetDegree = 0.0f; |
yuto17320508 | 0:f000d896d188 | 13 | const float offsetY = 0.15f; |
yuto17320508 | 0:f000d896d188 | 14 | |
yuto17320508 | 0:f000d896d188 | 15 | float TargetTheta1 = 0.00f; |
yuto17320508 | 0:f000d896d188 | 16 | float TargetTheta2 = 3.14f; |
yuto17320508 | 0:f000d896d188 | 17 | |
yuto17320508 | 0:f000d896d188 | 18 | void F_t(float t, float &tarX, float &tarY) |
yuto17320508 | 0:f000d896d188 | 19 | { |
yuto17320508 | 0:f000d896d188 | 20 | float theta = -omega * t + offsetDegree; |
yuto17320508 | 0:f000d896d188 | 21 | if (sin(theta) <= 0.0f) |
yuto17320508 | 0:f000d896d188 | 22 | { |
yuto17320508 | 0:f000d896d188 | 23 | tarX = 0.5f * LegLength2 * cos(theta);//0.5 |
yuto17320508 | 0:f000d896d188 | 24 | tarY = 0.15f * LegLength2 * sin(theta);//0.15 |
yuto17320508 | 0:f000d896d188 | 25 | } |
yuto17320508 | 0:f000d896d188 | 26 | else |
yuto17320508 | 0:f000d896d188 | 27 | { |
yuto17320508 | 0:f000d896d188 | 28 | tarX = 0.5f * LegLength2 * cos(theta); |
yuto17320508 | 0:f000d896d188 | 29 | tarY = 0.0f * LegLength2 * sin(theta); |
yuto17320508 | 0:f000d896d188 | 30 | } |
yuto17320508 | 0:f000d896d188 | 31 | } |
yuto17320508 | 0:f000d896d188 | 32 | |
yuto17320508 | 0:f000d896d188 | 33 | void PoseToAngles(float targetX, float targetY) |
yuto17320508 | 0:f000d896d188 | 34 | { |
yuto17320508 | 0:f000d896d188 | 35 | float alpha = atan2(targetY , targetX - dist ); |
yuto17320508 | 0:f000d896d188 | 36 | float beta = atan2(targetY , targetX + dist ); |
yuto17320508 | 0:f000d896d188 | 37 | //printf("alpha %.3f beta %.3f\r\n",alpha,beta); |
yuto17320508 | 0:f000d896d188 | 38 | float r1 = sqrt((targetX - dist) * (targetX - dist) + targetY * targetY); |
yuto17320508 | 0:f000d896d188 | 39 | float r2 = sqrt((targetX + dist) * (targetX + dist) + targetY * targetY); |
yuto17320508 | 0:f000d896d188 | 40 | //printf("r1 %.3f r2 %.3f\r\n",r1,r2); |
yuto17320508 | 0:f000d896d188 | 41 | float X1 = (LegLength1 * LegLength1 - LegLength2 * LegLength2 + r1 * r1) / (2.0f * r1 * LegLength1); |
yuto17320508 | 0:f000d896d188 | 42 | float X2 = (LegLength1 * LegLength1 - LegLength2 * LegLength2 + r2 * r2) / (2.0f * r2 * LegLength1); |
yuto17320508 | 0:f000d896d188 | 43 | //printf("X1 %.3f X2 %.3f\r\n",X1,X2); |
yuto17320508 | 0:f000d896d188 | 44 | float theta1pre1 = atan2(sqrt(1.0f - X1 * X1), X1); |
yuto17320508 | 0:f000d896d188 | 45 | float theta1pre2 = atan2(-sqrt(1.0f - X1 * X1), X1); |
yuto17320508 | 0:f000d896d188 | 46 | float theta2pre1 = atan2(sqrt(1.0f - X2 * X2), X2); |
yuto17320508 | 0:f000d896d188 | 47 | float theta2pre2 = atan2(-sqrt(1.0f - X2 * X2), X2); |
yuto17320508 | 0:f000d896d188 | 48 | |
yuto17320508 | 0:f000d896d188 | 49 | if (theta1pre1 >= 0 && theta1pre2 <= 0) |
yuto17320508 | 0:f000d896d188 | 50 | { |
yuto17320508 | 0:f000d896d188 | 51 | TargetTheta1 = alpha - theta1pre1; |
yuto17320508 | 0:f000d896d188 | 52 | } |
yuto17320508 | 0:f000d896d188 | 53 | else if(theta1pre1 <=0 && theta1pre2 >= 0) |
yuto17320508 | 0:f000d896d188 | 54 | { |
yuto17320508 | 0:f000d896d188 | 55 | TargetTheta1 = alpha - theta1pre2; |
yuto17320508 | 0:f000d896d188 | 56 | } |
yuto17320508 | 0:f000d896d188 | 57 | if (theta2pre1 >= 0 && theta2pre2 <= 0) |
yuto17320508 | 0:f000d896d188 | 58 | { |
yuto17320508 | 0:f000d896d188 | 59 | TargetTheta2 = beta - theta2pre2; |
yuto17320508 | 0:f000d896d188 | 60 | } |
yuto17320508 | 0:f000d896d188 | 61 | else if (theta2pre1 <= 0 && theta2pre2 >= 0) |
yuto17320508 | 0:f000d896d188 | 62 | { |
yuto17320508 | 0:f000d896d188 | 63 | TargetTheta2 = beta - theta2pre1; |
yuto17320508 | 0:f000d896d188 | 64 | } |
yuto17320508 | 0:f000d896d188 | 65 | } |
yuto17320508 | 0:f000d896d188 | 66 | |
yuto17320508 | 0:f000d896d188 | 67 | double Theta1; |
yuto17320508 | 0:f000d896d188 | 68 | double Theta2; |
yuto17320508 | 0:f000d896d188 | 69 | int flagMotor; |
yuto17320508 | 0:f000d896d188 | 70 | |
yuto17320508 | 0:f000d896d188 | 71 | void MoveLeg(float theta1, float theta2) |
yuto17320508 | 0:f000d896d188 | 72 | { |
yuto17320508 | 0:f000d896d188 | 73 | Theta1 = ((double)theta1*180.0/3.14) + 90.0; |
yuto17320508 | 0:f000d896d188 | 74 | Theta2 = 270.0 - (double)theta2*180.0/3.14; |
yuto17320508 | 0:f000d896d188 | 75 | |
yuto17320508 | 0:f000d896d188 | 76 | /*printf("target theta1:%.3f ",Theta1); |
yuto17320508 | 0:f000d896d188 | 77 | printf("target theta2:%.3f \r\n",Theta2);*/ |
yuto17320508 | 0:f000d896d188 | 78 | } |
yuto17320508 | 0:f000d896d188 | 79 | |
yuto17320508 | 0:f000d896d188 | 80 | void InputPose(float X, float Y) |
yuto17320508 | 0:f000d896d188 | 81 | { |
yuto17320508 | 0:f000d896d188 | 82 | PoseToAngles(X,Y); |
yuto17320508 | 0:f000d896d188 | 83 | MoveLeg(TargetTheta1,TargetTheta2); |
yuto17320508 | 0:f000d896d188 | 84 | } |
yuto17320508 | 0:f000d896d188 | 85 | |
yuto17320508 | 0:f000d896d188 | 86 | float t = 0.0f; |
yuto17320508 | 0:f000d896d188 | 87 | |
yuto17320508 | 0:f000d896d188 | 88 | void cb_timer() |
yuto17320508 | 0:f000d896d188 | 89 | { |
yuto17320508 | 0:f000d896d188 | 90 | float tX,tY; |
yuto17320508 | 0:f000d896d188 | 91 | if(flagMotor == 0) |
yuto17320508 | 0:f000d896d188 | 92 | { |
yuto17320508 | 0:f000d896d188 | 93 | servo.set_degree(1, Theta1); |
yuto17320508 | 0:f000d896d188 | 94 | F_t(t, tX, tY); |
yuto17320508 | 0:f000d896d188 | 95 | InputPose(tX,tY+offsetY); |
yuto17320508 | 0:f000d896d188 | 96 | t = t+ 2.0f*tickerTime; |
yuto17320508 | 0:f000d896d188 | 97 | flagMotor = 1; |
yuto17320508 | 0:f000d896d188 | 98 | } |
yuto17320508 | 0:f000d896d188 | 99 | else if(flagMotor == 1) |
yuto17320508 | 0:f000d896d188 | 100 | { |
yuto17320508 | 0:f000d896d188 | 101 | servo.set_degree(0, Theta2); |
yuto17320508 | 0:f000d896d188 | 102 | flagMotor = 0; |
yuto17320508 | 0:f000d896d188 | 103 | } |
yuto17320508 | 0:f000d896d188 | 104 | } |
yuto17320508 | 0:f000d896d188 | 105 | |
yuto17320508 | 0:f000d896d188 | 106 | int main() |
yuto17320508 | 0:f000d896d188 | 107 | { |
yuto17320508 | 0:f000d896d188 | 108 | fliper.attach(&cb_timer, tickerTime); |
yuto17320508 | 0:f000d896d188 | 109 | } |