Revision 6:f1c0b9476c88, committed 2018-11-23

Comitter:

nylon0212

Date:

Fri Nov 23 04:43:52 2018 +0000

Parent:

5:131450b16ce3

Commit message:

20181123

Changed in this revision

diff -r 131450b16ce3 -r f1c0b9476c88 LSM9DS1.cpp
--- a/LSM9DS1.cpp	Fri Aug 31 01:41:04 2018 +0000
+++ b/LSM9DS1.cpp	Fri Nov 23 04:43:52 2018 +0000
@@ -47,18 +47,18 @@
     
     // Gyro initialization stuff:
     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
-    setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
-    setGyroScale(gScale); // Set the gyro range
+//    setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
+//    setGyroScale(gScale); // Set the gyro range
     
     // Accelerometer initialization stuff:
     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
-    setAccelODR(aODR); // Set the accel data rate.
-    setAccelScale(aScale); // Set the accel range.
+//    setAccelODR(aODR); // Set the accel data rate.
+//    setAccelScale(aScale); // Set the accel range.
     
     // Magnetometer initialization stuff:
     initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
-    setMagODR(mODR); // Set the magnetometer output data rate.
-    setMagScale(mScale); // Set the magnetometer's range.
+//    setMagODR(mODR); // Set the magnetometer output data rate.
+//    setMagScale(mScale); // Set the magnetometer's range.
     
     // Once everything is initialized, return the WHO_AM_I registers we read:
     return (xgTest << 8) | mTest;
@@ -114,7 +114,7 @@
     // Write the address we are going to read from and don't end the transaction
     i2c.write(xgAddress, &subAddress, 1, true);
     // Read in all 8 bit registers containing the axes data
-    i2c.read(xgAddress, data, 6);
+    i2c.read(xgAddress, data, 6, false);
 
     // Reassemble the data and convert to g
     ax_raw = data[0] | (data[1] << 8);
