Rong Syuan Lin
20181116
Diff: main.cpp
- Revision:
- 5:4dbed091ec5a
- Parent:
- 4:f19e7669d1b5
- Child:
- 6:f48c51662e27
--- a/main.cpp Fri Aug 10 13:29:54 2018 +0000
+++ b/main.cpp Fri Aug 10 18:09:00 2018 +0000
@@ -58,7 +58,7 @@
int i = 0;
// PID
-PID motor_pid(7.2, 0, 0, Ts);// 6.4 0.13 7.2 0.13 4.8, 0.568, 0.142
+PID motor_pid(5, 0, 0, Ts);// 6.4 0.13 7.2 0.13 4.8, 0.568, 0.142
float PIDout = 0.0f;
// Dynamixel
@@ -82,7 +82,7 @@
//float friction = 0.0f;
//float check = 0.0f;
float Angle_Dif;
-
+short rotation_;
// function
void init_UART();
void init_TIMER();
@@ -114,88 +114,7 @@
init_TIMER();
while(1) {
- if (flag==1) {
- test = 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;
- 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;
- }
-
- // EMG
- emg_value = EMG.read();
- emg_value_f = lpf(emg_value,emg_value_f_old,15);
- emg_value_f_old = emg_value_f;
-
-// // AnalogOut
-// if (torque_measured*10 > 3.3) {
-// torque_measured = 0.33;
-// } else {
-// test2 = torque_measured*10;
-// }
- // IMU
-// imu.readAccel();
-// imu.readGyro();
-// imu2.readAccel();
-// imu2.readGyro();
- /*
- Acce_axis_data[0] = imu.ax*Acce_gain_x;
- Acce_axis_data[1] = imu.ay*Acce_gain_y;
- Acce_axis_data[2] = imu.az*Acce_gain_z;
- Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
- Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
- Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
-
- Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
- Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
- Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
- Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
- Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
- Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
-
- for(i=0; i<6; i++) {
- Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
- Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
- Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
- Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
- }
-
-
-
-
-
-
-
-
-
- */
- dynamixelClass.torque(SERVO_ID, servo_cmd);
- wait_ms(1);
- uart_tx();
- flag = 0;
- wait_ms(1);
- test = 0;
- }
}
}
@@ -207,7 +126,9 @@
void init_DYNAMIXEL()
{
- dynamixelClass.torqueMode(SERVO_ID, 1);
+// dynamixelClass.torqueMode(SERVO_ID, 0);
+ dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
+// dynamixelClass.setPID(SERVO_ID, 1, 0, 0);
wait_ms(1);
}
@@ -223,7 +144,86 @@
void timer1_ITR()
{
- flag = 1;
+ test = 1;
+ led2 = !led2;
+ angle_measure();
+ D_angle_measure();
+ find_torque();
+ motor_pid.Compute(torque_ref, torque_measured);
+ PIDout = motor_pid.output;
+ servo_cmd = -PIDout*22.73f; // 1023/45rpm = 22.73
+
+ if (servo_cmd > 0) {
+// servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*22.73f;
+ servo_cmd = servo_cmd;
+ rotation_ = 0;// 0：Move Left
+ if (servo_cmd >= 1023)
+ servo_cmd = 1023;
+ } else {
+// servo_cmd = -servo_cmd + 1024 + ((torque_ref)*rate+friction)*22.73f;
+// servo_cmd = -servo_cmd+1024;
+ servo_cmd = -servo_cmd;
+ rotation_ = 1;// 1：Move Right
+// if (servo_cmd >= 2047)
+// servo_cmd = 2047;
+ if (servo_cmd >= 1023)
+ servo_cmd = 1023;
+ }
+
+ row_cmd = servo_cmd;
+
+// if (servo_cmd > 1023) {
+// row_cmd = -(servo_cmd-1023);
+// } else {
+// row_cmd = servo_cmd;
+// }
+
+ // EMG
+ emg_value = EMG.read();
+ emg_value_f = lpf(emg_value,emg_value_f_old,15);
+ emg_value_f_old = emg_value_f;
+
+// // AnalogOut
+// if (torque_measured*10 > 3.3) {
+// torque_measured = 0.33;
+// } else {
+// test2 = torque_measured*10;
+// }
+ // IMU
+// imu.readAccel();
+// imu.readGyro();
+// imu2.readAccel();
+// imu2.readGyro();
+ /*
+ Acce_axis_data[0] = imu.ax*Acce_gain_x;
+ Acce_axis_data[1] = imu.ay*Acce_gain_y;
+ Acce_axis_data[2] = imu.az*Acce_gain_z;
+ Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
+ Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
+ Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
+
+ Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
+ Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
+ Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
+ Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
+ Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
+ Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
+
+ for(i=0; i<6; i++) {
+ Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
+ Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
+ Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);
+ Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
+ }
+
+ */
+// dynamixelClass.torque(SERVO_ID, servo_cmd);
+ dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd); //0~1023
+ wait_ms(1);
+ uart_tx();
+ flag = 0;
+ wait_ms(1);
+ test = 0;
}
void uart_tx()
@@ -236,13 +236,13 @@
splitter s6;
splitter s7;
- s1.j = _angle_difference*100;
- s2.j = D_angle;
+ s1.j = Angle_Dif;
+ s2.j = row_cmd;
// s2.j = 1;
- s3.j = row_cmd;
+ s3.j = servo_cmd;
s4.j = D_Angle;
- s5.j = Angle;
- s6.j = Angle_Dif;
+ s5.j = D_angle;
+ s6.j = Angle;
s7.j = 1;
T[2] = s1.C[0];
@@ -295,10 +295,10 @@
_angle_difference = Angle_Dif/4096*2*_PI;
// if ((_angle_difference < 0) && (D_angle < 0)) {
- if (_angle_difference < -99) {
+ if (_angle_difference < -99) {
_angle_difference = _angle_difference-100.54;
}
- if (_angle_difference > 99) {
+ if (_angle_difference > 99) {
_angle_difference = _angle_difference-100.54;
}
torque_measured = _angle_difference*ks;