Diff: main.cpp

Revision:

0:75b96455c9ac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 02 10:25:37 2022 +0000
@@ -0,0 +1,255 @@
+#include "MTi2.h"
+#include <stdio.h>
+#include "mbed.h"
+#include <math.h>
+#include <stdlib.h>
+
+#define GRAVITYACCELERATION 9.81f
+#define pi 3.14159265358979323846f
+SPI spi_MTI(PB_15, PB_14, PB_13);//MOSI MISO SCLK
+Serial pc(USBTX,USBRX);
+InterruptIn mybutton(USER_BUTTON);
+Ticker main_function; //interrupt
+
+DigitalOut led1(LED1);
+DigitalOut cs_MTI(PC_4);
+
+typedef union{
+    uint32_t data1;
+    float data2;
+}imu_data;
+
+imu_data eul[3];//euler angle
+imu_data acc[3];
+imu_data gry[3];
+
+bool button_state = false;
+float dt = 0.01; // sec
+
+void step_command();
+void Start_read();
+
+int i = 1;
+
+
+int main(void)
+{
+    spi_MTI.format(8, 3);
+    pc.baud(230400);
+    MTi2_Init();
+    mybutton.fall(&step_command);
+    
+    main_function.attach_us(&Start_read,dt*1000000);
+    while (1) {
+        //LL_mDelay(5);
+        //wait_us(5000); // ?
+
+    }
+
+}
+
+void Start_read()                      //interrupt function by TT
+{
+    if (button_state == true)
+    {
+        ReadData();
+        // printf("euler  %f,%f,%f\r\n",euler[0],euler[1],euler[2]);
+        //printf("%f,%f,%f,%f,%f,%f\r\n",accel[0],accel[1],accel[2],omega[0],omega[1],omega[2]);
+        printf("%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\r\n",euler[0],euler[1],euler[2],accel_[0],accel_[1],accel_[2],omega[0],omega[1],omega[2]);
+        //printf("omega  %f,%f,%f\r\n",omega[0],omega[1],omega[2]);
+    }
+}
+
+
+void step_command(){
+    led1 = !led1;
+    button_state = !button_state;
+}
+
+void SendOpcode(uint8_t Opcode)
+{
+//    printf("SendOpcode \r\n");
+    //LL_SPI_TransmitData8(SPI3, Opcode);
+    //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+    //FW[0] = LL_SPI_ReceiveData8(SPI3);
+    FW[0] = spi_MTI.write(Opcode);
+    
+    for(uint8_t i = 0;i<3;i++){// 3 fillword ?
+        //LL_SPI_TransmitData8(SPI3, i);
+        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+        //FW[i+1] = LL_SPI_ReceiveData8(SPI3);
+        FW[i+1] = spi_MTI.write(i);
+    }
+}
+
+uint8_t ReadProtInfo(){
+    len = 2;
+    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
+    cs_MTI = 0;
+    SendOpcode(ProtInfo);//send opcode
+    for(int i = 0;i<len;i++){//read data
+        //LL_SPI_TransmitData8(SPI3, 0x00);
+        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
+        buffer[i] = spi_MTI.write(0x00);
+    }
+    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
+    cs_MTI = 1;
+    if(FW[0]!=0xFA||FW[1]!=0xFF||FW[2]!=0xFF||FW[3]!=0xFF){
+        printf("Error!!\n");
+    }
+    return buffer[1];
+}
+
+void ConfigureProt(_Bool M,_Bool N,_Bool O,_Bool P)
+{
+//    printf("ConfigureProt \r\n");
+    uint8_t config = (M<<3) | (N<<2) | (O<<1) | (P<<0);
+    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
+    cs_MTI = 0;
+    SendOpcode(ConfigProt);
+    //LL_SPI_TransmitData8(SPI3, config);
+    spi_MTI.write(config);
+    //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+    //LL_SPI_ReceiveData8(SPI3);   ???
+    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
+    cs_MTI = 1;
+}
+
+void PipeStatus(){
+//    printf("PipeStatus \r\n");
+    len = 4;
+    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
+    cs_MTI = 0;
+    SendOpcode(PipeStat);//send opcode
+    for(int i = 0;i<len;i++){//read data
+        //LL_SPI_TransmitData8(SPI3, 0x00);
+        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
+        buffer[i] = spi_MTI.write(0x00);
+    }
+    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
+    cs_MTI = 1;
+    notificationSize = buffer[0] | (buffer[1]<<8);
+    measurementSize = buffer[2] | (buffer[3]<<8);
+//    printf("AAAAAAAAAAAAAAAAA %d\r\n",measurementSize);
+//    printf("nSize:%d\r\n",notificationSize);
+//    printf("mSize:%d\n",measurementSize);
+}
+
+void NotificationPipe(){
+    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
+    cs_MTI = 0;
+    SendOpcode(NotiPipe);//send opcode
+    for(int i = 0;i<notificationSize;i++){//read data
+        //LL_SPI_TransmitData8(SPI3, 0x00);
+        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
+        buffer[i] = spi_MTI.write(0x00);
+    }
+    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
+    cs_MTI = 1;
+}
+
+void MeasurementPipe(){
+//    printf("MeasurementPipe \r\n");
+    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
+    cs_MTI = 0;
+    SendOpcode(MeasPipe);//send opcode
+    for(int i = 0;i<measurementSize;i++){//read data
+        //LL_SPI_TransmitData8(SPI3, 0x00);
+        //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+        //buffer[i] = LL_SPI_ReceiveData8(SPI3);
+        buffer[i] = spi_MTI.write(0x00);
+    }
+    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
+    cs_MTI = 1;
+}
+
+void ControlPipe(){
+    //LL_GPIO_ResetOutputPin(GPIOB, LL_GPIO_PIN_10);//PB10(CS)
+    cs_MTI = 0;
+        SendOpcode(Control);//send opcode
+        for(int i = 0;i<ctrl_len;i++){//read data
+            //LL_SPI_TransmitData8(SPI3, ctrlBuf[i]);
+            //while(LL_SPI_IsActiveFlag_RXNE(SPI3) == RESET){}
+            //buffer[i] = LL_SPI_ReceiveData8(SPI3);
+            buffer[i] = spi_MTI.write(ctrlBuf[i]);
+        }
+    //LL_GPIO_SetOutputPin(GPIOB, LL_GPIO_PIN_10);//CS
+    cs_MTI = 1;
+}
+
+void ReadData(){
+//    printf("ReadData \r\n");
+    PipeStatus();
+    //LL_mDelay(1);
+    wait_us(100);
+    MeasurementPipe();
+    int len1,len2,len3,data_bytes;
+//    printf("Measurement FINISH \r\n");
+//    printf("buffer[0] == %d \r\n",buffer[0]);
+    if(buffer[0] == 0x36){
+//        printf("buffer \r\n");
+        if(buffer[2]== 0x20&&buffer[3]== 0x30){//Read Euler Angle
+            len1 = buffer[4];
+            data_bytes = len1/3;
+            for(int j=0;j<3;j++){
+                uint32_t temp = (buffer[5+j*data_bytes]<<24) | (buffer[6+j*data_bytes]<<16) | (buffer[7+j*data_bytes]<<8) | (buffer[8+j*data_bytes]);
+//                float *ptr = NULL;
+//                ptr = &temp;
+                eul[j].data1 = temp;
+//                euler[j] = *ptr;
+                euler[j] = lpf(eul[j].data2, euler[j], 13.0f);
+            }
+        }
+        if(buffer[4+len1+1]== 0x40&&buffer[4+len1+2]== 0x20){
+            len2 = buffer[4+len1+3];
+            data_bytes = len2/3;
+            for(int j=0;j<3;j++){
+//                printf("A\r\n");
+                uint32_t temp = (buffer[8+len1+j*data_bytes]<<24) | (buffer[9+len1+j*data_bytes]<<16) | (buffer[10+len1+j*data_bytes]<<8) | (buffer[11+len1+j*data_bytes]);
+//                float *ptr = NULL;
+//                ptr = &(float*)temp;
+                acc[j].data1 = temp;
+                //accel[j] = acc[j].data2;
+                accel[j] = lpf(acc[j].data2, accel[j], 13.0f);
+            }
+        }
+        if(buffer[7+len1+len2+1]== 0x80&&buffer[7+len1+len2+2]==0x20){
+            len3 = buffer[7+len1+len2+3];
+            data_bytes = len3/3;
+            for(int j=0;j<3;j++){
+                uint32_t temp = (buffer[11+len1+len2+j*data_bytes]<<24) | (buffer[12+len1+len2+j*data_bytes]<<16) | (buffer[13+len1+len2+j*data_bytes]<<8) | (buffer[14+len1+len2+j*data_bytes]);
+//                float *ptr = NULL;
+//                ptr = (float)temp;
+                gry[j].data1 = temp;
+                //omega[j] = gry[j].data2;
+                omega[j] = lpf(gry[j].data2, omega[j], 13.0f);
+            }
+        }
+
+    }
+    // calculate
+    accel_[0] = (accel[0] + sin(euler[1]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[1]/180.0f*pi) * (-1.0f);//deal with gravity * tilt angle ; *-1 because IMU on robot is 180 degree reverse
+    accel_[1] = (accel[1] - sin(euler[0]/180.0f*pi) * GRAVITYACCELERATION) / cos(euler[0]/180.0f*pi) * (-1.0f);
+    accel_[2] = accel[2];
+//    printf("NO ??? \r\n");
+}
+
+void MTi2_Init(){
+    //LL_SPI_Enable(SPI3);//Enable SPI
+//    printf("Init \r\n");
+    cs_MTI = 1;///???
+    ConfigureProt(1,0,0,0);//Configure DRDY
+
+    //LL_mDelay(500);//Wait 500ms MTi2 go to measurement mode
+
+}
+
+float lpf(float input, float output_old, float frequency)
+{
+    float output = 0;
+    output = (output_old + frequency * dt * input) / (1 + frequency * dt);
+    return output;
+}
\ No newline at end of file