![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
20181123
Diff: main.cpp
- Revision:
- 1:2823a39f70a9
- Parent:
- 0:c23e915f255b
- Child:
- 2:f630aff31d27
--- a/main.cpp Sun Aug 05 13:15:56 2018 +0000 +++ b/main.cpp Tue Aug 07 08:47:59 2018 +0000 @@ -22,7 +22,7 @@ // Timer Ticker timer1; -float ITR_time1 = 10000.0; // unit:us +float ITR_time1 = 4000.0; // unit:us float Ts = ITR_time1/1000000; uint8_t flag; @@ -42,6 +42,7 @@ // Dynamixel DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); int servo_cmd; +int row_cmd; int D_angle = 0; int D_angle_dif = 0; int D_Angle; @@ -53,10 +54,12 @@ float ks = 2.6393*2; //spring constant //int angle_dif = 0; float torque_ref = 0.0; +//float friction = 0.0f; float friction = 0.18f; -float rate = 0.8; +float rate = 0.5; //float friction = 0.0f; //float check = 0.0f; +float Angle_Dif; // function void init_UART(); @@ -82,7 +85,7 @@ wait_ms(500); led2 = 1; - led2f = 0; +// led2f = 0; LED = 0; // lighten init_TIMER(); @@ -95,7 +98,7 @@ 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 if (servo_cmd > 0) { servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f; @@ -106,7 +109,14 @@ if (servo_cmd >= 2047) servo_cmd = 2047; } -// dynamixelClass.torque(SERVO_ID, servo_cmd); + + if (servo_cmd >= 1023) { + row_cmd = -(servo_cmd-1023); + } else { + row_cmd = servo_cmd; + } + +// dynamixelClass.torque(SERVO_ID, servo_cmd); uart_tx(); flag = 0; } @@ -132,12 +142,12 @@ void timer1_ITR() { flag = 1; - if (led2f == 5) { - - led2f = 0; - } else { - led2f++; - } +// if (led2f == 5) { +// +// led2f = 0; +// } else { +// led2f++; +// } } void uart_tx() @@ -150,15 +160,16 @@ splitter s6; splitter s7; - s1.j = angle; + s1.j = 1; s2.j = Angle; - s3.j = 2; + s3.j = D_Angle; +// s3.j = servo_cmd; // s4.j = 1; // s5.j = 3; - s4.j = D_angle; - s5.j = D_Angle; - s6.j = torque_measured*1000; - s7.j = 3; + s4.j = angle_difference; + s5.j = torque_measured*1000; + s6.j = row_cmd; + s7.j = 1; T[2] = s1.C[0]; T[3] = s1.C[1]; @@ -180,7 +191,7 @@ uart.putc(T[i]); i++; } - if (i >= (sizeof(T)-1)) { + if (i >= sizeof(T)) { i = 0; break; } @@ -204,6 +215,10 @@ void find_torque() { + +// Angle_Dif = Angle*3-D_Angle; angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI; +// angle_difference = Angle_Dif/4096.0f*2*PI; + torque_measured = angle_difference*ks; -} \ No newline at end of file +}