test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 2:a92568bdeb5c
- Parent:
- 1:acaa3d7ede4c
- Child:
- 3:bcae0bb64b81
--- a/main.cpp Fri Feb 08 10:01:44 2019 +0000 +++ b/main.cpp Fri Feb 08 12:14:19 2019 +0000 @@ -1,40 +1,84 @@ #include "mbed.h" #include "KondoServo.h" - - -KondoServo servo(p9,p10); -Ticker fliper; - -const float tickerTime = 0.006f;//Ticker間隔 +////////////////Legクラス。下にmain文がある。 +const float Pi = 3.141592; +const float kRadToDegree = 180.0 / Pi; +//const float kDegreeToRad = Pi / 180.0; +const float LegLength1 = 0.1f; +const float LegLength2 = 0.2f; 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間隔 + float rad_[2]; + int id_[2]; + //パラメータ。実際の機体に合わせていじる + static const float dist_between_servo_half_m_ = 0.06f * 0.5; + KondoServo servo_; + + public: + Leg(PinName pin_serial_tx, PinName pin_serial_rx); + void MoveServo(int servo_num); + void CalServoRad(float x_m, float y_m); + void SetRad(float rad, int servo_num); + float GetRad(int servo_num); +}; + +Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx) +{ + rad_[0] = 0; + rad_[1] = Pi; +}; +void Leg::MoveServo(int servo_num) +{ + float degree = GetRad(servo_num) * kRadToDegree; + //servo1は反転させる + if (servo_num == 0) + degree += 90; + else + degree = 270 - degree; + servo_.set_degree(servo_num, degree); +} +void Leg::CalServoRad(float x_m, float y_m) +{ + //処理を軽くするために共通部分は先に計算 + float temp_x[] = {x_m + dist_between_servo_half_m_, + x_m - dist_between_servo_half_m_}; + float temp_y2 = y_m * y_m; + float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2; + + float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2); + float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2); + float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)), + atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))}; + for (size_t i = 0; i < 2; i++) + SetRad(targetTheta[i], i); +} +void Leg::SetRad(float rad, int servo_num) +{ + rad_[servo_num] = rad; +} +float Leg::GetRad(int servo_num) +{ + return rad_[servo_num]; +} + +//////////////////////////////////////////////////////////////Legクラスの定義終了 + +Leg leg(p9, p10); +Ticker fliper; + +const float tickerTime = 0.006f; //Ticker間隔 +const float omega = 3.1415f * 2.5f; //角速度 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 + tarX = 0.5f * LegLength2 * cos(theta); //0.5 + tarY = 0.15f * LegLength2 * sin(theta); //0.15 } else { @@ -42,58 +86,29 @@ 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) + static int servo_num = 0; + static float time_s = 0.0f; + float target_x_m = 0; + float target_y_m = 0; + //出力。片サーボずつ + leg.MoveServo(servo_num); + ++servo_num; + if (servo_num > 1) //両サーボ出力したら { - servo.set_degree(1, Theta1); - F_t(t, tX, tY); - InputPose(tX,tY+offsetY); - t = t+ 2.0f*tickerTime; - flagMotor = 1; + //目標角の更新 + F_t(time_s, target_x_m, target_y_m); + leg.CalServoRad(target_x_m, target_y_m + offsetY); + servo_num = 0; } - else if(flagMotor == 1) - { - servo.set_degree(0, Theta2); - flagMotor = 0; - }*/ + time_s += tickerTime; } - -int main() +int main() { + float target_x_m = 0; + float target_y_m = 0; + F_t(0, target_x_m, target_y_m); + leg.CalServoRad(target_x_m, target_y_m + offsetY); fliper.attach(&cb_timer, tickerTime); }