岡本 陸 / Mbed 2 deprecated 0311

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?

UserRevisionLine numberNew 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 }