![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20181123
Diff: main.cpp
- Revision:
- 3:c05acc05c3bd
- Parent:
- 2:f630aff31d27
- Child:
- 4:a59512fe0f9a
--- a/main.cpp Tue Aug 07 12:01:03 2018 +0000 +++ b/main.cpp Fri Aug 24 02:47:02 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 +PID motor_pid(10, 0, 0, Ts);// 6.4 0.13 7.2 0.13 float PIDout = 0.0f; // Dynamixel @@ -91,41 +91,14 @@ init_TIMER(); while(1) { - if (flag==1) { - led2 = !led2; - angle_measure(); - D_angle_measure(); - find_torque(); - motor_pid.Compute(torque_ref, torque_measured); - 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; - if (servo_cmd >= 1023) - servo_cmd = 1023; - } else { - servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; - if (servo_cmd >= 2047) - servo_cmd = 2047; - } - - if (servo_cmd >= 1023) { - row_cmd = -(servo_cmd-1023); - } else { - row_cmd = servo_cmd; - } - - dynamixelClass.torque(SERVO_ID, servo_cmd); - uart_tx(); - flag = 0; - } } } void init_DYNAMIXEL() { dynamixelClass.torqueMode(SERVO_ID, 1); +// dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0); wait_ms(1); } @@ -141,13 +114,35 @@ void timer1_ITR() { - flag = 1; -// if (led2f == 5) { -// -// led2f = 0; -// } else { -// led2f++; -// } + led2 = !led2; + angle_measure(); + D_angle_measure(); + find_torque(); + motor_pid.Compute(torque_ref, torque_measured); + 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; + if (servo_cmd >= 1023) + servo_cmd = 1023; + } else { +// servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*121.8f; + servo_cmd = -servo_cmd + 1024; + if (servo_cmd >= 2047) + servo_cmd = 2047; + } + + if (servo_cmd >= 1023) { + row_cmd = -(servo_cmd-1023); + } else { + row_cmd = servo_cmd; + } + + dynamixelClass.torque(SERVO_ID, servo_cmd); +// dynamixelClass.wheel(SERVO_ID, 0, 150); //0~1023 (rotation) + uart_tx(); } void uart_tx() @@ -160,13 +155,13 @@ splitter s6; splitter s7; - s1.j = 1; - s2.j = Angle; - s3.j = D_Angle; + s1.j = D_Angle; + s2.j = Angle*3; + s3.j = Angle_Dif*360/4096; // s3.j = servo_cmd; // s4.j = 1; // s5.j = 3; - s4.j = (Angle*3-D_Angle)*360/4096; + s4.j = D_angle; s5.j = torque_measured*1000; s6.j = row_cmd; s7.j = 1; @@ -208,6 +203,15 @@ d++; } else { D_angle_dif = D_angle - D_angle_old; + + if (D_angle_dif > 4096/2) { + D_angle_dif = -(4096-(D_angle_dif)); + } else if (D_angle_dif < -4096/2) { + D_angle_dif = -(-4096-(D_angle_dif)); + } else { + D_angle_dif = D_angle_dif; + } + D_Angle = D_Angle + D_angle_dif; D_angle_old = D_angle; } @@ -216,8 +220,8 @@ void find_torque() { -// Angle_Dif = Angle*3-D_Angle; - angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI; + Angle_Dif = Angle*3-D_Angle; + angle_difference = Angle_Dif/4096.0f*2*PI; // angle_difference = Angle_Dif/4096.0f*2*PI; torque_measured = angle_difference*ks;