@@ -365,7 +365,7 @@
     switch (aScale)
     {
         case A_SCALE_2G:
-            aRes = 2.0 / 32768.0;
+            aRes = 0.061e-3;
             break;
         case A_SCALE_4G:
             aRes = 4.0 / 32768.0;

diff -r 131450b16ce3 -r f1c0b9476c88 PID.lib
--- a/PID.lib	Fri Aug 31 01:41:04 2018 +0000
+++ b/PID.lib	Fri Nov 23 04:43:52 2018 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/LDSC_Robotics_TAs/code/PID/#4df4895863cd
+https://os.mbed.com/users/nylon0212/code/PID/#d2c5f7d65d0d

diff -r 131450b16ce3 -r f1c0b9476c88 main.cpp
--- a/main.cpp	Fri Aug 31 01:41:04 2018 +0000
+++ b/main.cpp	Fri Nov 23 04:43:52 2018 +0000
@@ -1,7 +1,8 @@
-#include "mbed.h"
+#include "mbed.h"       //不用gyro的data，只要輸出兩個imu的acce的xyz資料即可
 #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 
@@ -12,32 +13,6 @@
 #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
-//***************************************************************
-
-Serial uart(USBTX, USBRX);
-//Serial uart(D10,D2);            // TX : D10     RX : D2           // blueteeth
-DigitalOut LED(A4);            // check if the code is running
-DigitalOut led2(A5);            // check if the code is running interrupt
-uint8_t led2f;
-
-// Timer
-Ticker timer1;
-float ITR_time1 = 4000.0;  // unit：us
-float Ts = ITR_time1/1000000;
-uint8_t flag;
-
-// uart_tx
-union splitter {
-    short j;
-    char C[2];
-    // C[0] is lowbyte of j, C[1] is highbyte of j
-};
-char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
-int i = 0;
-
-// PID
-PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13   7.2 0.13     8.4       6.5, 0, 0.19
-float PIDout = 0.0f;
 
 // Dynamixel
 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
@@ -53,7 +28,7 @@
 float torque_measured = 0.0;
 float ks = 2.6393*2;  //spring constant
 //int angle_dif = 0;
-float torque_ref = 0.0;
+float torque_ref = 0.0; //可不令為0，而是以范僑芯的學習結果為input。令為0可使馬達扭力輸出為0，抵抗減速機摩擦力，使穿戴者感到跟沒有戴一樣。
 //float friction = 0.0f;
 float friction = 0.18f;
 float rate = 0.5;
@@ -61,17 +36,61 @@
 //float check = 0.0f;
 float Angle_Dif;
 short rotation_;
+//***************************************************************
+
+Serial uart(USBTX, USBRX);
+//Serial uart(D10,D2);            // TX : D10     RX : D2           // bluetooth
+AnalogIn EMG(PC_4);            //
+DigitalOut LED(A4);            // check if the code is running
+DigitalOut led2(A5);            // check if the code is running interrupt
+uint8_t led2f;
+/*IMU*******************************************************************/
+LSM9DS1 imu(D14, D15);    //SDA SCL
+LSM9DS1 imu2(PC_9,PA_8);   //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};     //_f代表經過低通濾波的資料
+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;  //timer interrupt unit：us  多少us計時一次 4毫秒
+float Ts = ITR_time1/1000000;    //控馬達的某個時間參數 不用理他
+
+// PID
+PID motor_pid(1500, 0, 40, Ts);// 6.4 0.13   7.2 0.13     8.4       6.5, 0, 0.19
+float PIDout = 0.0f;
+
+// EMG
+float lpf(float input, float output_old, float frequency);  //low pass filter
+float emg_value;
+float emg_value_f;
+float emg_value_f_old;
+float Tf = ITR_time1/1000000;     // 低通濾波的採樣週期
+
+// uart_tx
+union splitter {   //將data切割為兩個byte
+    short j;
+    char C[2];
+    // C[0] is lowbyte of j, C[1] is highbyte of j
+};
+uint8_t T[40] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+// T[16]中的16是2+14，2為start btye 255 255，14為要傳7個參數到電腦，拆成high byte和low byte，所以是7*2=14
+// 若要更改傳到電腦的參數個數，則改x*2=y，x為要傳的參數個數，y為char T[]裡面要有多少個0
+int i = 0;
 
 // function
 void init_UART();
+void uart_tx(); // 傳data到電腦
 void init_TIMER();
 void timer1_ITR();
-void uart_tx();
-
 void init_DYNAMIXEL();
 void D_angle_measure();
 void find_torque();
-
+/**************************************************************************/
 int main()
 {
     LED = 1; // darken
@@ -79,27 +98,73 @@
     // initial sensor
     init_SPI_encoder();
     init_encoder();
+    init_UART();
     init_DYNAMIXEL();
-    // initial uart
-    init_UART();
-
-    wait_ms(500);
-
-    led2 = 1;
-//    led2f = 0;
-    LED = 0; // lighten
-
+    init_IMU();
     init_TIMER();
 
     while(1) {
+        // EMG
+        //emg_value = EMG.read();
+//    emg_value_f = lpf(emg_value,emg_value_f_old,15);
+//    emg_value_f_old = emg_value_f;
 
+        // 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];
     }
+//        int16_t shit2 = imu.ax_raw;
+//        uart.printf("%d\n\r", shit2);
+//    uart.printf("x: %.4f, y: %.4f, z: %.4f\n\r", imu.ax, imu.ay, imu.az);
+    uart_tx();
+    init_TIMER();
+    while(1) {}
+
+//        wait_ms(500);
+    }
+}
+
+void init_IMU()
+{
+    uint16_t shit;
+    shit = imu.begin(imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625); //imu.G_SCALE_245DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_238_BW_14,imu.A_ODR_119,imu.M_ODR_0625
+//    imu2.begin(imu.G_SCALE_500DPS,imu.A_SCALE_2G,imu.M_SCALE_4GS,imu.G_ODR_476_BW_21,imu.A_ODR_476,imu.M_ODR_0625);
+    if(shit != 0x683D) {
+        while(1) {
+            uart.printf("IMU error\n\r");
+        }
+    }
+    uart.printf("IMU init success\n\r");
+
+//    uart.printf("%d\n\r", shit);
 }
 
 void init_DYNAMIXEL()
 {
 //    dynamixelClass.torqueMode(SERVO_ID, 1);
-    dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
+    dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);  //Wheel:= continuously rotating; others: torque, position
     wait_ms(1);
 }
 
@@ -113,7 +178,7 @@
     timer1.attach_us(&timer1_ITR, ITR_time1);
 }
 
