/*
 * Copyright (c) 2020-2021, Bluetrum Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2020/12/10     greedyhao    The first version
 */

/**
 * Notice!
 * All functions or data that are called during an interrupt need to be in RAM.
 * You can do it the way exception_isr() does.
 */

#include <rtthread.h>
#include "board.h"
#include "uart1.h"
#include "motor.h"

/* Standard includes. */
#include <stdio.h>
/* rtthread includes. */
#include <rtdevice.h>

float angle_last=90;
int main(void)
{
    int i=0;
    char txt[1],data=0x11;
    uint8_t dir1 = rt_pin_get("PF.0");
    uint8_t pin = rt_pin_get("PA.1");

    rt_pin_mode(pin, PIN_MODE_OUTPUT);
    rt_pin_mode(dir1, PIN_MODE_OUTPUT);

    uart1_init();

    Pwm_Init();
    rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL, period, 5000);

    while (1)
    {
        if(rx_flag){
            for(i=0;i<11;i++){
             if(rx_buf[i]==0x55&&rx_buf[i+1]==0x53&&i<6){
                 Roll = 90+(float)(((unsigned short)rx_buf[i+5]<<8)|rx_buf[i+4])*180.0/32768.0;
                 if(Roll>180)Roll-=360;
                 //rt_kprintf("a");
                 if(abs((int)(Roll-angle_last))>5)Roll=angle_last;
                 angle_last=Roll;
             }

         }
            rx_flag=0;
        }
        output = (int)(Roll-90)*500;
       if(output<0){
           output=-output;
           rt_pin_write(dir1, PIN_HIGH);
       }else{
           output=10000-output;
           rt_pin_write(dir1, PIN_LOW);
       }
       if(output>10000)output=10000;
       rt_pwm_set(pwm_dev, PWM_DEV_CHANNEL, period, output);
        SCI_Send_Datas();
        //rt_sprintf(txt, "%c", 0X11);
        //rt_device_write(serial, 0, txt, (sizeof(txt)) );
        //
        //rt_kprintf("angle = %d  \n",(int)(Roll*1000));
        //rt_pin_write(pin, PIN_LOW);
        rt_thread_mdelay(20);
        //rt_pin_write(pin, PIN_HIGH);
        //rt_thread_mdelay(1000);
    }
}
