Rong Syuan Lin
20181123
Diff: main.cpp
- Revision:
- 6:f1c0b9476c88
- Parent:
- 5:131450b16ce3
--- 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;
}
+