/*=====================================================================================================*/
void System_Init( void )
{
SystemInit();
LED_Config();
KEY_Config();
RS232_Config();
I2C_Config();
Motor_Config();
nRF24L01_Config();
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +3.5f;
PID_Pitch.Ki = +0.004f;
PID_Pitch.Kd = +4.0f;
PID_Roll.Kp = +3.5f;
PID_Roll.Ki = +0.004f;
PID_Roll.Kd = +4.0f;
PID_Yaw.Kp = +0.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +0.25f;
Delay_10ms(2);
}
/*=====================================================================================================*/
void System_Init(void)
{
SystemInit();
LED_Config();
KEY_Config();
RS232_Config();
Motor_Config();
PWM_Capture_Config();
Sensor_Config();
nRF24L01_Config();
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +1.5f;
PID_Pitch.Ki = +0.002f;
PID_Pitch.Kd = +1.0f;
PID_Roll.Kp = +1.5f;
PID_Roll.Ki = +0.002f;
PID_Roll.Kd = +1.0f;
PID_Yaw.Kp = +0.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +0.0f;
Delay_10ms(2);
}
void SCx_Init(void)
{
memset(&status_ctrl, 0, sizeof(status_ctrl));
PID_Init(&status_ctrl.PID_pitch, PID_MODE_DERIVATIV_SET, 0.005f);
PID_Init(&status_ctrl.PID_roll, PID_MODE_DERIVATIV_SET, 0.005f);
PID_Init(&status_ctrl.PID_yaw, PID_MODE_DERIVATIV_SET, 0.005f);
PID_Init(&status_ctrl.PID_alt, PID_MODE_DERIVATIV_CALC, 0.005f);
}
void Control_Init(void)
{
PID_Init(&RollRate_PID, 1.25, 0, 0.009); // *** INSIDE ROLL PASS *** 1.25 0 0.009 1.5 0 0.014
PID_Init(&Roll_PID, 1.16, 0, 0.002); // ***** OUTSIDE ROLL PASS ***** 1.16 0 0.002 2.5 0 0.01
// New
PID_Init(&PitchRate_PID, 2.0, 0, 0.016); // *** INSIDE PITCH PASS *** 2.0 0 0.016 1.65 0 0.015
PID_Init(&Pitch_PID, 0.65, 0, 0.0014); // ***** OUTSIDE ROLL PASS ***** 0.65 0 0.0014 2.2 0 0.008
}
void system_init(void)
{
LED_Config();
KEY_Config();
RS232_Config();
Motor_Config();
PWM_Capture_Config();
Sensor_Config();
nRF24L01_Config();
#if configSD_BOARD
SDIO_Config();
#endif
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +4.0f;
PID_Pitch.Ki = 0;//0.002f;
PID_Pitch.Kd = +1.5f;
PID_Roll.Kp = +4.0f;
PID_Roll.Ki = 0;//0.002f;
PID_Roll.Kd = 1.5f;
PID_Yaw.Kp = +5.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +15.0f;
Delay_10ms(10);
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
#if configFLIGHT_CONTROL_BOARD
/* nRF Check */
while ( nRF_Check()== ERROR );
/* Sensor Init */
while(Sensor_Init() == ERROR);
#endif
Delay_10ms(10);
/* Lock */
LED_R = 0;
LED_G = 1;
LED_B = 1;
sys_status = SYSTEM_INITIALIZED;
}
/*=====================================================================================================*/
void QCopterFC_Init( void )
{
u8 Sta = ERROR;
SystemInit();
LED_Config();
KEY_Config();
RS232_Config();
Motor_Config();
Sensor_Config();
nRF24L01_Config();
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +1.5f;
PID_Pitch.Ki = +0.002f;
PID_Pitch.Kd = +1.0f;
PID_Roll.Kp = +1.5f;
PID_Roll.Ki = +0.002f;
PID_Roll.Kd = +1.0f;
PID_Yaw.Kp = +0.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +0.0f;
RF_SendData.Packet = 0x00;
/* Throttle Config */
if(KEY == KEY_ON) {
LED_B = 0;
BLDC_CtrlPWM(BLDC_PWM_MAX, BLDC_PWM_MAX, BLDC_PWM_MAX, BLDC_PWM_MAX);
}
while(KEY == KEY_ON);
LED_B = 1;
BLDC_CtrlPWM(BLDC_PWM_MIN, BLDC_PWM_MIN, BLDC_PWM_MIN, BLDC_PWM_MIN);
/* nRF Check */
while(Sta == ERROR)
Sta = nRF_Check();
Delay_10ms(10);
/* Sensor Init */
if(Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
}
unsigned char Application_Init(void)
{
unsigned int PWM_Max_Value = 0;
PIDCoff Coff_MOTOR = {0, 0, 0};
TSVN_FOSC_Init();
TSVN_Led_Init(ALL);
TSVN_ACS712_Init();
TSVN_QEI_TIM1_Init(400);
TSVN_QEI_TIM3_Init(400);
TSVN_QEI_TIM4_Init(400);
TSVN_CAN_Init();
TSVN_USART_Init();
TSVN_TIM6_Init(2000);
FIR_Init();
PWM_Max_Value = TSVN_PWM_TIM5_Init(5000);
TSVN_PWM_TIM5_Set_Duty(0, MOTOR1_PWM);
TSVN_PWM_TIM5_Set_Duty(0, MOTOR2_PWM);
TSVN_PWM_TIM5_Set_Duty(0, MOTOR3_PWM);
TSVN_PWM_TIM5_Start();
DIR_Init();
PID_Mem_Create(3);
PID_WindUp_Init(MOTOR1, PWM_Max_Value);
PID_WindUp_Init(MOTOR2, PWM_Max_Value);
PID_WindUp_Init(MOTOR3, PWM_Max_Value);
Coff_MOTOR.Kp = 4;
Coff_MOTOR.Ki = 0.01;
Coff_MOTOR.Kd = 0.0;
PID_Init(MOTOR1, Coff_MOTOR);
PID_Init(MOTOR2, Coff_MOTOR);
PID_Init(MOTOR3, Coff_MOTOR);
Pos_Queue = xQueueCreate(200, sizeof(Pos_TypeDef));
CanRxQueue = xQueueCreate(200, sizeof(CanRxMsg));
Moment_Queue = xQueueCreate(200, sizeof(Moment_Typedef));
RxQueue = xQueueCreate(200, sizeof(xData));
UART_xCountingSemaphore = xSemaphoreCreateCounting(200, 0);
if (Pos_Queue != NULL && CanRxQueue != NULL &&
Moment_Queue != NULL && UART_xCountingSemaphore != NULL && RxQueue != NULL)
return SUCCESS;
return ERROR;
}
void LF_Init(void) {
PID_Init();
LF_currState = STATE_IDLE;
if (FRTOS1_xTaskCreate(LineTask, "Line", configMINIMAL_STACK_SIZE+100, NULL, tskIDLE_PRIORITY+2, NULL) != pdPASS) {
for(;;){} /* error */
}
}
/**
* @brief Init Function of Position Mode
*
* Calculate the time boundaries.
*
* @see PosInfo
* @param PIDS PID Struct to be inited as Position Mode
* @param acc Accleration
* @param topSpeed Top Speed
* @param dece Deceleration
* @param targetPos Target Position
* @param initPos Init Position
* @param motor Motor under control
* @param encoderChannel Encoder Channel
* @param divider Update Divider
*
* @return Pointer to inited Position Mode PID Struct
* */
struct PIDStruct* PID_Init_Pos(struct PIDStruct* PIDS,double acc,double topSpeed,double dece,int targetPos,int initPos,struct Motor_Struct* motor,int encoderChannel,int divider){
struct PosInfo* POSS = &PIDS->info.Pos;
double posDiff = targetPos - initPos;
if(posDiff < 0)
posDiff = -posDiff;
// POSS = (struct PosInfo*)malloc(sizeof(struct PosInfo));
POSS->acceleration = acc;
POSS->topSpeed =topSpeed;
POSS->deceleration = dece;
POSS->targetPos = targetPos;
POSS->initPos = initPos;
POSS->time = 0;
POSS->T1 = POSS->topSpeed / POSS->acceleration;
POSS->Tend = (posDiff / POSS->topSpeed)
+ (POSS->topSpeed / (POSS->acceleration * 2.0))
+ (POSS->topSpeed / (POSS->deceleration * 2.0));
POSS->T2 = POSS->Tend - (POSS->topSpeed / POSS->deceleration);
POSS->timeRegion = 0;
// POSS->pwmChannel= pwmChannel;
POSS->motor = motor;
POSS->encoderChannel = encoderChannel;
return PID_Init(PIDS,PosInfoErrCal,PosInfoOutFunc,divider);
}
void Extruder_Start_Heating(uint16_t _target)
{
DBG_MSG("Target=%d", _target);
PID_Init(&pid, EXTRUDER_PID_KP, EXTRUDER_PID_KI, EXTRUDER_PID_KD, EXTRUDER_PID_INIT_SUM);
PWM_Channel(Ex1Heat_Ch, 90, false);
currentOutput = 0;
bHeating = true;
targetTemp = _target;
}
/**
* @brief Init Function of Theta Mode
*
* @param PIDS PID Struct to be inited as Theta Mode
* @param targetTheta Target Theta
* @param motorL Left Motor Velocity Mode PID Struct
* @param motorR Right Motor Velocity Mode PID Struct
* @param pos Position Struct
* @param divider Update Divider
*
* @return Pointer to inited Velocity Mode PID Struct
* */
struct PIDStruct* PID_Init_Theta(struct PIDStruct* PIDS,double targetTheta,struct VelInfo* motorL, struct VelInfo* motorR, struct Pos* pos,int divider){
struct ThetaInfo* THES = &PIDS->info.Theta;
// THES = (struct ThetaInfo*)malloc(sizeof(struct ThetaInfo));
THES->targetTheta = targetTheta;
THES->pos = pos;
THES->motorL = motorL;
THES->motorR = motorR;
return PID_Init(PIDS,ThetaInfoErrCal,ThetaInfoOutFunc,divider);
}
/**
* @brief Init Function of Velocity Mode
*
* @param PIDS PID Struct to be inited as Velocity Mode
* @param targetSpeed Target Velocity
* @param motor Motor under control
* @param encoderChannel Encoder Channel
* @param divider Update Divider
*
* @return Pointer to inited Velocity Mode PID Struct
* */
struct PIDStruct* PID_Init_Vel(struct PIDStruct* PIDS,double targetSpeed,struct Motor_Struct* motor,int encoderChannel,int divider){
struct VelInfo* VELS = &PIDS->info.Vel;
// VELS = (struct VelInfo*)malloc(sizeof(struct VelInfo));
VELS->targetSpeed = targetSpeed;
VELS->oldPos = 0;
// VELS->pwmChannel = pwmChannel;
VELS->motor = motor;
VELS->encoderChannel = encoderChannel;
return PID_Init(PIDS,VelInfoErrCal,VelInfoOutFunc,divider);
}
void system_init(void)
{
LED_Config();
Serial_Config(Serial_Baudrate);
Motor_Config();
PWM_Capture_Config();
//IMU Config
Sensor_Config();
nRF24L01_Config();
//SD Config
if ((SD_status = SD_Init()) != SD_OK)
system.status = SYSTEM_ERROR_SD;
PID_Init(&PID_Pitch, 4.0, 0.0, 1.5);
PID_Init(&PID_Roll, 4.0, 0.0, 1.5);
PID_Init(&PID_Yaw, 5.0, 0.0, 15.0);
Delay_10ms(10);
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while (nRF_Check() == ERROR);
/* Sensor Init */
while (Sensor_Init() == ERROR);
Delay_10ms(10);
/* Lock */
SetLED(LED_R, DISABLE);
SetLED(LED_G, ENABLE);
SetLED(LED_B, ENABLE);
//Check if no error
if (system.status != SYSTEM_ERROR_SD)
system.status = SYSTEM_INITIALIZED;
}
void set_pitch(s16 p,s16 i,s16 d)
{
PID_Init(&pitch_pid,p/10.0,i/100.0,d/10.0);
settings.pitch_p = pitch_pid.p;
settings.pitch_i = pitch_pid.i;
settings.pitch_d = pitch_pid.d;
save_settings(&settings,"/setting");
get_pid();
}
void set_yaw(s16 p,s16 i,s16 d)
{
PID_Init(&yaw_pid,p/10.0,i/100.0,d/10.0);
settings.yaw_p = yaw_pid.p;
settings.yaw_i = yaw_pid.i;
settings.yaw_d = yaw_pid.d;
save_settings(&settings,"/setting");
get_pid();
}
void set_roll(s16 p,s16 i,s16 d)
{
PID_Init(&roll_pid,p/10.0,i/100.0,d/10.0);
settings.roll_p = roll_pid.p;
settings.roll_i = roll_pid.i;
settings.roll_d = roll_pid.d;
save_settings(&settings,"/setting");
get_pid();
}
int main()
{
PLL_Init(); // 80 MHz system clock
SysTick_Init(80); // 1 us SysTick periodic interrupts
PWM_Init(); // Initialize PF1,PF2 and PF3 for PWM operation
MotorInput_Init(); // Initialize motor inputs for 3 motors
UART_Init(); // Initialize UART4 with 115200 baud rate
EdgeInterrupts_Init(); // Initialize all available edge interrupts
PID_Init(5, 0, 100, 200000); // Initialize PID s by setting all the PID constants
OmniControl_Init(); // Timer initializations for PID loops
Ultrasonic1_Init(); // Intitialize HC-SR04 ultrasonic sensor1
Ultrasonic2_Init(); // Intitialize HC-SR04 ultrasonic sensor2
while(1)
{
}
}
void SCU_TASK(void *p_arg)
{
uint32_t* speed;
uint8_t err;
int PWM_Duty = 0;
int B;
(void)p_arg;
LCD_Print(1,2,"speed = 1.0m/s");
while(1)
{
PID_Init();
speed = OSMboxPend(Str_Box_1,0,&err); //请求消息；获得当前速度
// printf("process_point = %d\n",*speed);
PWM_Duty += PID_Calc(0, *speed, 0);
B = PWM_Duty;
printf("PWM = %d\n",B);
if(PWM_Duty >= 1000)
{
FTM_PWM_ChangeDuty(HW_FTM0,HW_FTM_CH0,1000); /* 0-10000 对应 0-100%占空比 */
// LCD_Print(1,5,"FULL_SPEED");
printf("PWM_Duty = 100 \n");
}
if(PWM_Duty > 0 && PWM_Duty < 1000)
{
FTM_PWM_ChangeDuty(HW_FTM0,HW_FTM_CH0,PWM_Duty); /* 0-10000 对应 0-100%占空比 */
// LCD_Print(1,7,"Part_SPEED");
printf("PWM_Duty = %d\n",PWM_Duty/100);
}
if (PWM_Duty <= 0)
{
FTM_PWM_ChangeDuty(HW_FTM0,HW_FTM_CH0,0); /* 0-10000 对应 0-100%占空比 */
printf("PWM_Duty = 0\n");
}
OSTimeDlyHMSM(0,0,0,50);
}
}
//控制初始化
void control_init()
{
GPIO_InitTypeDef gpio_init;
GPIO_StructInit(&gpio_init);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
gpio_init.GPIO_Mode = GPIO_Mode_IN;
gpio_init.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_3 | GPIO_Pin_4;
gpio_init.GPIO_PuPd = GPIO_PuPd_DOWN;
GPIO_Init(GPIOE, &gpio_init);
//默认参数
PID_Init(&p_rate_pid, 0, 0, 0);
PID_Init(&r_rate_pid, 0, 0, 0);
PID_Init(&y_rate_pid, 0, 0, 0);
PID_Init(&p_angle_pid, 0, 0, 0);
PID_Init(&r_angle_pid, 0, 0, 0);
PID_Init(&y_angle_pid, 0, 0, 0);
PID_Init(&x_v_pid, 0, 0, 0);
PID_Init(&y_v_pid, 0, 0, 0);
PID_Init(&x_d_pid, 0, 0, 0);
PID_Init(&y_d_pid, 0, 0, 0);
PID_Init(&h_v_pid, 0, 0, 0);
PID_Init(&h_d_pid, 1.5f, 0, 0.8f);
//加载PID参数
load_settings(&settings, "/setting",
&p_angle_pid, &p_rate_pid,
&r_angle_pid, &r_rate_pid,
&y_angle_pid, &y_rate_pid,
&x_d_pid, &x_v_pid,
&y_d_pid, &y_v_pid,
&h_v_pid);
//硬编码电机设置
settings.roll_min = settings.pitch_min = settings.yaw_min = 1017;
settings.th_min = 1017;
settings.roll_max = settings.pitch_max = settings.yaw_max = 2021;
settings.th_max = 2021;
settings.roll_mid = settings.pitch_mid = settings.yaw_mid = 1519;
get_pid(); //打印PID表
PID_Set_Filt_Alpha(&p_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&p_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&x_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&x_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&h_v_pid, 1.0 / 60.0, 20.0);
PID_Set_Filt_Alpha(&h_d_pid, 1.0 / 60.0, 20.0);
h_v_pid.maxi = 150;
rt_hw_exception_install(hardfalt_protect);
rt_assert_set_hook(assert_protect);
rt_sem_init(&watchdog, "watchdog", 0, RT_IPC_FLAG_FIFO);
rt_thread_init(&control_thread,
"control",
control_thread_entry,
RT_NULL,
control_stack,
2048, 3, 5);
rt_thread_startup(&control_thread);
rt_thread_init(&watchdog_thread,
"watchdog",
watchdog_entry,
RT_NULL,
watchdog_stack,
512, 1, 1);
rt_thread_startup(&watchdog_thread);
extern void line_register(void);
line_register();
}
void TRANSFER_TASK(void *pvParameters)
{
portBASE_TYPE xStatus;
xData ReadValue;
char *Cmd;
PIDCoff PID_Coff;
while(1)
{
xSemaphoreTake(UART_xCountingSemaphore, portMAX_DELAY);
if (uxQueueMessagesWaiting(RxQueue) != NULL)
{
xStatus = xQueueReceive(RxQueue, &ReadValue, 1);
if (xStatus == pdPASS)
{
if (ReadValue.ID == USART_ID)
{
if (TSVN_USART_Create_Frame(ReadValue.Value) == End)
{
Cmd = TSVN_Get_Parameters(1, TSVN_USART_Get_Frame());
if (!strcmp(Cmd, "PID1"))
{
Cmd = TSVN_Get_Parameters(2, TSVN_USART_Get_Frame());
PID_Coff.Kp = atof(Cmd);
Cmd = TSVN_Get_Parameters(3, TSVN_USART_Get_Frame());
PID_Coff.Ki = atof(Cmd);
Cmd = TSVN_Get_Parameters(4, TSVN_USART_Get_Frame());
PID_Coff.Kd = atof(Cmd);
PID_Init(MOTOR1, PID_Coff);
vTaskSuspendAll();
printf("PID MOTOR1: %0.5f\t%0.5f\t%0.5f\n", PID_Coff.Kp, PID_Coff.Ki, PID_Coff.Kd);
xTaskResumeAll();
}
else if (!strcmp(Cmd, "PID2"))
{
Cmd = TSVN_Get_Parameters(2, TSVN_USART_Get_Frame());
PID_Coff.Kp = atof(Cmd);
Cmd = TSVN_Get_Parameters(3, TSVN_USART_Get_Frame());
PID_Coff.Ki = atof(Cmd);
Cmd = TSVN_Get_Parameters(4, TSVN_USART_Get_Frame());
PID_Coff.Kd = atof(Cmd);
PID_Init(MOTOR2, PID_Coff);
vTaskSuspendAll();
printf("PID MOTOR2: %0.5f\t%0.5f\t%0.5f\n", PID_Coff.Kp, PID_Coff.Ki, PID_Coff.Kd);
xTaskResumeAll();
}
else if (!strcmp(Cmd, "PID3"))
{
Cmd = TSVN_Get_Parameters(2, TSVN_USART_Get_Frame());
PID_Coff.Kp = atof(Cmd);
Cmd = TSVN_Get_Parameters(3, TSVN_USART_Get_Frame());
PID_Coff.Ki = atof(Cmd);
Cmd = TSVN_Get_Parameters(4, TSVN_USART_Get_Frame());
PID_Coff.Kd = atof(Cmd);
PID_Init(MOTOR3, PID_Coff);
vTaskSuspendAll();
printf("PID MOTOR3: %0.5f\t%0.5f\t%0.5f\n", PID_Coff.Kp, PID_Coff.Ki, PID_Coff.Kd);
xTaskResumeAll();
}
}
}
}
}
}
}
/*******************************************************************************
* Function Name : main
* Description : Main program.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
int main(void)
{
SystemInit();
/*速度反馈初始化*/
ENC_Init( );
/*电流反馈初始化*/
SVPWM_3ShuntInit();
/*时基初始化，并启动TIM6计算速度，初始化PID*/
TB_Init( );
Tim6_Init( );
PID_Init(&PID_Torque_InitStructure, &PID_Flux_InitStructure, &PID_Speed_InitStructure);
/*温度，电压数组初始化*/
MCL_Init_Arrays();
/*交互界面初始化*/
KEYS_Init( );
LCD_Display_init();
/*-------------------*/
Res_f=1; //上电完成
//串口示波器初始化
usart_init(115200);
while(1)
{
/*UI显示，以及电源报警，用户管理*/
Display_LCD( );
MCL_ChkPowerStage( );
KEYS_process( );
/*状态机开启运行*/
switch (State)
{
case IDLE: //通过sel按键进入INIT ，在WAIT和FAULT中进入IDEL
break;
case INIT:
MCL_Init( );//初始化电机控制层
TB_Set_StartUp_Timeout(3000);
State = START;
break;
case START:
//passage to state RUN is performed by startup functions;
break;
case RUN: //电机运行过程中，检测速度反馈是否存在问题
if(ENC_ErrorOnFeedback() == TRUE)
{
MCL_SetFault(SPEED_FEEDBACK);
}
break;
case STOP: //关闭TIM1的输出，状态转为等待，停止电流检测，设置Valpha和Vbeta为0，计算三相占空比
TIM_CtrlPWMOutputs(TIM1, DISABLE);
State = WAIT;
SVPWM_3ShuntAdvCurrentReading(DISABLE);
Stat_Volt_alfa_beta.qV_Component1 = Stat_Volt_alfa_beta.qV_Component2 = 0;
SVPWM_3ShuntCalcDutyCycles(Stat_Volt_alfa_beta);
TB_Set_Delay_500us(2000); // 1 sec delay
break;
case WAIT: // 等待速度为零时，将状态转为IDEL
if (TB_Delay_IsElapsed( ) == TRUE)
{
if(ENC_Get_Mechanical_Speed( ) ==0)
{
State = IDLE;
}
}
break;
case FAULT: //状态变为IDEL，全局变量设为第一次启动
if (MCL_ClearFault( ) == TRUE)
{
if(wGlobal_Flags & SPEED_CONTROL == SPEED_CONTROL)
{
//速度控制模式
bMenu_index = CONTROL_MODE_MENU_1;
}
else
{
//力矩控制模式
bMenu_index = CONTROL_MODE_MENU_6;
}
State = IDLE;
wGlobal_Flags |= FIRST_START;
Hall_Start_flag=0;
}
break;
default:
break;
}
usart_watcher(Speed_Iq_Id_watch);
/********End of Usart_watch**************/
}
}
void PL_Init(void) {
#if PL_CONFIG_HAS_LED
LED_Init();
#endif
#if PL_CONFIG_HAS_EVENTS
EVNT_Init();
#endif
#if PL_CONFIG_HAS_TIMER
TMR_Init();
#endif
#if PL_CONFIG_HAS_TRIGGER
TRG_Init();
#endif
#if PL_CONFIG_HAS_BUZZER
BUZ_Init();
#endif
#if PL_CONFIG_HAS_RTOS
RTOS_Init();
#endif
#if PL_CONFIG_HAS_SHELL
SHELL_Init();
#endif
#if PL_CONFIG_HAS_SHELL_QUEUE
SQUEUE_Init();
#endif
#if PL_CONFIG_HAS_MOTOR
MOT_Init();
#endif
#if PL_CONFIG_HAS_LINE_SENSOR
REF_Init();
#endif
#if PL_CONFIG_HAS_MOTOR_TACHO
TACHO_Init();
#endif
#if PL_CONFIG_HAS_MCP4728
MCP4728_Init();
#endif
#if PL_CONFIG_HAS_ULTRASONIC
US_Init();
#endif
#if PL_CONFIG_HAS_PID
PID_Init();
#endif
#if PL_CONFIG_HAS_DRIVE
DRV_Init();
#endif
#if PL_CONFIG_HAS_TURN
TURN_Init();
#endif
#if PL_CONFIG_HAS_LINE_FOLLOW
LF_Init();
#endif
#if PL_CONFIG_HAS_RADIO
RNETA_Init();
#endif
#if PL_CONFIG_HAS_REMOTE
REMOTE_Init();
#endif
#if PL_CONFIG_HAS_IDENTIFY
ID_Init();
#endif
#if PL_CONFIG_HAS_LINE_MAZE
MAZE_Init();
#endif
}
///----------------------------------------------------------------------------
///
/// \brief main
/// \return -
/// \remarks -
///
///----------------------------------------------------------------------------
int32_t main(void)
{
uint16_t j;
Test_Pid.fGain = 1.0f; //
Test_Pid.fMin = -1.0f; //
Test_Pid.fMax = 1.0f; //
/* Proportional only gain */
Test_Pid.fKp = 1.0f; // init gains
Test_Pid.fKi = 0.0f; //
Test_Pid.fKd = 0.0f; //
PID_Init(&Test_Pid); // initialize PID
fSet = 0.0f;
for (j = 0; j < KP_ONLY_CYCLES; j++) { // settle
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // 0 -> 1 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = -0.1f; // 0 -> -1 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // -1 -> 1 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KP_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
/* Integral only gain */
Test_Pid.fKp = 0.0f; // init gains
Test_Pid.fKi = 1.0f; //
Test_Pid.fKd = 0.0f; //
PID_Init(&Test_Pid); // initialize PID
fSet = 0.1f; // 0 -> 1 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = -0.1f; // 0 -> -1 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // -1 -> 1 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KI_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
/* Derivative only gain */
Test_Pid.fKp = 0.0f; // init gains
Test_Pid.fKi = 0.0f; //
Test_Pid.fKd = 1.0f; //
PID_Init(&Test_Pid); // initialize PID
fSet = 0.0f;
for (j = 0; j < KD_ONLY_CYCLES; j++) { // settle
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // 0 -> 1 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = -0.1f; // 0 -> -1 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.1f; // -1 -> 1 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
fSet = 0.0f; // 1 -> 0 step
for (j = 0; j < KD_ONLY_CYCLES; j++) {
fOut = PID_Compute(&Test_Pid, 0.0f, fSet);
}
while (1) {
}
}
void rt_init_thread_entry(void* parameter)
{
rt_components_init();
LED_init();
Motor_Init();
rt_kprintf("start device init\n");
//rt_hw_i2c1_init();
i2cInit();
rt_hw_spi2_init();
rt_hw_spi3_init();
rt_event_init(&ahrs_event, "ahrs", RT_IPC_FLAG_FIFO);
dmp_init();
sonar_init();
HMC5983_Init();
adns3080_Init();
//config_bt();
rt_thread_init(&led_thread,
"led",
led_thread_entry,
RT_NULL,
led_stack,
256, 16, 1);
rt_thread_startup(&led_thread);
spi_flash_init();
// bmp085_init("i2c1");
rt_kprintf("device init succeed\n");
if (dfs_mount("flash0", "/", "elm", 0, 0) == 0)
{
rt_kprintf("flash0 mount to /.\n");
}
else
{
rt_kprintf("flash0 mount to / failed.\n");
}
//default settings
PID_Init(&p_rate_pid, 0, 0, 0);
PID_Init(&r_rate_pid, 0, 0, 0);
PID_Init(&y_rate_pid, 0, 0, 0);
PID_Init(&p_angle_pid, 0, 0, 0);
PID_Init(&r_angle_pid, 0, 0, 0);
PID_Init(&y_angle_pid, 0, 0, 0);
PID_Init(&x_v_pid, 0, 0, 0);
PID_Init(&y_v_pid, 0, 0, 0);
PID_Init(&x_d_pid, 0, 0, 0);
PID_Init(&y_d_pid, 0, 0, 0);
PID_Init(&h_pid, 0, 0, 0);
load_settings(&settings, "/setting", &p_angle_pid, &p_rate_pid
, &r_angle_pid, &r_rate_pid
, &y_angle_pid, &y_rate_pid
, &x_d_pid, &x_v_pid
, &y_d_pid, &y_v_pid
, &h_pid);
settings.roll_min = settings.pitch_min = settings.yaw_min = 1000;
settings.th_min = 1000;
settings.roll_max = settings.pitch_max = settings.yaw_max = 2000;
settings.th_max = 2000;
// if(settings.pwm_init_mode)
// {
// Motor_Set(1000,1000,1000,1000);
//
// rt_thread_delay(RT_TICK_PER_SECOND*5);
//
// Motor_Set(0,0,0,0);
//
// settings.pwm_init_mode=0;
// save_settings(&settings,"/setting");
//
// rt_kprintf("pwm init finished!\n");
// }
get_pid();
PID_Set_Filt_Alpha(&p_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&p_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_angle_pid, 1.0 / 75.0, 20.0);
PID_Set_Filt_Alpha(&x_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&x_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&h_pid, 1.0 / 60.0, 20.0);
rt_thread_init(&control_thread,
"control",
control_thread_entry,
RT_NULL,
control_stack,
1024, 3, 5);
rt_thread_startup(&control_thread);
rt_thread_init(&correct_thread,
"correct",
correct_thread_entry,
RT_NULL,
correct_stack,
1024, 12, 1);
rt_thread_startup(&correct_thread);
LED1(5);
}
int main(void)
{
float q[4];
u8 tmp_buf[32] = {0};
float test = 0;
static u8 led_on = 0;
RCC_HSE_Configuration();
SysTick_Init();
NVIC_Configuration();
USART1_Config(115200);
LED_Init();
LED3_Flash(2,100);
ANO_TC_I2C2_INIT(0xA6, 400000, 1, 1, 3, 3);
//硬实时
Tim3_Init(500);//0.005s
TIM2_Init(999, 0);
Mpu6050init();
MOT_GPIO_init();
MOT_PWM_init();
Set_PWM(0, 0, 0, 0);
NRF24L01_Init();
while (NRF24L01_Check())
{
LED2_Flash(2,500000);
}
//TX mode
NRF24L01_Mode_Config(4);
PID_Init();
ADC1_Init();
while (1)
{
if (getMpu6050Data == 1)
{
//0.01ms?
Read_Mpu6050();
Mpu6050_Analyze();
getMpu6050Data = 0;
}
if (calculateAngle == 1)//2ms period
{
//0.2ms T
//LED2_ON;
//100us
IMU_Quateration_Update((float)fGYRO_X , (float)fGYRO_Y , (float)fGYRO_Z , (float)fACCEL_X, (float)fACCEL_Y, (float)fACCEL_Z,ypr);
//LED2_OFF;
calculateAngle = 0;
}
if (sendData == 1)//2ms period
{
if(led_on)
{
LED2_OFF;
led_on = 0;
}
else
{
LED2_ON;
led_on = 1;
}
if (NRF24L01_RxPacket(tmp_buf) == 0)
{
//10us
Rc_Data_Analyze(tmp_buf,&Rc_Data);
}
//if wait for the IRQ it need 9ms
//if not wait for IRQ it runtime need 100us*1.2=0.12ms
sendSenser((int16_t)fACCEL_X, (int16_t)fACCEL_Y, (int16_t)fACCEL_Z, (int16_t)fGYRO_X, (int16_t) fGYRO_Y, (int16_t)fGYRO_Z, (int16_t)(ypr[0] * 100), (signed short int)(ypr[1] * 100));
send_wave(32);
//0.14ms run time
sendPwmVoltage(&Rc_Data,(uint16_t)(motor0 / 1000.0 * 100), (uint16_t)(motor1 / 1000.0 * 100), (uint16_t)(motor2 / 1000.0 * 100), (uint16_t)(motor3 / 1000.0 * 100));//0.00003974s
send_wave(32);
sendData = 0;
}
//moveFilterAccData(fACCEL_X, fACCEL_Y, fACCEL_Z, AngleOut);
if (!strcmp(Rc_Data.status, "stop"))
{
Set_PWM(0, 0, 0, 0);
}
else if (!strcmp(Rc_Data.status, "start"))
{
//LED2_ON;
expRoll = Rc_Data.roll;
expPitch = Rc_Data.pitch;
expThro = Rc_Data.throttle;
surRoll = ypr[2];
surPitch = ypr[1];
PID_Set();
Set_PWM(motor0, motor1, motor2, motor3);
//LED2_OFF;
}
////Uart1_Send_PID(320,PID_ROLL.KI,PID_ROLL.KD,1,0,0);
////send_wave(32);
// if (STA == 1)
// {
// receive_Data();
// STA = 0;
// p = 0;
// }
}
}
void Setup() {
/* Initialize LCD */
lcd_initialize();
/* Clear LCD */
lcd_clear();
/* Display message on LCD */
lcd_buffer_print(LCD_LINE2, " TEST ");
/* Initialize motors */
Motors_Init();
/* Turn on motors relay */
Motors_On();
/* Send arm signal to motors */
Motor_Arm(MOTOR_UPPER);
Motor_Arm(MOTOR_BOTTOM);
/* Initialize servos */
Servos_Init();
/* Initialize sonar */
sonarInitialize(); //must be initialized before IIC, otherwise it will not work
/* Setup the 12-bit A/D converter */
S12ADC_init();
/* Initialize I2C with control */
riic_ret_t iic_ret = RIIC_OK;
iic_ret |= riic_master_init();
while (RIIC_OK != iic_ret) {
nop(); /* Failure to initialize here means demo can not proceed. */
}
/* Setup Compare Match Timer */
CMT_init();
/* Initialize PID structure used for PID properties */
PID_Init(&z_axis_PID, 0.7, 0.05, 0.30, dt, 0, 0.5); //0.7 0.05 0.15
PID_Init(&Pitch_PID, 1, 0, 0.01, dt, -30, 30);
PID_Init(&Roll_PID, 1, 0, 0.01, dt, -30, 30);
Init_AVG(0, &pitchAVG);
Init_AVG(0, &rollAVG);
/* Make the port connected to SW1 an input */
PORT4.PDR.BIT.B0 = 0;
/*MPU6050 Initialization*/
MPU6050_Test_I2C();
Setup_MPU6050();
Calibrate_Gyros();
// Calibrate_Accel();
/*Kalman Initialization*/
init_Kalman();
//MS5611-01BA01 init
// MS5611_Init();
desiredState.key.motor_diff_us = 0;
desiredState.key.abs.pos.z = 0.20;
altitudeValue = 200;
mainWDT = WDT_Init(500, Fallback);
WDT_Start(&mainWDT);
sonarWDT = WDT_Init(60, Sonar_Fallback);
WDT_Start(&sonarWDT);
}
