![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20181123
Diff: main.cpp
- Revision:
- 4:a59512fe0f9a
- Parent:
- 3:c05acc05c3bd
- Child:
- 5:131450b16ce3
--- a/main.cpp Fri Aug 24 02:47:02 2018 +0000 +++ b/main.cpp Mon Aug 27 15:36:28 2018 +0000 @@ -36,7 +36,7 @@ int i = 0; // PID -PID motor_pid(10, 0, 0, Ts);// 6.4 0.13 7.2 0.13 +PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19 float PIDout = 0.0f; // Dynamixel @@ -60,6 +60,7 @@ //float friction = 0.0f; //float check = 0.0f; float Angle_Dif; +short rotation_; // function void init_UART(); @@ -122,13 +123,14 @@ PIDout = motor_pid.output; servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857 + // 電流控制 if (servo_cmd > 0) { -// servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; + // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; servo_cmd = servo_cmd; if (servo_cmd >= 1023) servo_cmd = 1023; } else { -// servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; + // servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; servo_cmd = -servo_cmd + 1024; if (servo_cmd >= 2047) servo_cmd = 2047; @@ -139,8 +141,30 @@ } else { row_cmd = servo_cmd; } - + /* + // 速度控制 + if (servo_cmd > 0) { + row_cmd = servo_cmd; + // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.78f; + servo_cmd = servo_cmd; + rotation_ = 0; // 0:Move Left + if (servo_cmd >= 1023) { + servo_cmd = 1023; + row_cmd = servo_cmd; + } + } else { + row_cmd = servo_cmd; + // servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f; + servo_cmd = -servo_cmd; + rotation_ = 1; // 1:Move Right + if (servo_cmd >= 1023) { + servo_cmd = 1023; + row_cmd = -servo_cmd; + } + } + */ dynamixelClass.torque(SERVO_ID, servo_cmd); +// dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation) // dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation) uart_tx(); } @@ -155,14 +179,14 @@ splitter s6; splitter s7; - s1.j = D_Angle; - s2.j = Angle*3; - s3.j = Angle_Dif*360/4096; + s1.j = torque_ref*1000; + s2.j = torque_measured*1000; + s3.j = Angle_Dif/4096*3600; // s3.j = servo_cmd; // s4.j = 1; // s5.j = 3; s4.j = D_angle; - s5.j = torque_measured*1000; + s5.j = Angle*3; s6.j = row_cmd; s7.j = 1;