Diff: main.cpp

Revision:

9:07de3af99031

Parent:

8:9c3b291b3288

--- a/main.cpp	Tue Aug 14 06:22:27 2018 +0000
+++ b/main.cpp	Mon Nov 05 09:29:20 2018 +0000
@@ -1,7 +1,9 @@
-#include "mbed.h"
+#define DEBUG_STDIO 1
+
+#include "mbed.h"       //不用gyro的data，只要輸出兩個imu的acce的xyz資料即可
 #include "encoder.h"
-#include "Mx28.h"
-#include "PID.h"
+//#include "Mx28.h"
+//#include "PID.h"
 #include "LSM9DS1.h"
 
 //********************* Dynamxiel ******************************
@@ -16,20 +18,21 @@
 //***************************************************************
 
 Serial uart(USBTX, USBRX);
-//Serial uart(D10,D2);            // TX : D10     RX : D2           // blueteeth
+//Serial uart(D10,D2);            // TX : D10     RX : D2           // bluetooth
 DigitalOut LED(A4);            // check if the code is running
-DigitalOut led2(A5);            // check if the code is running interrupt
-uint8_t led2f;
-AnalogIn EMG(PC_4);
+DigitalOut led2(PA_5);            // check if the code is running interrupt
+uint8_t led2f;                 //測試用
+AnalogIn EMG(PC_4);            //肌電訊號
 DigitalOut test(PC_8);
+DigitalOut CS(PA_8);
 //AnalogOut test2(PA_5);
-// IMU
+/*IMU*******************************************************************/
 LSM9DS1 imu(D14, D15);    //SDA SCL
-LSM9DS1 imu2(PC_9, D7);   //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};
+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};
@@ -37,99 +40,60 @@
 
 // Timer
 Ticker timer1;
-float ITR_time1 = 6000.0;  // unit：us
-float Ts = ITR_time1/1000000;
+float ITR_time1 = 100.0;  //timer interrupt unit：us  多少us計時一次 10毫秒
+float Ts = ITR_time1/1000000;    //控馬達的某個時間參數 不用理他
 uint8_t flag;
 
 // EMG
-float lpf(float input, float output_old, float frequency);
+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*0.000001f;
+float Tf = ITR_time1/1000000;     // 低通濾波的採樣週期
 
 // uart_tx
-union splitter {
+union splitter {   //將data切割為兩個byte
     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};
+uint8_t T[16] = {255,255,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;
 
-// PID
-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
-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;
-int D_angle_old;
-unsigned short d = 0;
-// Find Torque
-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.0f; //*************************************************
-float friction = 0.0f;
-//float friction = 0.5f;
-float rate = 0.8;
-//float friction = 0.0f;
-//float check = 0.0f;
-float Angle_Dif;
-short rotation_;
 // function
 void init_UART();
 void init_TIMER();
 void timer1_ITR();
-void uart_tx();
-
-void init_DYNAMIXEL();
-void D_angle_measure();
-void find_torque();
-
+void uart_tx(); // 傳data到電腦
+/**************************************************************************/
 int main()
 {
-    LED = 1; // darken
-    wait_ms(500);
+//    LED = 1; // darken
+//    wait_ms(500);
     // initial sensor
-    init_SPI_encoder();
-    init_encoder();
+//    init_SPI_encoder();
+//    init_encoder();
     init_IMU();
-    init_DYNAMIXEL();
     // initial uart
     init_UART();
 
-    wait_ms(500);
+//    wait_ms(500);
 
-    led2 = 1;
+//    led2 = 1;
 //    led2f = 0;
-    LED = 0; // lighten
+//    LED = 0; // lighten
 
     init_TIMER();
 
-    while(1) {
-
-    }
+    while(1) {}
 }
 
 void init_IMU()
 {
-    imu.begin();
-    imu2.begin();
-}
-
-void init_DYNAMIXEL()
-{
-//    dynamixelClass.torqueMode(SERVO_ID, 0);
-    dynamixelClass.setMode(SERVO_ID, WHEEL, 0, 0);
-//    dynamixelClass.setPID(SERVO_ID, 1, 0, 0);
-    wait_ms(1);
+    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
+//    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);
 }
 
 void init_UART()
@@ -142,95 +106,51 @@
     timer1.attach_us(&timer1_ITR, ITR_time1);
 }
 
-void timer1_ITR()
+void timer1_ITR()    //開始讀取資料
 {
-    test = 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.78f;   //  1023/8.4Nm = 121.78
-
-    if (servo_cmd > 0) {
-        row_cmd = servo_cmd;
-        servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.78f;
-//        servo_cmd = servo_cmd;
-        rotation_ = 0;    // 0：Move Left
-        if (servo_cmd >= 1023) {
-            servo_cmd = 1023;
-            row_cmd = servo_cmd;
-        }
-    } else {
-        row_cmd = servo_cmd;
-        servo_cmd = -servo_cmd + ((torque_ref)*rate+friction)*121.78f;
-//        servo_cmd = -servo_cmd;
-        rotation_ = 1;    // 1：Move Right
-        if (servo_cmd >= 1023) {
-            servo_cmd = 1023;
-            row_cmd = -servo_cmd;
-        }
-    }
-
-//    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;
+    //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;
-//            }
+    CS = 1;
     // 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;
+    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;
+    //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];
-                }
+    //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];
+//    }
+    uart.printf("x: %7.4f, y: %7.4f, z: %7.4f\n\r",imu.ax, imu.ay, imu.az);
+    //uart_tx();
 
-    */
-//    dynamixelClass.torque(SERVO_ID, servo_cmd);
-//    dynamixelClass.wheel(SERVO_ID, rotation_, servo_cmd);  //0~1023
-        dynamixelClass.wheel(SERVO_ID, 0, 300);  //0~1023
+//    flag = 0;
 //    wait_ms(1);
-    uart_tx();
-    flag = 0;
-    wait_ms(1);
-    test = 0;
+//    test = 0;
 }
 
