Diff: main.cpp

Revision:

0:6dc8ac1c7c00

Child:

2:bd98f7a4e231

diff -r 000000000000 -r 6dc8ac1c7c00 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 13 04:52:05 2017 +0000
@@ -0,0 +1,258 @@
+#include "mbed.h"
+#include "LSM9DS0.h"
+#include "Mx28.h"
+#include <math.h>
+
+// Dynamixel
+#define SERVO_ID 0x01               // ID of which we will set Dynamixel too 
+#define SERVO_ControlPin A2       // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer.
+#define SERVO_SET_Baudrate 1000000  // Baud rate speed which the Dynamixel will be set too (1Mbps)
+#define TxPin A0
+#define RxPin A1
+#define CW_LIMIT_ANGLE 0x001        // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
+#define CCW_LIMIT_ANGLE 0xFFF       // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
+
+#define pi 3.14
+#define Ts 5                          // sampling time
+#define A 20                         // angle
+#define f 0.2                        // Hz
+
+unsigned long t = 0;
+double deg = 0;
+int Pos = 0;
+int zero = 190;
+char mode = 'a';
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
+//
+DigitalIn mybutton(USER_BUTTON);
+
+// sampling time Lowpass Filter
+float T = 0.001;
+
+// Interrupt
+Ticker timer1;
+
+// UART
+Serial uart_1(D10,D2);            // TX : D10     RX : D2           // serial 1
+
+//
+void init_uart(void);
+void separate(void);
+void uart_send(void);
+void init_TIMER(void);
+void timer1_interrupt(void);
+void change_mode();
+
+float lpf(float input, float output_old, float frequency);
+
+// uart send data
+int display[6] = {0,0,0,0,0,0};
+char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0};      // char num[0] , num[1] : 2 start byte
+
+void init_sensor(void);
+void read_sensor(void);
+void sensor_filter(void);
+
+LSM9DS0 sensor(SPI_MODE, D9, D6);    // SPI_CS1 : D7 , SPI_CS2 : D6
+
+int sensor_flag = 0;                 // sensor initial flag
+int sensor_count = 0;
+
+// sensor data
+int16_t Gyro_axis_data[3] = {0};     // X, Y, Z axis
+int16_t Acce_axis_data[3] = {0};     // X, Y, Z axis
+
+// sensor gain
+#define Gyro_gain_x   0        
+#define Gyro_gain_y   0           
+#define Gyro_gain_z   0            // datasheet : 70 mdeg/digit
+#define Acce_gain_x   0                
+#define Acce_gain_y   0                  
+#define Acce_gain_z   0     
+
+// sensor filter correction data
+float Gyro_axis_data_f[3];
+float Gyro_axis_data_f_old[3];
+float Gyro_scale[3];                          // scale = (data - zero) * gain
+float Gyro_axis_zero[3] = {0,0,0};            
+
+float Acce_axis_data_f[3];
+float Acce_axis_data_f_old[3];
+float Acce_scale[3];                          // scale = (data - zero) * gain
+float Acce_axis_zero[3] = {0,0,0};             
+
+int main()
+{
+    pc.baud(115200);
+    dynamixelClass.setMode(SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE);    // set mode to SERVO and set angle limits
+    
+    init_uart();
+    init_sensor();
+    init_TIMER();
+    uart_1.attach(&change_mode); 
+           
+    while(1) 
+    {
+        pc.printf("%d\n",Pos);
+    }   // while(1) end
+}
+
+// UART
+void init_uart()
+{
+    uart_1.baud(115200);      // 設定baudrate
+}
+
+int i = 0;
+void uart_send(void)
+{
+    // uart send data
+    display[0] = Gyro_axis_data[0];
+    display[1] = Gyro_axis_data[1];
+    display[2] = Gyro_axis_data[2];
+    display[3] = Acce_axis_data[0];
+    display[4] = Acce_axis_data[1];
+    display[5] = Acce_axis_data[2];
+    separate();
+
+    int j = 0;
+    int k = 1;
+    while(k) 
+    {
+        if(uart_1.writeable()) 
+        {            
+            uart_1.putc(num[i]);
+            i++;
+            j++;
+        }
+
+        if(j>1)                     // send 2 bytes
+        {
+            k = 0;
+            j = 0;
+        }
+    }
+
+    if(i>15)
+        i = 0;
+}
+
+void separate(void)
+{
+    num[2] = display[0] >> 8;           // HSB(8bit)資料存入陣列
+    num[3] = display[0] & 0x00ff;       // LSB(8bit)資料存入陣列
+    num[4] = display[1] >> 8;
+    num[5] = display[1] & 0x00ff;
+    num[6] = display[2] >> 8;
+    num[7] = display[2] & 0x00ff;
+    num[8] = display[3] >> 8;
+    num[9] = display[3] & 0x00ff;
+    num[10] = display[4] >> 8;
+    num[11] = display[4] & 0x00ff;
+    num[12] = display[5] >> 8;
+    num[13] = display[5] & 0x00ff;
+}
+
+// sensor
+void init_sensor(void)
+{
+    sensor.begin();
+}
+
+void read_sensor(void)
+{
+    // sensor raw data
+    Gyro_axis_data[0] = sensor.readRawGyroX();
+    Gyro_axis_data[1] = sensor.readRawGyroY();
+    Gyro_axis_data[2] = sensor.readRawGyroZ();
+
+    Acce_axis_data[0] = sensor.readRawAccelX();
+    Acce_axis_data[1] = sensor.readRawAccelY();
+    Acce_axis_data[2] = sensor.readRawAccelZ();
+}
+
+void sensor_filter(void)
+{
+    // gyro filter
+    Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
+    Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
+    Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
+    Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
+    Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
+    Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
+
+    // acce filter
+    Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
+    Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
+    Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
+    Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
+    Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
+    Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
+
+    Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x;
+    Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y;
+    Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z;
+
+    Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
+    Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
+    Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
+}
+
+// Timer
+void init_TIMER(void)
+{
+    timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
+} 
+
+void timer1_interrupt(void)
+{
+    // sensor initial start
+    if(sensor_flag == 0) {
+        sensor_count++;
+
+        if(sensor_count >= 50) {
+            sensor_flag  = 1;
+            sensor_count = 0;
+        }
+    } else {
+        read_sensor();
+        sensor_filter();
+        uart_send();
+    }
+
+    switch (mode) {
+        case 'a':
+            deg = zero*4096.0f/360.0f;
+            break;
+        case 'b':
+            deg = (zero + 90)*4096.0f/360.0f;
+            break;
+        case 'c':
+            deg = (zero-A*sin(2*pi*f*t*0.001*Ts))*4096.0f/360.0f;       // 在正負A度搖
+            t = t + 1;
+            if(t >= 1/(f*0.001*Ts))                              // 2*pi*f*t*0.001*Ts = 2*pi -> t = 1/(f*0.001*T)
+                t = 0;
+            break;
+
+    }
+    
+    dynamixelClass.servo(SERVO_ID,deg,0x400);
+    Pos = dynamixelClass.readPosition(SERVO_ID);
+}
+
+void change_mode()
+{
+    mode = uart_1.getc();
+}
+
+float lpf(float input, float output_old, float frequency)
+{
+    float output = 0;
+    
+    output = (output_old + frequency*T*input) / (1 + frequency*T);
+    
+    return output;
+}