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)