-void uart_tx()
+void uart_tx()    //分割資料
 {
     splitter s1;
     splitter s2;
@@ -239,19 +159,19 @@
     splitter s5;
     splitter s6;
     splitter s7;
+//    splitter s8;
 
-    s1.j = D_angle;
-//    s2.j = Angle;
-//    s3.j = Angle_Dif*360/4096;
-    s2.j = D_Angle;
-    s3.j = Angle;
-    s4.j = _angle_difference*100;
-    s5.j = torque_measured*100;
-    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 = emg_value_f;
+//    s8.j = 1; // 若要傳的參數不足8個，則隨意打一個常數即可
 
-    T[2] = s1.C[0];
-    T[3] = s1.C[1];
+    T[2] = Acce_axis_data[0];  //0x5A;
+    T[3] = Acce_axis_data[0] >>8;
     T[4] = s2.C[0];
     T[5] = s2.C[1];
     T[6] = s3.C[0];
@@ -264,67 +184,38 @@
     T[13] = s6.C[1];
     T[14] = s7.C[0];
     T[15] = s7.C[1];
-
-    while(1) {
-        if (uart.writeable() == 1) {
-            uart.putc(T[i]);
-            i++;
-        }
-        if (i >= sizeof(T)) {
-            i = 0;
-            break;
-        }
-    }
-}
-
-void D_angle_measure()
-{
-    D_angle = dynamixelClass.readPosition(SERVO_ID);
-
-    if (d == 0) {
-        D_Angle = 0;
-        D_angle_old = D_angle;
-        d++;
-    } else {
-
-        D_angle_dif = D_angle - D_angle_old;
+    
+    
+    
+    
 
-        if (D_angle_dif > 4096/2) {
-            D_angle_dif = -(4096-(D_angle_dif));
-        } else if (D_angle_dif < -4096/2) {
-            D_angle_dif = -(-4096-(D_angle_dif));
-        } else {
-            D_angle_dif = D_angle_dif;
-        }
-
-        D_Angle = D_Angle + D_angle_dif;
-        D_angle_old = D_angle;
-
-    }
-}
-
-void find_torque()
-{
+//    uart.putc(T[0]);
+//    uart.putc(T[1]);
+//    uart.putc(T[2]);
+//    uart.putc(T[3]);
+//    uart.putc(T[4]);
+//    uart.putc(T[5]);
+//    uart.putc(T[6]);
+//    uart.putc(T[7]);
+//    uart.putc(T[8]);
+//    uart.putc(T[9]);
+//    uart.putc(T[10]);
+//    uart.putc(T[11]);
+//    uart.putc(T[12]);
+//    uart.putc(T[13]);
+//    uart.putc(T[14]);
+//    uart.putc(T[15]);
 
-    Angle_Dif = (-Angle*3+D_Angle);
-//    _angle_difference = (Angle*3-D_Angle)/4096.0f*2*_PI;
-
-    _angle_difference = Angle_Dif/4096*2*_PI;
-//    if ((_angle_difference < 0) && (D_angle < 0)) {
-//    if (_angle_difference < -99) {
-//        _angle_difference = _angle_difference-100.54;
+//    while(1) {    //開始傳到USB
+//        if (uart.writeable() == 1) {
+//            uart.putc(T[i]);
+//            i++;
+//        }
+//        if (i >= sizeof(T)) {
+//            i = 0;
+//            break;
+//        }
 //    }
-//    if (_angle_difference > 99) {
-//        _angle_difference = _angle_difference-100.54;
-//    }
-    /*        if (_angle_difference > 6) {
-                _angle_difference = _angle_difference-6.4;
-            }
-            if (_angle_difference < -6) {
-                _angle_difference = _angle_difference-6.4;
-            }*/
-    torque_measured = _angle_difference*ks;
-//    torque_measured = Angle_Dif;
 }
 
 float lpf(float input, float output_old, float frequency)
@@ -334,4 +225,6 @@
     output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
 
     return output;
-}
\ No newline at end of file
+}
+
+//有估測器有delay，丟資料給機器學習時就不是及時動作，所以之後決定直接丟資料