#include "r_cg_macrodriver.h"

#include "r_cg_cgc.h"

#include "r_cg_port.h"

#include "r_cg_intc.h"

#include "r_cg_serial.h"

#include "r_cg_timer.h"

/* Start user code for include. Do not edit comment generated here */

#include <stdio.h>

#include "control.h"

#include "pid.h"

/* End user code. Do not edit comment generated here */

#include "r_cg_userdefine.h"

/***********************************************************************************************************************

Global variables and functions

***********************************************************************************************************************/

/* Start user code for global. Do not edit comment generated here */

uint8_t uart1_getbuf[30];

uint8_t uart_sendbuf[10];

char LED_buf[20];

unsigned char sign;

unsigned char StartFlag;

float angle[3],T;

int uart1_get_count;

uint16_t throttle;

uint16_t SetThrottle = 4300;

uint16_t SetFlyTime = 3;

float SetHeight = 50;

uint8_t i;

ANGLE DesireRate;

ANGLE PID_OUT;

float CorrectAngle[3];

uint8_t loop_6ms;

uint8_t loop_100ms;

uint8_t loop_100ms_flag;

uint8_t loop_6ms_flag;

uint16_t secure_count;

/* End user code. Do not edit comment generated here */

void R_MAIN_UserInit(void);

/***********************************************************************************************************************

* Function Name: main

* Deion  : This function implements main function.

* Arguments    : None

* Return Value : None

***********************************************************************************************************************/

void main(void)

{

       R_MAIN_UserInit();

       /* Start user code. Do not edit comment generated here */

//    InitMPU6050();

       R_INTC0_Start();

       R_INTC1_Start();

       R_INTC2_Start();

//     R_INTC3_Start();

       R_INTC7_Start();

       OLED_Init();

       OLED_ShowString(0,0,"-----NJIT-----");

       sprintf(LED_buf,"SetTime:%3ds",SetFlyTime);

       OLED_ShowString(0,16,LED_buf);

       sprintf(LED_buf,"SetThro:%5d",SetThrottle);

       OLED_ShowString(0,32,LED_buf);

       moto_init();

       fly_enable = 1;

       /*

              pidInit(&pidRollRate,0,7,0,0.15);

              pidInit(&pidPitchRate,0,7,0,0.15);

              pidInit(&pidYawRate,0,-10,0,0);

              pidInit(&pidheight,0,200,0,8);

       */

       pidInit(&pidRollRate,0,3.8,0,0.046);

       pidInit(&pidPitchRate,0,3.8,0,0.046);

       pidInit(&pidYawRate,0,-12,0,0);

       pidInit(&pidheight,0,100,0,6);

       pidSetIntegralLimit(&pidRollRate, 1000);

       pidSetIntegralLimit(&pidPitchRate, 1000);

       pidSetIntegralLimit(&pidYawRate, 1000);

       pidInit(&pidRoll, 0, 8.5, 0, 0);

       pidInit(&pidPitch, 0, 8.5, 0, 0);

       pidInit(&pidYaw, 0, -4, 0, 0);

       pidSetIntegralLimit(&pidRoll, 20);

       pidSetIntegralLimit(&pidPitch, 20);

       pidSetIntegralLimit(&pidYaw, 100);

       uart_sendbuf[0] = 0xD6;

       uart_sendbuf[1] = 0x6D;

       uart_sendbuf[2] = 0x11;

       uart_sendbuf[3] = 0x11;

//     R_UART1_Send(uart_sendbuf,4);

       P7.3 = 0;

       throttle = 1300;

       P14.0 = 1;

       while(StartFlag != 1);

       switch(timu_count)

       {

              case 1: P7.3 = 1; break;

              case 2: SetThrottle += 300; break;

              case 3: P7.3 = 1;              break;

              case 4: P7.3 = 1; break;

              case 5: break;

       }

       P2.3 = 0;

       delay_ms(3000);

       R_INTC8_Start();

       R_TAU0_Channel5_Start();    //1s timer

       R_TAU0_Channel6_Start();   //2ms timer

       R_UART1_Start();

       R_TAU0_Channel7_Start();

       getsrc_off();

       DesireAngle[0] = -0.2;

       DesireAngle[1] = 0.3;

       CorrectAngle[0] = 1.5;

       CorrectAngle[1] = 0;

       CorrectAngle[2] = 0;

       while (1U)

       {

              if(sign == 1)

              {

                     secure_count = 0;

                     sign = 0;

                     get_attitude();

                            GetPWMout_PID(uart_GyroPrepared[0]*0.61,uart_GyroPrepared[1]*0.61,uart_GyroPrepared[2]*0.61,DesireRate.ROLL,DesireRate.PITCH,DesireRate.YAW,&PID_OUT.ROLL,&PID_OUT.PITCH,&PID_OUT.YAW);

                     moto1 = throttle + PID_OUT.ROLL - PID_OUT.PITCH - PID_OUT.YAW;

                     moto2 = throttle - PID_OUT.ROLL - PID_OUT.PITCH + PID_OUT.YAW;

                     moto3 = throttle - PID_OUT.ROLL + PID_OUT.PITCH - PID_OUT.YAW;

                     moto4 = throttle + PID_OUT.ROLL + PID_OUT.PITCH + PID_OUT.YAW;

                     moto2 -= 10;

                     moto4 += 30;

              }

              if(loop_6ms_flag)

              {

                     loop_6ms_flag=0;

                     GetAttitude_PID(uart_AnglePrepared[0],uart_AnglePrepared[1],uart_AnglePrepared[2],

                                                 DesireAngle[0] + CorrectAngle[0],DesireAngle[1] + CorrectAngle[1],DesireAngle[2] + 10 + CorrectAngle[2],

                                                 &DesireRate.ROLL,&DesireRate.PITCH,&DesireRate.YAW);

              }

              if(loop_100ms_flag)

              {

                     loop_100ms_flag = 0;

                     if(start_get_attitude == 0)

                     {

                            start_get_attitude = 1;

                            P14.0 = 1;

                            delay_us(15);

                            P14.0 = 0;

                     }

                     // sprintf(LED_buf,"%3d  ",(int)uart_AnglePrepared[2]);

                     // OLED_ShowString(0,0,LED_buf);

                     GetThrottle();

                     RaysControl();

              }

              /*           secure_count++;

                            if(secure_count >= 50000)

                            {

                                   fly_enable = 0;

                            }*/

              moto_flush();

       }

       /* End user code. Do not edit comment generated here */

}