Diff: main.cpp

Revision:

0:6bee2baac968

Child:

1:fdfd9a35acc4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 15 08:25:54 2021 +0000
@@ -0,0 +1,687 @@
+//20211018 version
+#include "MTi2.h"
+#include <stdio.h>
+#include "mbed.h"
+#include <math.h>
+#include <stdlib.h>
+#include "PMW3901.h"
+
+#define GRAVITYACCELERATION 9.81f
+#define pi 3.14159265358979323846f
+//motor
+#define maximum_volt 12.0f
+#define minimum_volt 0.45f // Need to test for different loads.
+
+#define INPUT_VOLTAGE       12.5f 
+#define PWM_FREQUENCY       10.0f   // the default value we set is 20.0 (unit : kHz)
+#define PWM_STOP            0.5f    //the pwm dutycycle value is from 0~1 and 0.5 can let motor stop
+
+#define FRICTION_VOLTAGE    0.45f
+#define HALL_RESOLUTION     64.0f
+#define GEAR_RATIO          56.0f      
+#define VOLT_CMD            8.0f    // unit(voltage)
+
+//Common
+Serial pc(USBTX,USBRX);
+InterruptIn mybutton(USER_BUTTON);
+Ticker main_function; //interrupt
+DigitalOut led1(LED1);
+DigitalOut led2(A4);
+DigitalOut led3(A5);
+
+//IMU
+SPI spi_MTI(PB_15, PB_14, PB_13);//MOSI MISO SCLK
+DigitalOut cs_MTI(PC_4);
+
+//GSS
+SPI spi(PC_12,PC_11,PC_10);
+DigitalOut cs(PA_4);
+
+//motor
+PwmOut pwm1A(D7);
+PwmOut pwm1B(D8);
+PwmOut pwm2A(D11);
+PwmOut pwm2B(A3);
+
+//Common
+bool button_state = false;
+float dt = 0.01; // sec
+//IMU
+typedef union{
+    uint32_t data1;
+    float data2;
+}imu_data;
+
+imu_data eul[3];//euler angle
+imu_data acc[3];
+imu_data gry[3];
+//GSS
+int GSS_X = 0;
+int GSS_Y = 0;
+
+//motor
+int readcount = 0;
+int RX_flag2 = 0;
+char getData[6] = {0,0,0,0,0,0};
+short data_received[2] = {0,0};
+
+float command = 0;
+float velocityA = 0; //rpm
+float velocityB = 0;
+float positionA = 0;
+float positionB = 0;
+short EncoderPositionA;
+short EncoderPositionB;
+float last_voltA = 0;
+float last_voltB = 0;
+float errorA = 0;
+float error_drA = 0;
+float errorB = 0;
+float error_drB = 0;
+float dutycycle = PWM_STOP;
+float VELOCITY_SPEED_A = 0.0;
+float VELOCITY_SPEED_B = 0.0;
+int pub_count = 0;
+
+//Common
+void step_command();
+
+//IMU
+void Start_read();
+
+//GSS
+void grabData(void);
+//void printData(void);
+void initializeSensor(void);
+void writeRegister(uint8_t addr, uint8_t data);
+uint8_t readRegister(uint8_t addr);
+void delayus(uint32_t us);
+
+//motor
+float PD(float e, float last_e, float last_u, float P, float D);
+float PDF(float e, float last_e, float last_u, float P, float D, float F);
+void ReadVelocity();
+void ReadPosition(float *positionA, float *positionB);
+void motor_drive(float voltA, float voltB);
+void InitMotor(float pwm_frequency);
+void InitEncoder(void);
+void control_speed();
+
+void RX_ITR();
+void init_UART();
+
+int main(void)
+{
+    pc.baud(230400);
+    led2 = 1;
+    led3 = 1;
+    //IMU
+    spi_MTI.format(8, 3);
+    MTi2_Init();
+    //GSS
+    spi.format(8, 3);
+    initializeSensor();
+    //motor
+    init_UART();
+    InitEncoder();
+    InitMotor(PWM_FREQUENCY);
+    
+    mybutton.fall(&step_command);
+    main_function.attach_us(&Start_read,dt*1000000);
+    while (1) {
+    }
+
+}
+
+void Start_read()                      //interrupt function by TT
+{
+    //IMU
+    ReadData();
+    //GSS
+    cs = 0;
+    grabData();
+    cs = 1;
+    if(button_state == true){
+        pub_count++;
+//        VELOCITY_SPEED_A = -10.0f;
+//        VELOCITY_SPEED_B = -10.0f;
+        ReadVelocity();
+        control_speed();
+        if (pub_count >= 10){
+            //printf("%.3f,%.3f\r\n",velocityA, velocityB);  // velocityA or velocityB
+            //printf("CMD %.3f,%.3f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
+            pub_count = 0;
+        }
+    }else{
+        uint16_t dutycycleA = PWM_STOP *uint16_t(TIM1->ARR);
+        uint16_t dutycycleB = PWM_STOP *uint16_t(TIM1->ARR);
+        TIM1->CCR1 = dutycycleA;
+        TIM1->CCR2 = dutycycleB;
+        command = 0;
+    }
+    if (button_state == true)
+    {
+        
+//        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("%d,%d\n\r", GSS_X, GSS_Y);
+//        printf("%.3f,%.3f\r\n",velocityA, velocityB);  // velocityA or velocityB
+
+        printf("%.3f,%.3f,%d,%d,%.4f,%.4f,%.4f\r\n",velocityA, velocityB, GSS_X, GSS_Y, accel_[0],accel_[1], omega[2]);
+//        printf("%.2f,%.2f\r\n",VELOCITY_SPEED_A, VELOCITY_SPEED_B);
+    }
+}
+
+
+void step_command(){
+    led1 = !led1;
+    led2 = !led2;
+    led3 = !led3;
+    button_state = !button_state;
+}
+//IMU
+void SendOpcode(uint8_t Opcode)
+{
+//    printf("SendOpcode \r\n");
+    FW[0] = spi_MTI.write(Opcode);
+    
+    for(uint8_t i = 0;i<3;i++){// 3 fillword ?
+        FW[i+1] = spi_MTI.write(i);
+    }
+}
+
+//uint8_t ReadProtInfo(){
+//    len = 2;
+//    cs_MTI = 0;
+//    SendOpcode(ProtInfo);//send opcode
+//    for(int i = 0;i<len;i++){//read data
+//        buffer[i] = spi_MTI.write(0x00);
+//    }
+//    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);
+    cs_MTI = 0;
+    SendOpcode(ConfigProt);
+    spi_MTI.write(config);
+    cs_MTI = 1;
+}
+
+void PipeStatus(){
+//    printf("PipeStatus \r\n");
+    len = 4;
+    cs_MTI = 0;
+    SendOpcode(PipeStat);//send opcode
+    for(int i = 0;i<len;i++){//read data
+        buffer[i] = spi_MTI.write(0x00);
+    }
+    cs_MTI = 1;
+    notificationSize = buffer[0] | (buffer[1]<<8);
+    measurementSize = buffer[2] | (buffer[3]<<8);
+    //printf("nSize:%d\r\n",notificationSize);
+    //printf("mSize:%d\r\n",measurementSize);
+}
+
+//void NotificationPipe(){
+//    cs_MTI = 0;
+//    SendOpcode(NotiPipe);//send opcode
+//    for(int i = 0;i<notificationSize;i++){//read data
+//        buffer[i] = spi_MTI.write(0x00);
+//    }
+//    cs_MTI = 1;
+//}
+
+void MeasurementPipe(){
+//    printf("MeasurementPipe \r\n");
+    cs_MTI = 0;
+    SendOpcode(MeasPipe);//send opcode
+    for(int i = 0;i<measurementSize;i++){//read data
+        buffer[i] = spi_MTI.write(0x00);
+    }
+    cs_MTI = 1;
+}
+
+void ControlPipe(){
+    cs_MTI = 0;
+        SendOpcode(Control);//send opcode
+        for(int i = 0;i<ctrl_len;i++){//read data
+            buffer[i] = spi_MTI.write(ctrlBuf[i]);
+        }
+    cs_MTI = 1;
+}
+
+void ReadData(){
+//    printf("ReadData \r\n");
+    PipeStatus();
+    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]);
+                eul[j].data1 = temp;
+                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++){
+                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]);
+                acc[j].data1 = temp;
+                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]);
+                gry[j].data1 = temp;
+                omega[j] = lpf(gry[j].data2, omega[j], 13.0f);
+            }
+        }
+
+    }
+    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];
+}
+
+void MTi2_Init(){
+//    printf("Init \r\n");
+    cs_MTI = 1;///???
+    ConfigureProt(1,0,0,0);//Configure DRDY
+}
+
+float lpf(float input, float output_old, float frequency)
+{
+    float output = 0;
+    output = (output_old + frequency * dt * input) / (1 + frequency * dt);
+    return output;
+}
+
+//GSS
+
+uint8_t readRegister(uint8_t addr) {
+    wait_us(10);                                //tswr
+
+    cs = 0;
+    addr = addr & 0x7F;                         //Set MSB to 0 to indicate read operation
+
+    spi.write(addr);
+    
+    wait_us(35);
+
+    uint8_t data_read = spi.write(0U);
+    
+    wait_us(1);                             //tsclk-ncs
+    cs = 1;
+    wait_us(20);                            //tsclk-ncs
+    return data_read;                           //Returns 8-bit data from register
+}
+
+//=========================================================================
+void writeRegister(uint8_t addr, uint8_t data) {
+    cs = 0;
+    addr = addr | 0x80;             //Set MSB to 1 to indicate write operation
+
+    spi.write(addr);
+    
+    spi.write(data);
+
+    wait_us(25);                            //tsclk-ncs
+    cs = 1;
+    wait_us(1);                             //tsclk-ncs
+}
+
+//=========================================================================
+void initializeSensor(void) {
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x55, 0x01);
+    writeRegister(0x50, 0x07);
+    writeRegister(0x7F, 0x0E);
+    writeRegister(0x43, 0x10);
+
+    if (readRegister(0x67) & 0x40)
+        writeRegister(0x48, 0x04);
+    else
+        writeRegister(0x48, 0x02);
+
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x51, 0x7B);
+    writeRegister(0x50, 0x00);
+    writeRegister(0x55, 0x00);
+    writeRegister(0x7F, 0x0E);
+
+    if (readRegister(0x73) == 0x00) {
+        writeRegister(0x7F, 0x00);
+        writeRegister(0x61, 0xAD);
+        writeRegister(0x51, 0x70);
+        writeRegister(0x7F, 0x0E);
+
+        if (readRegister(0x70) <= 28)
+            writeRegister(0x70, readRegister(0x70) + 14);
+        else
+            writeRegister(0x70, readRegister(0x70) + 11);
+
+        writeRegister(0x71, readRegister(0x71) * 45 / 100);
+    }
+
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x61, 0xAD);
+    writeRegister(0x7F, 0x03);
+    writeRegister(0x40, 0x00);
+    writeRegister(0x7F, 0x05);
+    writeRegister(0x41, 0xB3);
+    writeRegister(0x43, 0xF1);
+    writeRegister(0x45, 0x14);
+    writeRegister(0x5B, 0x32);
+    writeRegister(0x5F, 0x34);
+    writeRegister(0x7B, 0x08);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x44, 0x1B);
+    writeRegister(0x40, 0xBF);
+    writeRegister(0x4E, 0x3F);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x44, 0x1B);
+    writeRegister(0x40, 0xBF);
+    writeRegister(0x4E, 0x3F);
+    writeRegister(0x7F, 0x08);
+    writeRegister(0x65, 0x20);
+    writeRegister(0x6A, 0x18);
+    writeRegister(0x7F, 0x09);
+    writeRegister(0x4F, 0xAF);
+    writeRegister(0x5F, 0x40);
+    writeRegister(0x48, 0x80);
+    writeRegister(0x49, 0x80);
+    writeRegister(0x57, 0x77);
+    writeRegister(0x60, 0x78);
+    writeRegister(0x61, 0x78);
+    writeRegister(0x62, 0x08);
+    writeRegister(0x63, 0x50);
+    writeRegister(0x7F, 0x0A);
+    writeRegister(0x45, 0x60);
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x4D, 0x11);
+    writeRegister(0x55, 0x80);
+    writeRegister(0x74, 0x21);
+    writeRegister(0x75, 0x1F);
+    writeRegister(0x4A, 0x78);
+    writeRegister(0x4B, 0x78);
+    writeRegister(0x44, 0x08);
+    writeRegister(0x45, 0x50);
+    writeRegister(0x64, 0xFF);
+    writeRegister(0x65, 0x1F);
+    writeRegister(0x7F, 0x14);
+    writeRegister(0x65, 0x67);
+    writeRegister(0x66, 0x08);
+    writeRegister(0x63, 0x70);
+    writeRegister(0x7F, 0x15);
+    writeRegister(0x48, 0x48);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x41, 0x0D);
+    writeRegister(0x43, 0x14);
+    writeRegister(0x4B, 0x0E);
+    writeRegister(0x45, 0x0F);
+    writeRegister(0x44, 0x42);
+    writeRegister(0x4C, 0x80);
+    writeRegister(0x7F, 0x10);
+    writeRegister(0x5B, 0x02);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x40, 0x41);
+    writeRegister(0x70, 0x00);
+
+    wait_ms(10);
+
+    writeRegister(0x32, 0x44);
+    writeRegister(0x7F, 0x07);
+    writeRegister(0x40, 0x40);
+    writeRegister(0x7F, 0x06);
+    writeRegister(0x62, 0xF0);
+    writeRegister(0x63, 0x00);
+    writeRegister(0x7F, 0x0D);
+    writeRegister(0x48, 0xC0);
+    writeRegister(0x6F, 0xD5);
+    writeRegister(0x7F, 0x00);
+    writeRegister(0x5B, 0xA0);
+    writeRegister(0x4E, 0xA8);
+    writeRegister(0x5A, 0x50);
+    writeRegister(0x40, 0x80);
+
+    wait_ms(250);
+
+    writeRegister(0x7F, 0x14);
+    writeRegister(0x6F, 0x1C);
+    writeRegister(0x7F, 0x00);
+
+}
+void grabData(void) {
+    static int totalX = 0;
+    static int totalY = 0;
+    uint8_t check = 0;
+    if(button_state == true){
+        check = readRegister(0x02) & 0x80;
+        if (check) {
+            deltaX_low = readRegister(0x03);        //Grabs data from the proper registers.
+            deltaX_high = (readRegister(0x04) << 8) & 0xFF00; //Grabs data and shifts it to make space to be combined with lower bits.
+            deltaY_low = readRegister(0x05);
+            deltaY_high = (readRegister(0x06) << 8) & 0xFF00;
+    
+            deltaY = deltaX_high | deltaX_low;      //Combines the low and high bits.
+            deltaX = deltaY_high | deltaY_low;
+            totalX += deltaX;
+            totalY += deltaY;
+
+        }
+        GSS_X = totalX;
+        GSS_Y = totalY;
+    }
+}
+
+//motor
+void ReadVelocity(){
+    /*
+    The velocity is calculated by follow :
+    velocity = EncoderPosition /Encoder CPR (Counts per round) /gear ratio *2pi /dt
+    unit : rad/sec
+    */
+    
+    EncoderPositionA = TIM2->CNT ;
+    EncoderPositionB = TIM3->CNT ;
+    TIM2->CNT = 0;
+    TIM3->CNT = 0;
+    // rad/s
+    velocityA = EncoderPositionA /HALL_RESOLUTION /GEAR_RATIO /dt *60;
+    velocityB = EncoderPositionB /HALL_RESOLUTION /GEAR_RATIO /dt *60;
+    // RPM
+//    *velocityA = EncoderPositionA /64.0 /56.0 /dt *60.0;
+//    *velocityB = EncoderPositionB /64.0 /56.0 /dt *60.0;
+}
+
+
+void motor_drive(float voltA, float voltB){
+    // Input voltage is in range -12.5V ~ 12.5V
+    if(abs(voltA) <= minimum_volt){
+        if(voltA > 0){ voltA = minimum_volt; }
+        else{ voltA = -minimum_volt; }
+    }
+    if(abs(voltB) <= minimum_volt){
+        if(voltB > 0){ voltB = minimum_volt; }
+        else{ voltB = -minimum_volt; }
+    }
+    
+    // Convet volt to pwm
+    uint16_t dutycycleA = (0.5f - 0.5f *voltA /INPUT_VOLTAGE) *uint16_t(TIM1->ARR);
+    uint16_t dutycycleB = (0.5f - 0.5f *voltB /INPUT_VOLTAGE) *uint16_t(TIM1->ARR);
+    TIM1->CCR1 = dutycycleA;
+    TIM1->CCR2 = dutycycleB;
+}
+
+
+void control_speed(){
+    float voltA;
+    float voltB;
+    // if receive 0 command than reset every thing
+    if(VELOCITY_SPEED_A == 0 && VELOCITY_SPEED_B == 0)
+    {
+        velocityA = 0;
+        velocityB = 0;
+        last_voltA = 0;
+        last_voltB = 0;
+        errorA = 0;
+        error_drA = 0;
+        errorB = 0;
+        error_drB = 0;
+    }
+    errorA = (VELOCITY_SPEED_A - velocityA);//(command from TX2 - read from odometry)
+    voltA = last_voltA + 0.4f*errorA - 0.35f*error_drA;
+    error_drA = errorA;
+    last_voltA = voltA;
+    if(abs(voltA) > INPUT_VOLTAGE){
+        if(voltA > 0){voltA = INPUT_VOLTAGE;}
+        else{voltA = -INPUT_VOLTAGE;}    
+    }
+    
+    errorB = (VELOCITY_SPEED_B - velocityB);
+    voltB = last_voltB + 0.4f*errorB - 0.35f*error_drB;
+    error_drB = errorB;
+    last_voltB = voltB;
+    if(abs(voltB) > INPUT_VOLTAGE){
+        if(voltB > 0){voltB = INPUT_VOLTAGE;}
+        else{voltB = -INPUT_VOLTAGE;}    
+    }
+    
+    motor_drive(voltA, voltB);
+
+    //printf("%.3f, %.3f, %.3f\r\n",error1, last_error, voltA);
+}
+
+
+void InitEncoder(void) {
+    // Hardware Quadrature Encoder AB for Nucleo F446RE
+    // Output on debug port to host PC @ 9600 baud
+
+    /* Connections
+    PA_0 = Encoder1 A
+    PA_1 = Encoder1 B
+    PB_5 = Encoder2 A
+    PB_4 = Encoder2 B
+    */
+    
+    // configure GPIO PA0, PA1, PB5 & PB4 as inputs for Encoder
+    RCC->AHB1ENR |= 0x00000003;  // Enable clock for GPIOA & GPIOB
+ 
+    GPIOA->MODER   |= GPIO_MODER_MODER0_1 | GPIO_MODER_MODER1_1 ;           // PA0 & PA1 as Alternate Function  /*!< GPIO port mode register,               Address offset: 0x00      */
+    GPIOA->PUPDR   |= GPIO_PUPDR_PUPDR0_0 | GPIO_PUPDR_PUPDR1_0 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
+    GPIOA->AFR[0]  |= 0x00000011 ;                                          // AF1 for PA0 & PA1                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+    GPIOA->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+   
+ 
+    GPIOB->MODER   |= GPIO_MODER_MODER4_1 | GPIO_MODER_MODER5_1 ;           // PB5 & PB4 as Alternate Function  /*!< GPIO port mode register,               Address offset: 0x00      */
+    GPIOB->PUPDR   |= GPIO_PUPDR_PUPDR4_0 | GPIO_PUPDR_PUPDR5_0 ;           // Pull Down                        /*!< GPIO port pull-up/pull-down register,  Address offset: 0x0C      */
+    GPIOB->AFR[0]  |= 0x00220000 ;                                          // AF2 for PB5 & PB4                /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+    GPIOB->AFR[1]  |= 0x00000000 ;                                          //                                  /*!< GPIO alternate function registers,     Address offset: 0x20-0x24 */
+   
+    // configure TIM2 & TIM3 as Encoder input
+    RCC->APB1ENR |= 0x00000003;  // Enable clock for TIM2 & TIM3
+
+    TIM2->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1  
+    TIM2->SMCR  = 0x0003;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
+    TIM2->CCMR1 = 0xF1F1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
+    TIM2->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
+    TIM2->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
+    TIM2->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
+    TIM2->ARR   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
+  
+    TIM2->CNT = 0x0000;  //reset the counter before we use it
+ 
+    TIM3->CR1   = 0x0001;     // CEN(Counter ENable)='1'     < TIM control register 1    
+    TIM3->SMCR  = 0x0003;     // SMS='011' (Encoder mode 3)  < TIM slave mode control register
+    TIM3->CCMR1 = 0xF1F1;     // CC1S='01' CC2S='01'         < TIM capture/compare mode register 1
+    TIM3->CCMR2 = 0x0000;     //                             < TIM capture/compare mode register 2
+    TIM3->CCER  = 0x0011;     // CC1P CC2P                   < TIM capture/compare enable register
+    TIM3->PSC   = 0x0000;     // Prescaler = (0+1)           < TIM prescaler
+    TIM3->ARR   = 0xffffffff; // reload at 0xfffffff         < TIM auto-reload register
+  
+    TIM3->CNT = 0x0000;  //reset the counter before we use it
+}
+
+
+void InitMotor(float pwm_frequency){
+    uint16_t reload = 90000000 /int(pwm_frequency * 1000) - 1;
+    uint16_t stop   = 90000000 /int(pwm_frequency * 1000) /2 - 1;
+    
+    TIM1->CR1   &= (~0x0001);   // Set counter disable in Control Register 1 at initial
+    TIM1->PSC    = 1U;          // Prescaler system clock (1 + PSC) for Timer 1
+    TIM1->ARR    = reload;      // Set auto-reload, the pwm freq is (system clk /(1+PSC) /ARR)
+    TIM1->CCMR1 |= 0x0808;      // Not necessary
+    TIM1->CCER  |= 0x0055;      // Enable complementary mode for channel 1, channel 2
+    TIM1->BDTR  |= 0x0C00;      // Set off-state selection
+    TIM1->EGR    = 0x0001;      // Update generation
+    TIM1->CR1   |= 0x0001;      // Counter enable
+/*
+    pc.printf("CR1 : %d\r",uint16_t(TIM1->CR1));
+    pc.printf("PSC : %d\r",uint16_t(TIM1->PSC));
+    pc.printf("ARR : %d\r",uint16_t(TIM1->ARR));
+    pc.printf("CCMR1 : %x\r",TIM1->CCMR1);
+    pc.printf("CCER : %x\r",TIM1->CCER);
+    pc.printf("BDTR : %x\r",TIM1->BDTR);
+    pc.printf("EGR : %x\r",TIM1->EGR);
+    pc.printf("stop : %d\r",stop);
+*/
+    TIM1->CCR1 = stop;
+    TIM1->CCR2 = stop;
+    
+//    bool cc1ne_bit = (TIM1->CCER >> 2) & 0x0001;
+//    pc.printf("CC1NE bit : %d\r",cc1ne_bit);
+}
+
+
+void init_UART()
+{
+    pc.baud(230400);  // baud rate設為9600
+    pc.attach(&RX_ITR, Serial::RxIrq);  // Attach a function(RX_ITR) to call whenever a serial interrupt is generated.
+}
+
+
+void RX_ITR()
+{
+    while(pc.readable()) {
+        char uart_read;
+        uart_read = pc.getc();
+        if(uart_read == 115) {
+            RX_flag2 = 1;
+            readcount = 0;
+            getData[5] = 0;
+        } 
+        if(RX_flag2 == 1) {
+            getData[readcount] = uart_read;
+            readcount++;
+            if(readcount >= 6 & getData[5] == 101) {
+                readcount = 0;
+                RX_flag2 = 0;               
+                ///code for decoding///
+                data_received[0] = (getData[2] << 8) | getData[1];
+                data_received[1] = (getData[4] << 8) | getData[3];    
+//                VELOCITY_SPEED_A = data_received[0]/100;
+//                VELOCITY_SPEED_B = data_received[1]/100;
+                VELOCITY_SPEED_A = (float)data_received[0]/100.0f;
+                VELOCITY_SPEED_B = (float)data_received[1]/100.0f;
+                ///////////////////////
+            }
+        } 
+    }
+}
\ No newline at end of file