![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20181123
Diff: main.cpp
- Revision:
- 5:131450b16ce3
- Parent:
- 4:a59512fe0f9a
- Child:
- 6:f1c0b9476c88
--- a/main.cpp Mon Aug 27 15:36:28 2018 +0000 +++ b/main.cpp Fri Aug 31 01:41:04 2018 +0000 @@ -36,7 +36,7 @@ int i = 0; // PID -PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19 +PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13 7.2 0.13 8.4 6.5, 0, 0.19 float PIDout = 0.0f; // Dynamixel @@ -98,8 +98,8 @@ void init_DYNAMIXEL() { - dynamixelClass.torqueMode(SERVO_ID, 1); -// dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); +// dynamixelClass.torqueMode(SERVO_ID, 1); + dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); wait_ms(1); } @@ -121,8 +121,9 @@ find_torque(); motor_pid.Compute(torque_ref, torque_measured); PIDout = motor_pid.output; - servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857 - +// servo_cmd = -PIDout*121.8f; // 1023/8.4Nm = 121.7857 + servo_cmd = -PIDout; // 1023/8.4Nm = 121.7857 +/* // 電流控制 if (servo_cmd > 0) { // servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; @@ -141,11 +142,11 @@ } 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 + friction; servo_cmd = servo_cmd; rotation_ = 0; // 0:Move Left if (servo_cmd >= 1023) { @@ -154,7 +155,7 @@ } } else { row_cmd = servo_cmd; - // servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f; +// servo_cmd = -servo_cmd - friction; servo_cmd = -servo_cmd; rotation_ = 1; // 1:Move Right if (servo_cmd >= 1023) { @@ -162,9 +163,9 @@ row_cmd = -servo_cmd; } } - */ - dynamixelClass.torque(SERVO_ID, servo_cmd); -// dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023 (rotation) + +// 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(); } @@ -185,7 +186,7 @@ // s3.j = servo_cmd; // s4.j = 1; // s5.j = 3; - s4.j = D_angle; + s4.j = D_Angle; s5.j = Angle*3; s6.j = row_cmd; s7.j = 1;