-void timer1_ITR()
+void timer1_ITR()    //開始讀取資料
 {
     led2 = !led2;
     angle_measure();
@@ -121,28 +186,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;   // 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;
-    }
-*/
+    servo_cmd = -PIDout;   // 1023/8.4Nm = 121.7857 可視情況調整負號
     // 速度控制
         if (servo_cmd > 0) {
             row_cmd = servo_cmd;
@@ -165,12 +209,12 @@
         }
     
 //    dynamixelClass.torque(SERVO_ID, servo_cmd);
-    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023   (rotation)
+    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023   (rotation) 12V之下的最高轉速為45rpm，切割為等分
 //    dynamixelClass.wheel(SERVO_ID, 0, 150);  //0~1023      (rotation)
     uart_tx();
 }
 
-void uart_tx()
+void uart_tx()    //分割資料
 {
     splitter s1;
     splitter s2;
@@ -179,17 +223,38 @@
     splitter s5;
     splitter s6;
     splitter s7;
+    splitter s8;
+    splitter s9;
+    splitter s10;
+    splitter s11;
+    splitter s12;
+    splitter s13;
+    splitter s14;
+    splitter s15;
+    splitter s16;
+    splitter s17;
+    splitter s18;
+    splitter s19;
 
-    s1.j = torque_ref*1000;
-    s2.j = torque_measured*1000;
-    s3.j = Angle_Dif/4096*3600;
-//    s3.j = servo_cmd;
-//    s4.j = 1;
-//    s5.j = 3;
-    s4.j = D_Angle;
-    s5.j = Angle*3;
-    s6.j = row_cmd;
-    s7.j = 1;
+    s1.j = Acce_axis_data_f[0];  //  0x6161;//
+    s2.j = Acce_axis_data_f[1];
+    s3.j = Acce_axis_data_f[2];
+    s4.j = Acce_axis_data_f[3];
+    s5.j = Acce_axis_data_f[4];
+    s6.j = Acce_axis_data_f[5];
+    s7.j = Gyro_axis_data_f[0];
+    s8.j = Gyro_axis_data_f[1];
+    s9.j = Gyro_axis_data_f[2];
+    s10.j = Gyro_axis_data_f[3];
+    s11.j = Gyro_axis_data_f[4];
+    s12.j = Gyro_axis_data_f[5];
+    s13.j = emg_value_f;
+    s14.j = torque_ref*1000; //即reference input指令
+    s15.j = torque_measured*1000; //由angle_difference*ks而來
+    s16.j = Angle_Dif/4096*3600;
+    s17.j = D_Angle;
+    s18.j = Angle*3; //經過減速比
+    s19.j = row_cmd;
 
     T[2] = s1.C[0];
     T[3] = s1.C[1];
@@ -205,19 +270,46 @@
     T[13] = s6.C[1];
     T[14] = s7.C[0];
     T[15] = s7.C[1];
+    T[16] = s8.C[0];
+    T[17] = s8.C[1];
+    T[18] = s9.C[0];
+    T[19] = s9.C[1];
+    T[20] = s10.C[0];
+    T[21] = s10.C[1];
+    T[22] = s11.C[0];
+    T[23] = s11.C[1];
+    T[24] = s12.C[0];
+    T[25] = s12.C[1];
+    T[26] = s13.C[0];
+    T[27] = s13.C[1];
+    T[28] = s14.C[0];
+    T[29] = s14.C[1];
+    T[30] = s15.C[0];
+    T[31] = s15.C[1];
+    T[32] = s16.C[0];
+    T[33] = s16.C[1];
+    T[34] = s17.C[0];
+    T[35] = s17.C[1];
+    T[36] = s18.C[0];
+    T[37] = s18.C[1];
+    T[38] = s19.C[0];
+    T[39] = s19.C[1];
 
-    while(1) {
-        if (uart.writeable() == 1) {
-            uart.putc(T[i]);
-            i++;
-        }
-        if (i >= sizeof(T)) {
-            i = 0;
-            break;
-        }
+    for(int k=0;k<40;k++)
+    {
+        uart.putc(T[k]);
     }
 }
 
+float lpf(float input, float output_old, float frequency)
+{
+    float output = 0;
+
+    output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
+
+    return output;
+}
+
 void D_angle_measure()
 {
     D_angle = dynamixelClass.readPosition(SERVO_ID);
@@ -251,3 +343,4 @@
 
     torque_measured = angle_difference*ks;
 }
+