Rong Syuan Lin
20181116
Diff: main.cpp
- Revision:
- 4:f19e7669d1b5
- Parent:
- 3:98cdee5d9b63
- Child:
- 5:4dbed091ec5a
--- a/main.cpp Tue Aug 07 14:20:09 2018 +0000
+++ b/main.cpp Fri Aug 10 13:29:54 2018 +0000
@@ -2,6 +2,7 @@
#include "encoder.h"
#include "Mx28.h"
#include "PID.h"
+#include "LSM9DS1.h"
//********************* Dynamxiel ******************************
#define SERVO_ID 0x01 // ID of which we will set Dynamixel too
@@ -11,7 +12,7 @@
#define RxPin A1
#define CW_LIMIT_ANGLE 1 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
#define CCW_LIMIT_ANGLE 4095 // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
-#define PI 3.14159265f
+#define _PI 3.14159265f
//***************************************************************
Serial uart(USBTX, USBRX);
@@ -20,10 +21,23 @@
DigitalOut led2(A5); // check if the code is running interrupt
uint8_t led2f;
AnalogIn EMG(PC_4);
+DigitalOut test(PC_8);
+//AnalogOut test2(PA_5);
+// IMU
+LSM9DS1 imu(D14, D15); //SDA SCL
+LSM9DS1 imu2(PC_9, D7); //SDA SCL
+void init_IMU();
+int16_t Gyro_axis_data[6] = {0}; // X, Y, Z axis
+int16_t Acce_axis_data[6] = {0}; // X, Y, Z axis
+float Acce_axis_data_f[6] = {0};
+float Acce_axis_data_f_old[6] = {0};
+float Gyro_axis_data_f[6] = {0};
+float Gyro_axis_data_f_old[6] = {0};
+
// Timer
Ticker timer1;
-float ITR_time1 = 4000.0; // unit：us
+float ITR_time1 = 6000.0; // unit：us
float Ts = ITR_time1/1000000;
uint8_t flag;
@@ -32,7 +46,7 @@
float emg_value;
float emg_value_f;
float emg_value_f_old;
-float Tf = ITR_time1*0.000001;
+float Tf = ITR_time1*0.000001f;
// uart_tx
union splitter {
@@ -44,7 +58,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(7.2, 0, 0, Ts);// 6.4 0.13 7.2 0.13 4.8, 0.568, 0.142
float PIDout = 0.0f;
// Dynamixel
@@ -57,14 +71,14 @@
int D_angle_old;
unsigned short d = 0;
// Find Torque
-double angle_difference = 0.0;
-float torque_measured = 0.0;
+double _angle_difference = 0.0;
+double torque_measured = 0.0;
float ks = 2.6393*2; //spring constant
//int angle_dif = 0;
-float torque_ref = 0.0;
+float torque_ref = 0;
//float friction = 0.0f;
-float friction = 0.18f;
-float rate = 0.5;
+float friction = 0.0f;
+float rate = 0.8;
//float friction = 0.0f;
//float check = 0.0f;
float Angle_Dif;
@@ -86,6 +100,7 @@
// initial sensor
init_SPI_encoder();
init_encoder();
+ init_IMU();
init_DYNAMIXEL();
// initial uart
init_UART();
@@ -100,25 +115,28 @@
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
+ 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;
}
- if (servo_cmd >= 1023) {
+ if (servo_cmd > 1023) {
row_cmd = -(servo_cmd-1023);
} else {
row_cmd = servo_cmd;
@@ -128,13 +146,65 @@
emg_value = EMG.read();
emg_value_f = lpf(emg_value,emg_value_f_old,15);
emg_value_f_old = emg_value_f;
-// dynamixelClass.torque(SERVO_ID, servo_cmd);
+
+// // 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;
}
}
}
+void init_IMU()
+{
+ imu.begin();
+ imu2.begin();
+}
+
void init_DYNAMIXEL()
{
dynamixelClass.torqueMode(SERVO_ID, 1);
@@ -154,12 +224,6 @@
void timer1_ITR()
{
flag = 1;
-// if (led2f == 5) {
-//
-// led2f = 0;
-// } else {
-// led2f++;
-// }
}
void uart_tx()
@@ -172,15 +236,13 @@
splitter s6;
splitter s7;
- s1.j = 1;
- s2.j = Angle;
- s3.j = D_Angle;
-// s3.j = servo_cmd;
-// s4.j = 1;
-// s5.j = 3;
- s4.j = (Angle*3-D_Angle)*360/4096;
- s5.j = emg_value_f*100;
- s6.j = row_cmd;
+ s1.j = _angle_difference*100;
+ s2.j = D_angle;
+// s2.j = 1;
+ s3.j = row_cmd;
+ s4.j = D_Angle;
+ s5.j = Angle;
+ s6.j = Angle_Dif;
s7.j = 1;
T[2] = s1.C[0];
@@ -228,11 +290,19 @@
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;
+ Angle_Dif = (-Angle*3+D_Angle);
+// _angle_difference = (Angle*3-D_Angle)/4096.0f*2*_PI;
- torque_measured = angle_difference*ks;
+ _angle_difference = Angle_Dif/4096*2*_PI;
+// if ((_angle_difference < 0) && (D_angle < 0)) {
+ if (_angle_difference < -99) {
+ _angle_difference = _angle_difference-100.54;
+ }
+ if (_angle_difference > 99) {
+ _angle_difference = _angle_difference-100.54;
+ }
+ torque_measured = _angle_difference*ks;
+// torque_measured = Angle_Dif;
}
float lpf(float input, float output_old, float frequency)